CODEWITHSUNDEEP
pythonsympytrigonometrysymbolic-mathsimplify
jstoquica
jstoquica

Reputation: 9

SymPy cannot solve an trigonemetric expression, but Matlab did

I don't understand why the expression below is not being simplified. The example shows the bug:

import pandas as pd
import numpy as np
from sympy import symbols
from sympy.solvers import solve
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import sympy as sym
from sympy import Matrix, simplify, trigsimp, fraction, lambdify, sin, cos 

X = 0
Y = -320
Z = 710     
        
RX = 0
RY = 90
RZ = 0 
        
s1, c1 = sym.symbols('sin(th1) cos(th1)')
s2, c2 = sym.symbols('sin(th2) cos(th2)')
s3, c3 = sym.symbols('sin(th3) cos(th3)')
s4, c4 = sym.symbols('sin(th4) cos(th4)')
s5, c5 = sym.symbols('sin(th5) cos(th5)')
s6, c6 = sym.symbols('sin(th6) cos(th6)')

#%%

matRotX = Matrix([[1, 0, 0], 
           [0, np.cos(np.deg2rad(RX)), -np.sin(np.deg2rad(RX))],
           [0, np.sin(np.deg2rad(RX)), np.cos(np.deg2rad(RX))]])
matRotY = Matrix([[np.cos(np.deg2rad(RY)), 0, np.sin(np.deg2rad(RX))],
           [0, 1, 0], 
           [-np.sin(np.deg2rad(RX)), 0, np.cos(np.deg2rad(RX))]])
matRotZ = Matrix([[np.cos(np.deg2rad(RY)), -np.sin(np.deg2rad(RX)), 0],
           [np.sin(np.deg2rad(RX)), np.cos(np.deg2rad(RX)), 0],
           [0, 0, 1]])

matRot = matRotZ * matRotY
matRot = matRot * matRotX

d1 = 352
a1 = 70
alfa1 = np.deg2rad(-90)

RotZ1 = Matrix([[c1, -s1, 0, 0],
         [s1, c1, 0, 0],
         [0, 0, 1, 0],
         [0, 0, 0, 1]])
Trans_d1 = Matrix([[1, 0, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, d1],
            [0, 0, 0, 1]])
Tran_a1 = Matrix([[1, 0, 0, a1],
           [0, 1, 0, 0], 
           [0, 0, 1, 0], 
           [0, 0, 0, 1]])
RotX1 = Matrix([[1, 0, 0, 0],
         [0, np.cos(alfa1), -np.sin(alfa1), 0],
         [0, np.sin(alfa1), np.cos(alfa1), 0],
         [0, 0, 0, 1]])

A01 = RotZ1 * Trans_d1 * Tran_a1 * RotX1
  
d2 = 0
a2 = 360
alfa2 = np.deg2rad(0)
beta2 = np.deg2rad(0)

RotZ2 = Matrix([[c2, -s2, 0, 0],
               [s2, c2, 0, 0],
               [0, 0, 1, 0],
               [0, 0, 0, 1]])
Trans_d2 = Matrix([[1, 0, 0, 0], 
                  [0, 1, 0, 0], 
                  [0, 0, 1, d2], 
                  [0, 0, 0, 1]])
Tran_a2 = Matrix([[1, 0, 0, a2], 
                 [0, 1, 0, 0],
                 [0, 0, 1, 0], 
                 [0, 0, 0, 1]])
RotX2 = Matrix([[1, 0, 0, 0], 
               [0, np.cos(alfa2), -np.sin(alfa2), 0],
               [0, np.sin(alfa2), np.cos(alfa2), 0], 
               [0, 0, 0, 1]])
RotY2 = Matrix([[np.cos(beta2), 0, np.sin(beta2), 0],
               [0, 1, 0, 0], 
               [-np.sin(beta2), 0, np.cos(beta2), 0],
               [0, 0, 0, 1]])

A12 = RotZ2 * Tran_a2 * RotX2 * RotY2 #Hayati

   
d3 = 0
a3 = 0
alfa3 = np.deg2rad(-90)

RotZ3 = Matrix([[c3, -s3, 0, 0],
                 [s3, c3, 0, 0],
                 [0, 0, 1, 0],
                 [0, 0, 0, 1]])
Trans_d3 = Matrix([[1, 0, 0, 0],
                   [0, 1, 0, 0],
                   [0, 0, 1, d3],
                   [0, 0, 0, 1]])
