SymPy 无法求解三角表达式，但 Matlab 可以
SymPy cannot solve an trigonemetric expression, but Matlab did
我不明白为什么下面的表达式没有被简化。该示例显示错误：
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] 是前面用符号变量进行代数运算的结果，但是 simplify() 或 lambdify() 不起作用。但是，如果我手动复制并粘贴最终表达式，symPy 可以简化表达式，如下所示：
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)
手动后 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
这种行为有什么原因吗？
此致。
这就是您创建的表达式：
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)
我们不需要您的所有其余代码来生成此表达式。复制同一对象的简单方法是使用 srepr 例如：
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
仔细查看 srepr 的输出，我发现了问题所在。您的表达式看起来有 cos(th1) 之类的术语，但这些实际上是带有 name "cos(th1)" 的符号。看这里的区别：
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'))"
三角恒等式仅在您具有三角函数时适用，但名称恰好看起来像三角函数的符号与实际三角函数不同。当您复制粘贴 repr 时，您会得到实际的三角函数，这就是结果可以简化的原因。
所以问题出在您的代码顶部：
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)')
应该是：
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)
我不明白为什么下面的表达式没有被简化。该示例显示错误：
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] 是前面用符号变量进行代数运算的结果，但是 simplify() 或 lambdify() 不起作用。但是，如果我手动复制并粘贴最终表达式，symPy 可以简化表达式，如下所示：
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)
手动后 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
这种行为有什么原因吗？
此致。
这就是您创建的表达式：
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)
我们不需要您的所有其余代码来生成此表达式。复制同一对象的简单方法是使用 srepr 例如：
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
仔细查看 srepr 的输出，我发现了问题所在。您的表达式看起来有 cos(th1) 之类的术语，但这些实际上是带有 name "cos(th1)" 的符号。看这里的区别：
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'))"
三角恒等式仅在您具有三角函数时适用，但名称恰好看起来像三角函数的符号与实际三角函数不同。当您复制粘贴 repr 时，您会得到实际的三角函数，这就是结果可以简化的原因。
所以问题出在您的代码顶部：
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)')
应该是：
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)