lzyue
Reputation: 1
python sympy to solve 6 nonlinear equations?
I wanna use sympy to solve 6 nonlinear equations, but I got the infinite process time and without any solution. However,it's very fast to use matlab solve. I have try to use some flags such as manual=True, but it does not work.
Can anybody help me? Thank you very much.
My code show as follows:
xcom_0=sympy.Symbol("xcom_0",real=True)
ycom_0=sympy.Symbol("ycom_0",real=True)
zcom_0=sympy.Symbol("zcom_0",real=True)
dxcom_0=sympy.Symbol("dxcom_0",real=True)
dycom_0=sympy.Symbol("dycom_0",real=True)
dzcom_0=sympy.Symbol("dzcom_0",real=True)
psi_com_0=sympy.Symbol("psi_com_0",real=True)
theta_com_0=sympy.Symbol("theta_com_0",real=True)
pha_com_0=sympy.Symbol("pha_com_0",real=True)
dpsi_com_0=sympy.Symbol("dpsi_com_0",real=True)
dtheta_com_0=sympy.Symbol("dtheta_com_0",real=True)
dpha_com_0=sympy.Symbol("dpha_com_0",real=True)
x_foot_rear_0=sympy.Symbol("x_foot_rear_0",real=True)
y_foot_rear_0=sympy.Symbol("y_foot_rear_0",real=True)
z_foot_rear_0=sympy.Symbol("z_foot_rear_0",real=True)
#real time com position and velocity,pitch angular information
x_com_T=sympy.Symbol("x_com_T",real=True)
z_com_T=sympy.Symbol("z_com_T",real=True)
dx_com_T=sympy.Symbol("dx_com_T",real=True)
dz_com_T=sympy.Symbol("dz_com_T",real=True)
theta_com_T=sympy.Symbol("theta_com_T",real=True)
dtheta_com_T=sympy.Symbol("dtheta_com_T",real=True)
t=sympy.Symbol("t",real=True) #time
m=sympy.Symbol("m",real=True) #robot mass
g=sympy.Symbol("g",real=True) #gravity
I=sympy.Symbol("I",real=True) #rotation inertial matrix
a0=sympy.Symbol("a0",real=True)
a1=sympy.Symbol("a1",real=True)
a2=sympy.Symbol("a2",real=True)
a3=sympy.Symbol("a3",real=True)
b0=sympy.Symbol("b0",real=True)
b1=sympy.Symbol("b1",real=True)
b2=sympy.Symbol("b2",real=True)
b3=sympy.Symbol("b3",real=True)
# print(x_com_T)
eq1=sympy.Eq(a0*t**2/(2*m) + a1*t**3/(6*m) + a2*t**4/(12*m) + a3*t**5/(20*m) + dxcom_0*t + xcom_0-x_com_T,0)
eq2=sympy.Eq(b1*t**3/(6*m) + b2*t**4/(12*m) + b3*t**5/(20*m) + dxcom_0*t + zcom_0 + t**2*(b0 - g*m)/(2*m)-z_com_T,0)
eq3=sympy.Eq(a0*t/m + a1*t**2/(2*m) + a2*t**3/(3*m) + a3*t**4/(4*m) + dxcom_0-dx_com_T,0)
eq4=sympy.Eq(b1*t**2/(2*m) + b2*t**3/(3*m) + b3*t**4/(4*m) + dxcom_0 + t*(b0 - g*m)/m-dz_com_T,0)
eq5=sympy.Eq(dtheta_com_0 + t**3*(a0*g - 2*a1*dxcom_0 + 2*a2*z_foot_rear_0 - 2*a2*zcom_0 + 2*b1*dxcom_0 - 2*b2*x_foot_rear_0 + 2*b2*xcom_0)/(6*I) + t**2*(-a0*dxcom_0 + a1*z_foot_rear_0 - a1*zcom_0 + b0*dxcom_0 - b1*x_foot_rear_0 + b1*xcom_0)/(2*I) + t*(a0*z_foot_rear_0 - a0*zcom_0 - b0*x_foot_rear_0 + b0*xcom_0)/I + t**8*(a2*b3 - a3*b2)/(240*I*m) + t**7*(a1*b3 - a3*b1)/(60*I*m) + t**6*(27*a0*b3 + 5*a1*b2 - 5*a2*b1 - 27*a3*b0 + 30*a3*g*m)/(360*I*m) + t**5*(5*a0*b2 - 5*a2*b0 + 6*a2*g*m - 12*a3*dxcom_0*m + 12*b3*dxcom_0*m)/(60*I*m) + t**4*(2*a0*b1 - 2*a1*b0 + 3*a1*g*m - 6*a2*dxcom_0*m + 6*a3*m*z_foot_rear_0 - 6*a3*m*zcom_0 + 6*b2*dxcom_0*m - 6*b3*m*x_foot_rear_0 + 6*b3*m*xcom_0)/(24*I*m)-dtheta_com_T,0)
eq6=sympy.Eq(dtheta_com_0*t + theta_com_0 + t**4*(a0*g - 2*a1*dxcom_0 + 2*a2*z_foot_rear_0 - 2*a2*zcom_0 + 2*b1*dxcom_0 - 2*b2*x_foot_rear_0 + 2*b2*xcom_0)/(24*I) + t**3*(-a0*dxcom_0 + a1*z_foot_rear_0 - a1*zcom_0 + b0*dxcom_0 - b1*x_foot_rear_0 + b1*xcom_0)/(6*I) + t**2*(a0*z_foot_rear_0 - a0*zcom_0 - b0*x_foot_rear_0 + b0*xcom_0)/(2*I) + t**9*(a2*b3 - a3*b2)/(2160*I*m) + t**8*(a1*b3 - a3*b1)/(480*I*m) + t**7*(27*a0*b3 + 5*a1*b2 - 5*a2*b1 - 27*a3*b0 + 30*a3*g*m)/(2520*I*m) + t**6*(5*a0*b2 - 5*a2*b0 + 6*a2*g*m - 12*a3*dxcom_0*m + 12*b3*dxcom_0*m)/(360*I*m) + t**5*(2*a0*b1 - 2*a1*b0 + 3*a1*g*m - 6*a2*dxcom_0*m + 6*a3*m*z_foot_rear_0 - 6*a3*m*zcom_0 + 6*b2*dxcom_0*m - 6*b3*m*x_foot_rear_0 + 6*b3*m*xcom_0)/(120*I*m)-theta_com_T,0)
# eq1=sympy.Eq(x_com_T-self.x_com_T,0)
# eq2=sympy.Eq(z_com_T-self.z_com_T,0)
# eq3=sympy.Eq(dxc_com_T-self.dx_com_T,0)
# eq4=sympy.Eq(dzc_com_T-self.dz_com_T,0)
# eq5=sympy.Eq(dtheta_com_T-self.dtheta_com_T,0)
# eq6=sympy.Eq(theta_com_T-self.theta_com_T,0)
eqns=(eq1,eq2,eq3,eq4,eq5,eq6)
slv=sympy.solve(eqns,(a1,a2,a3,b1,b2,b3),force=True, manual=True)
Upvotes: 0
Views: 106
Answers (2)
Oscar Benjamin
Reputation: 14480
As noted by JohanC it is possible for SymPy to find a solution but it is quite slow to get there using solve even if you use check=False.
Generally speaking a nonlinear system with fully arbitrary symbols like this would not be expected to have any analytic solution but yours is a curious case in which you have moderate nonlinearity that disappears. The first 4 equations are actually linear. We can solve those to get 4 of the variables in terms of the other 2. After doing that we can substitute the solution for those into the final 2 equations which upon expansion become linear because somehow all the nonlinear terms cancel away.
In [62]: syms = (a1,a2,a3,b1,b2,b3)
In [63]: [sol4] = linsolve(eqns[:4], syms)
In [64]: sol4 # underdetermined solution in terms of a3 and b3
Out[64]:
⎛ 2 2
⎜3⋅a₃⋅t - 6⋅a₀⋅t - 6⋅dx_com_T⋅m⋅t - 18⋅dxcom₀⋅m⋅t + 24⋅m⋅x_com_T - 24⋅m⋅xcom₀ 6⋅a₃⋅t 6⋅a₀⋅
⎜─────── + ──────────────────────────────────────────────────────────────────────, - ────── + ─────
⎜ 10 3 5
⎝ t
2 2 2
t + 12⋅dx_com_T⋅m⋅t + 24⋅dxcom₀⋅m⋅t - 36⋅m⋅x_com_T + 36⋅m⋅xcom₀ 3⋅b₃⋅t - 6⋅b₀⋅t - 18⋅dxco
────────────────────────────────────────────────────────────────, a₃, ─────── + ───────────────────
4 10
t
2 2
m₀⋅m⋅t - 6⋅dz_com_T⋅m⋅t + 6⋅g⋅m⋅t + 24⋅m⋅z_com_T - 24⋅m⋅zcom₀ 6⋅b₃⋅t 6⋅b₀⋅t + 24⋅dxcom₀⋅m⋅t
──────────────────────────────────────────────────────────────, - ────── + ────────────────────────
3 5
t
2 ⎞
+ 12⋅dz_com_T⋅m⋅t - 6⋅g⋅m⋅t - 36⋅m⋅z_com_T + 36⋅m⋅zcom₀ ⎟
────────────────────────────────────────────────────────, b₃⎟
4 ⎟
t ⎠
In [65]: rep = dict(zip(syms, sol4))
In [66]: eq4 = eqns[4].subs(rep).expand()
In [67]: eq5 = eqns[5].subs(rep).expand()
In [68]: linsolve([eq4, eq5], [a3, b3])
Out[68]:
<long output elided>
That last result from linsolve can be substituted back to into sol4 to give the final solution:
In [69]: [(a3sol, b3sol)] = linsolve([eq4, eq5], [a3, b3])
In [71]: [a1sol, a2sol, a3sol, b1sol, b2sol, b3sol] = Tuple(*sol4).subs({a3:a3sol, b3:b3sol})
The final result is quite complicated but e.g. the solution for a1 is:
In [72]: a1sol
Out[72]:
2 ⎛
- 6⋅a₀⋅t - 6⋅dx_com_T⋅m⋅t - 18⋅dxcom₀⋅m⋅t + 24⋅m⋅x_com_T - 24⋅m⋅xcom₀ 3⋅⎝-1200⋅I⋅dtheta_com_0 +
────────────────────────────────────────────────────────────────────── + ──────────────────────────
3
t
3 2
1200⋅I⋅dtheta_com_T - 20⋅a₀⋅g⋅t + 60⋅dx_com_T⋅g⋅m⋅t + 1200⋅dx_com_T⋅m⋅z_com_T - 1200⋅dx_com_T⋅m⋅z
───────────────────────────────────────────────────────────────────────────────────────────────────
2
_foot_rear_0 - 180⋅dxcom₀⋅g⋅m⋅t - 1200⋅dxcom₀⋅m⋅x_foot_rear_0 + 1200⋅dxcom₀⋅m⋅xcom₀ + 1200⋅dxcom₀⋅
───────────────────────────────────────────────────────────────────────────────────────────────────
4
10⋅g⋅t
m⋅z_foot_rear_0 - 1200⋅dxcom₀⋅m⋅zcom₀ - 1200⋅dz_com_T⋅m⋅x_com_T + 1200⋅dz_com_T⋅m⋅x_foot_rear_0 - 4
───────────────────────────────────────────────────────────────────────────────────────────────────
⎞
80⋅g⋅m⋅t⋅x_com_T + 1200⋅g⋅m⋅t⋅x_foot_rear_0 - 720⋅g⋅m⋅t⋅xcom₀⎠
──────────────────────────────────────────────────────────────
Altogether this takes about 2 seconds on my computer with the subs/expand lines being the slowest part.
Upvotes: 1
smichr
Reputation: 19029
This is the sort of problem that benefits from aggressive symbol collection otherwise the symbolic explosion slows down the solution process. In my model branch I have a function which collects all but desired symbols and assigns them a single symbolic constant.
>>> meqs,reps = zip(*[model(e,*want,name=str(i)) for i,e in enumerate(eqs)])
>>> reps = list(reps)
Here are the equations to solve in "model" form:
>>> for i in meqs:i
...
04*(01 + 02*a2 + 03*a1 + a3)
14*(11 + 12*b2 + 13*b1 + b3)
24*(21 + 22*a1 + 23*a2 + a3)
34*(31 + 32*b1 + 33*b2 + b3)
410*(411 + 413*a2 + 414*a3 + 415*a1*(41 + b3) + 415*b1*(416*a2 + 45 - a3) + b2*(
412*a1 + 47 - a3) + b3*(49 + a2))
510*(511 + 513*a2 + 514*a3 + 515*a1*(51 + b3) + 515*b1*(516*a2 + 55 - a3) + b2*(
512*a1 + 57 - a3) + b3*(59 + a2))
The first 4 are linear and easy to solve:
>>> s4 = solve(meqs[:4],want)
>>> list(s4)
[a1, a2, b1, b2]
Now we repeat the model step for the last two:
>>> e5,r5=model(collect(meqs[-2].subs(s4).expand(),(a3,b3)),a3,b3,name='7')
>>> e6,r6=model(collect(meqs[-1].subs(s4).expand(),(a3,b3)),a3,b3,name='8')
>>> reps.append(r5)
>>> reps.append(r6)
>>> e5
767*(768 + 784*b3 + a3*(733 + 769*b3 + 770*b3 + 771*b3 + 772*b3 + 773*b3 + 774*b
3 + 775*b3 + 776*b3 + 777*b3 + 778*b3 + 779*b3 + 780*b3 + 781*b3 + 782*b3 + 783*
b3 - b3))
>>> sa3=solve(_,a3,dict=True)[0]
>>> e6.subs(sa3)
867*(868 + 884*b3 + (-768 - 784*b3)*(833 + 869*b3 + 870*b3 + 871*b3 + 872*b3 + 8
73*b3 + 874*b3 + 875*b3 + 876*b3 + 877*b3 + 878*b3 + 879*b3 + 880*b3 + 881*b3 +
882*b3 + 883*b3 - b3)/(733 + 769*b3 + 770*b3 + 771*b3 + 772*b3 + 773*b3 + 774*b3
+ 775*b3 + 776*b3 + 777*b3 + 778*b3 + 779*b3 + 780*b3 + 781*b3 + 782*b3 + 783*b
3 - b3))
Repeat constant collection to expose the simple quadratic in b3
>>> model(collect(_.as_numer_denom()[0].expand(),b3),b3,name='9')
(92*(93 + 94*b3 + b3**2), [(94, 90/91), (93, 733*867*868/91 - 768*833*867/91), (
92, 91), (91, 769*867*884 + 770*867*884 + 771*867*884 + 772*867*884 + 773*867*88
4 + 774*867*884 + 775*867*884 + 776*867*884 + 777*867*884 + 778*867*884 + 779*86
7*884 + 780*867*884 + 781*867*884 + 782*867*884 + 783*867*884 - 784*867*869 - 78
4*867*870 - 784*867*871 - 784*867*872 - 784*867*873 - 784*867*874 - 784*867*875
- 784*867*876 - 784*867*877 - 784*867*878 - 784*867*879 - 784*867*880 - 784*867*
881 - 784*867*882 - 784*867*883 + 784*867 - 867*884), (90, 733*867*884 - 768*867
*869 - 768*867*870 - 768*867*871 - 768*867*872 - 768*867*873 - 768*867*874 - 768
*867*875 - 768*867*876 - 768*867*877 - 768*867*878 - 768*867*879 - 768*867*880 -
768*867*881 - 768*867*882 - 768*867*883 + 768*867 + 769*867*868 + 770*867*868 +
771*867*868 + 772*867*868 + 773*867*868 + 774*867*868 + 775*867*868 + 776*867*8
68 + 777*867*868 + 778*867*868 + 779*867*868 + 780*867*868 + 781*867*868 + 782*8
67*868 + 783*867*868 - 784*833*867 - 867*868)])
>>> e6,r6=_;reps.append(r6);sb3=solve(e6,b3)
>>> sb3
[-94/2 - sqrt(-4*93 + 94**2)/2, -94/2 + sqrt(-4*93 + 94**2)/2
>>> var('sgn')
sgn
>>> sb3[-1].as_independent(Pow)
(-94/2, sqrt(-4*93 + 94**2)/2)
>>> sb3={b3:sgn*_[1]+_[0]}
Now do back-substitution to collect the full solution:
>>> sol={}
>>> sol.update(sb3)
>>> sol.update({a3:sa3[a3].xreplace(sol)})
>>> sol.update({k:v.xreplace(sol) for k,v in s4.items()})
>>> for r in reversed(reps): sol = {k:v.xreplace(dict(r)) for k,v in sol.items()}
>>> Dict(sol).count_ops()
126284
Running cse on the resulting values gives about 200 constants and a final representation of
cse_ans = [
x195,
x199,
x199*x201 - x200*(x103 - x94),
-x199*x202 - x200*(x71 - x75),
x195*x201 - x200*(x102 - x51),
-x195*x202 - x200*(-x100 + x84)]
(Values of the constants are left to the interested reader. There are actually 2 sets of solutions, 1 for each sign in the solution for b3.)
Upvotes: 1
Related Questions
- Solving a system of equation with Sympy, python2.7
- Solving equations with sympy
- Python/Sympy: solve equations with different values
- Why sympy.solve doen't work with these 12 variables nonlinear equation system?
- Solve a nonlinear system of equation via sympy
- Solve a system of symbolic equations in python sympy
- How to solve symbolically a pair of nonlinear equations using Python (sympy)?
- how to use python sympy to solve for 3 unknowns with 3 equations
- Solving non-linear set of symbolic equations with SymPy (Python)
- Solving Systems of Equations with SymPy