How to solve equation sets with SymPy to find some angles?-CodePudding

I do not quite understand how to solve my equation set with SymPy in Python. I have tried using solve() and nsolve(). solve() gives me no answer, it just stands and goes, nsolve() gives me this error.

ValueError: Could not find root within given tolerance. (7378.8100001153525709 > 2.16840434497100886801e-19) Try another starting point or tweak arguments.

This is what I have written when I tried using solve():

from sympy import *

theta_1, theta_2, theta_3 = symbols('theta_1 theta_2 theta_3')

eq1 = -136.2*sin(theta_2)*sin(theta_3)*cos(theta_1)   136.2*cos(theta_1)*cos(theta_2)*cos(theta_3)   222.1*cos(theta_1)*cos(theta_2)

eq2 = -136.2*sin(theta_1)*sin(theta_2)*cos(theta_3)   136.2*sin(theta_1)*cos(theta_2)*cos(theta_3)   222.1*sin(theta_1)*cos(theta_2)

eq3 =  136.2*sin(theta_2)*cos(theta_3)  222.1*sin(theta_2)   136.2*sin(theta_3)*cos(theta_2)   100.9 

eq1 = Eq(eq1, 0)
eq2 = Eq(eq2, (-323.90))
eq3 = Eq(eq3, 176.71)

ans = solve((eq1, eq2, eq3), (theta_1, theta_2, theta_3))

print(ans)

This is what I have written when I tried using nsolve():

from sympy import *

theta_1, theta_2, theta_3 = symbols('theta_1 theta_2 theta_3')

eq1 = -136.2*sin(theta_2)*sin(theta_3)*cos(theta_1)   136.2*cos(theta_1)*cos(theta_2)*cos(theta_3)   222.1*cos(theta_1)*cos(theta_2)

eq2 = -136.2*sin(theta_1)*sin(theta_2)*cos(theta_3)   136.2*sin(theta_1)*cos(theta_2)*cos(theta_3)   222.1*sin(theta_1)*cos(theta_2)

eq3 =  136.2*sin(theta_2)*cos(theta_3)  222.1*sin(theta_2)   136.2*sin(theta_3)*cos(theta_2)   100.9 

ans = nsolve((eq1, eq2, eq3), (theta_1, theta_2, theta_3), (0, -323.9, 176.71))

print(ans)

I want to find an expression for theta1, theta2, and theta3. Preferably in the form of arctan2. Does anyone know how I can do this? or why this does not work?

CodePudding user response：

I'm presuming that your inputs are supposed to be angles in degrees. Here nsolve can give you an answer if you give the inputs in radians:

In [48]: p = (pi/180).evalf()

In [49]: nsolve((eq1, eq2, eq3), (theta_1, theta_2, theta_3), (0, -323.9*p, 176.71*p))
Out[49]: 
⎡-1.5707963267949 ⎤
⎢                 ⎥
⎢-9.36640725122338⎥
⎢                 ⎥
⎣-2.49008907692194⎦

We can reduce your system to a system of polynomials with rational coefficients like this:

In [60]: c1, c2, c3, s1, s2, s3 = symbols('c1:4 s1:4')
    ...: rep = {cos(theta_1): c1, cos(theta_2): c2, cos(theta_3): c3,
    ...:        sin(theta_1): s1, sin(theta_2): s2, sin(theta_3): s3}
    ...: 
    ...: eqs = [eq.subs(rep) for eq in [eq1, eq2, eq3]]   [
    ...:         c1**2   s1**2 - 1, c2**2   s2**2 - 1, c3**2   s3**2 - 1]
    ...: eqs = [nsimplify(eq) for eq in eqs]

In [61]: for eq in eqs:
    ...:     print(eq)
    ...: 
Eq(681*c1*c2*c3/5 - 681*c1*s2*s3/5   2221*c1/10, 0)
Eq(681*c2*c3*s1/5 - 681*c3*s1*s2/5   2221*s1/10, -3239/10)
Eq(681*c2*s3/5   681*c3*s2/5   2221*s2/10   1009/10, 17671/100)
c1**2   s1**2 - 1
c2**2   s2**2 - 1
c3**2   s3**2 - 1

From there you can use SymPy's groebner function to compute a Groebner basis for the solution of the polynomial system. For symmetry we will add a separating element t:

In [62]: gb = groebner(eqs, order='grevlex').fglm('lex')

This is like a new list of separated equations whose solutions are the cos and sin values for the different thetas that you wanted.

The final equation is a polynomial of degree 20 in c3 and its solutions can be substituted into the other equations to give the full solution of the system. The polynomial has 4 real roots each of which is a root of a smaller irreducible polynomial of degree 8. Due to Abel-Ruffini there cannot be radical expressions for the roots but it is possible to compute them numerically:

In [70]: c3num = [r.n() for r in real_roots(gb[-1])]

In [71]: c3num
Out[71]: [-0.795172954862362, -0.552080421425289, 0.702791508766261, 0.99923774387104]

Each of those can be used to solve the rest of the Groebner basis:

In [73]: for c3n in c3num:
     ...:     for eq in gb[:-1]:
     ...:         print(solve([eq.subs(c3, c3n)], dict=True))
     ...:     print(solve([c3 - c3n], dict=True))
     ...:     print()
     ...: 
[{s1: -1.00000000000000}]
[{s2: -0.0583375690202956}]
[{s3: -0.606382694211788}]
[{c1: 0}]
[{c1: 0.0}]
[{c2: -0.998296913792728}]
[{c3: -0.795172954862362}]

[{s1: -1.00000000000000}]
[{s2: 0.881316344165583}]
[{s3: 0.833790866034178}]
[{c1: 0}]
[{c1: 0.0}]
[{c2: -0.472526720395763}]
[{c3: -0.552080421425289}]

[{s1: -1.00000000000000}]
[{s2: -0.0656752871541357}]
[{s3: 0.711395878031908}]
[{c1: 0}]
[{c1: 0.0}]
[{c2: 0.997841047763359}]
[{c3: 0.702791508766261}]

[{s1: -1.00000000000000}]
[{s2: 0.226102985521720}]
[{s3: -0.0390375619754195}]
[{c1: -3.05175781250000e-5}, {c1: 3.05175781250000e-5}]
[{c1: 0.0}]
[{c2: 0.974103403161280}]
[{c3: 0.999237743871040}]

The last case appears to be numerically inconsistent but this might be rounding errors because this is quite numerically unstable and perhaps the solution should be c1=0. I think this happens because your equations are a slightly degenerate case.

The above gives all solutions for the cos and sin of your angles and you can use atan2 to get from there to the angles themselves. You can use nsolve to refine any of the above as accurately as you want. The first of the 4 cases is the one found by nsolve above.

If you wanted an explicit analytic expression here then it seems unlikely.