Tran_a3 = Matrix([[1, 0, 0, a3],
                  [0, 1, 0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])
RotX3 = Matrix([[1, 0, 0, 0],
                [0, np.cos(alfa3), -np.sin(alfa3), 0],
                [0, np.sin(alfa3), np.cos(alfa3), 0],
                [0, 0, 0, 1]])

A23 = RotZ3 * Trans_d3 * Tran_a3 * RotX3

#A4
d4 = 380;
a4 = 0;
alfa4 = np.deg2rad(90);

RotZ4 = Matrix([[c4, -s4, 0, 0],
                [s4, c4, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1]])
Trans_d4 = Matrix([[1, 0, 0, 0],
                   [0, 1, 0, 0],
                   [0, 0, 1, d4],
                   [0, 0, 0, 1]])
Tran_a4 = Matrix([[1, 0, 0, a4],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])
RotX4 = Matrix([[1, 0, 0, 0],
          [0, np.cos(alfa4), -np.sin(alfa4), 0],
          [0, np.sin(alfa4), np.cos(alfa4), 0],
          [0, 0, 0, 1]])

A34 = RotZ4 * Trans_d4 * Tran_a4 * RotX4

#A5
d5 = 0
a5 = 0
alfa5 = np.deg2rad(-90)

RotZ5 = Matrix([[c5, -s5, 0, 0],
                [s5, c5, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1]])
Trans_d5 = Matrix([[1, 0, 0, 0],
                   [0, 1, 0, 0],
                   [0, 0, 1, d5],
                   [0, 0, 0, 1]])
Tran_a5 = Matrix([[1, 0, 0, a5],
                  [0, 1, 0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])
RotX5 = Matrix([[1, 0, 0, 0],
                [0, np.cos(alfa5), -np.sin(alfa5), 0],
                [0, np.sin(alfa5), np.cos(alfa5), 0],
                [0, 0, 0, 1]])

A45 = RotZ5 * Trans_d5 * Tran_a5 * RotX5

d6 = 65
a6 = 0
alfa6 = np.deg2rad(0)

RotZ6 = Matrix([[c6, -s6, 0, 0],
                [s6, c6, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1]])
Trans_d6 = Matrix([[1, 0, 0, 0],
                   [0, 1, 0, 0], 
                   [0, 0, 1, d6],
                   [0, 0, 0, 1]])
Tran_a6 = Matrix([[1, 0, 0, a6],
                  [0, 1, 0, 0], 
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])
RotX6 = Matrix([[1, 0, 0, 0], 
               [0, np.cos(alfa6), -np.sin(alfa6), 0],
               [0, np.sin(alfa6), np.cos(alfa6), 0], 
               [0, 0, 0, 1]])

A56 = RotZ6 * Trans_d6 * Tran_a6 * RotX6


T06 = Matrix(np.zeros((4,4)))

T06[0,3] = sym.symbols('px')
T06[1,3] = sym.symbols('py')
T06[2,3] = sym.symbols('pz')
T06[3,3] = 1
T06[0,0] = sym.symbols('nx')
T06[1,0] = sym.symbols('ny')
T06[2,0] = sym.symbols('nz')
T06[3,0] = 0
T06[0,1] = sym.symbols('ox')
T06[1,1] = sym.symbols('oy')
T06[2,1] = sym.symbols('oz')
T06[3,1] = 0
T06[0,2] = sym.symbols('ax')
T06[1,2] = sym.symbols('ay')
T06[2,2] = sym.symbols('az')
T06[3,2] = 0

Theta1rad = -1
  
A01 = A01.subs({'cos(th1)':np.cos(Theta1rad)})
A01 = A01.subs({'sin(th1)':np.sin(Theta1rad)})

T06 = T06.subs({'px':X})
T06 = T06.subs({'py':Y})
T06 = T06.subs({'pz':Z})
T06 = T06.subs({'ax':matRot[0,2]})
T06 = T06.subs({'ay':matRot[1,2]})
T06 = T06.subs({'az':matRot[2,2]})
T06 = T06.subs({'ox':matRot[0,1]})
T06 = T06.subs({'oy':matRot[1,1]})
T06 = T06.subs({'oz':matRot[2,1]})
T06 = T06.subs({'nx':matRot[0,0]})
T06 = T06.subs({'ny':matRot[1,0]})
T06 = T06.subs({'nz':matRot[2,0]})
  
Theta31rad = 0.57

A23 = A23.subs({'cos(th3)':np.cos(Theta31rad)})
A23 = A23.subs({'sin(th3)':np.sin(Theta31rad)})

Eq27 = A12.inv() * A01.inv() * T06 * A56.inv()
Eq27 = simplify(Eq27)

Eq27[0,3] is the result of previous algebraic operations with Symbolic variables, but simplify() or lambdify() are not working. However, if I copy and paste manually the final expression, symPy can simplify the expression, as it is presented as a follows:

simplify(Eq27[0,3])
Out[123]: (-360.0*cos(th2)**2 + 199.270715138527*cos(th2) - 360.0*sin(th2)**2 - 293.0*sin(th2))/(cos(th2)**2 + sin(th2)**2)

after manually copy/paste:

a = (-360.0*cos(th2)**2 + 199.270715138527*cos(th2) - 360.0*sin(th2)**2 - 293.0*sin(th2))/(cos(th2)**2 + sin(th2)**2)

simplify(a)
Out[125]: -293.0*sin(th2) + 199.270715138527*cos(th2) - 360.0

Is any reason for this behavior?

Regards.

Upvotes: 1

Views: 159

Answers (1)

Oscar Benjamin
Oscar Benjamin

Reputation: 14480

So this is the expression that you have created:

In [39]: a = simplify(Eq27[0,3])

In [40]: a
Out[40]: 
    ⎛                2                                             2                 ⎞
1.0⋅⎝- 360.0⋅cos(th2)  + 199.270715138527⋅cos(th2) - 360.0⋅sin(th2)  - 293.0⋅sin(th2)⎠
──────────────────────────────────────────────────────────────────────────────────────
                                        2           2                                 
                                cos(th2)  + sin(th2)

We don't need all of the rest of your code in order to produce this expression. The simple way to reproduce the same object is using srepr e.g.:

In [41]: srepr(a)
Out[41]: "Mul(Float('1.0', precision=53), Pow(Add(Pow(Symbol('cos(th2)'), Integer(2)), Pow(Symbol('sin(th2)'), Integer(2))), Integer(-1)), Add(Mul(Integer(-1), Float('360.0', precision=53), Pow(Symbol('cos(th2)'), Integer(2))), Mul(Float('199.27071513852684', precision=53), Symbol('cos(th2)')), Mul(Integer(-1), Float('360.0', precision=53), Pow(Symbol('sin(th2)'), Integer(2))), Mul(Integer(-1), Float('293.0', precision=53), Symbol('sin(th2)'))))"

In [42]: b = eval(srepr(a))

In [43]: b == a
Out[43]: True

Looking closely at the output from srepr I see the problem. Your expression looks like it has terms like cos(th1) but those are actually symbols with the name "cos(th1)". See the difference here:

In [44]: c = Symbol('cos(th1)')

In [45]: d = cos(Symbol('th1'))

In [46]: c
Out[46]: cos(th1)

In [47]: d
Out[47]: cos(th₁)

In [48]: srepr(c)
Out[48]: "Symbol('cos(th1)')"

In [49]: srepr(d)
Out[49]: "cos(Symbol('th1'))"

Trig identities will only apply if you have trig functions but a symbol whose name happens to look like a trig function is not the same thing as an actual trig function. When you copy paste the repr you get actual trig functions which is why the result can be simplified.

So the problem is at the top of your code:

s1, c1 = sym.symbols('sin(th1) cos(th1)')
s2, c2 = sym.symbols('sin(th2) cos(th2)')
s3, c3 = sym.symbols('sin(th3) cos(th3)')
s4, c4 = sym.symbols('sin(th4) cos(th4)')
s5, c5 = sym.symbols('sin(th5) cos(th5)')
s6, c6 = sym.symbols('sin(th6) cos(th6)')

That should be:

th1, th2, th3, th4, th5, th6 = symbols('th1:7')
s1, c1 = sin(th1), cos(th1)
s2, c2 = sin(th2), cos(th2)
s3, c3 = sin(th3), cos(th3)
s4, c4 = sin(th4), cos(th4)
s5, c5 = sin(th5), cos(th5)
s6, c6 = sin(th6), cos(th6)

Upvotes: 2

Related Questions