问题描述
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,np.cos(np.deg2rad(RX)),-np.sin(np.deg2rad(RX))],np.sin(np.deg2rad(RX)),np.cos(np.deg2rad(RX))]])
matRotY = Matrix([[np.cos(np.deg2rad(RY)),np.sin(np.deg2rad(RX))],1,[-np.sin(np.deg2rad(RX)),np.cos(np.deg2rad(RX))]])
matRotZ = Matrix([[np.cos(np.deg2rad(RY)),-np.sin(np.deg2rad(RX)),[np.sin(np.deg2rad(RX)),1]])
matRot = matRotZ * matRotY
matRot = matRot * matRotX
d1 = 352
a1 = 70
alfa1 = np.deg2rad(-90)
RotZ1 = Matrix([[c1,-s1,[s1,c1,1]])
Trans_d1 = Matrix([[1,d1],1]])
Tran_a1 = Matrix([[1,a1],1]])
RotX1 = Matrix([[1,np.cos(alfa1),-np.sin(alfa1),np.sin(alfa1),1]])
A01 = RotZ1 * Trans_d1 * Tran_a1 * RotX1
d2 = 0
a2 = 360
alfa2 = np.deg2rad(0)
beta2 = np.deg2rad(0)
RotZ2 = Matrix([[c2,-s2,[s2,c2,1]])
Trans_d2 = Matrix([[1,d2],1]])
Tran_a2 = Matrix([[1,a2],1]])
RotX2 = Matrix([[1,np.cos(alfa2),-np.sin(alfa2),np.sin(alfa2),1]])
RotY2 = Matrix([[np.cos(beta2),np.sin(beta2),[-np.sin(beta2),np.cos(beta2),1]])
A12 = RotZ2 * Tran_a2 * RotX2 * RotY2 #Hayati
d3 = 0
a3 = 0
alfa3 = np.deg2rad(-90)
RotZ3 = Matrix([[c3,-s3,[s3,c3,1]])
Trans_d3 = Matrix([[1,d3],1]])
Tran_a3 = Matrix([[1,a3],1]])
RotX3 = Matrix([[1,np.cos(alfa3),-np.sin(alfa3),np.sin(alfa3),1]])
A23 = RotZ3 * Trans_d3 * Tran_a3 * RotX3
#A4
d4 = 380;
a4 = 0;
alfa4 = np.deg2rad(90);
RotZ4 = Matrix([[c4,-s4,[s4,c4,1]])
Trans_d4 = Matrix([[1,d4],1]])
Tran_a4 = Matrix([[1,a4],1]])
RotX4 = Matrix([[1,np.cos(alfa4),-np.sin(alfa4),np.sin(alfa4),1]])
A34 = RotZ4 * Trans_d4 * Tran_a4 * RotX4
#A5
d5 = 0
a5 = 0
alfa5 = np.deg2rad(-90)
RotZ5 = Matrix([[c5,-s5,[s5,c5,1]])
Trans_d5 = Matrix([[1,d5],1]])
Tran_a5 = Matrix([[1,a5],1]])
RotX5 = Matrix([[1,np.cos(alfa5),-np.sin(alfa5),np.sin(alfa5),1]])
A45 = RotZ5 * Trans_d5 * Tran_a5 * RotX5
d6 = 65
a6 = 0
alfa6 = np.deg2rad(0)
RotZ6 = Matrix([[c6,-s6,[s6,c6,1]])
Trans_d6 = Matrix([[1,d6],1]])
Tran_a6 = Matrix([[1,a6],1]])
RotX6 = Matrix([[1,np.cos(alfa6),-np.sin(alfa6),np.sin(alfa6),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] 是之前使用 Symbolic 变量进行代数运算的结果，但simple() 或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)
手动复制/粘贴后：
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',Pow(Symbol('cos(th2)'),Mul(Float('199.27071513852684',Symbol('cos(th2)')),Mul(Integer(-1),Float('293.0',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'))"
Trig 标识仅在您具有触发函数但名称恰好看起来像触发函数的符号与实际触发函数不同的情况下才适用。当您复制粘贴 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)