sympy 符号计算库在机器人旋转矩阵方面的应用(代码)
机器人旋转变换
一、变换矩阵
若计算变换矩阵是基于旋转坐标系(上一个坐标系)来计算的,则计算过程用右乘;若计算变换矩阵是基于固定坐标系(初始坐标系)来计算的,则计算过程用左乘,同时,若已知变换矩阵计算坐标系中某点坐标,也使用左乘。
以下代码是在Jupyter中运行的,同时也是基于旋转坐标系的公式推导,先Z再Y最后Z。
from sympy import sin, cos, pprint
x,y,z=sym.symbols('x,y,z')
Q_z=sym.Matrix([[cos(x),-sin(x),0],[sin(x),cos(x),0],[0,0,1]])
Q_y_1=sym.Matrix([[cos(y),0,sin(y)],[0,1,0],[-sin(y),0,cos(y)]])
Q_z_2=sym.Matrix([[cos(z),-sin(z),0],[sin(z),cos(z),0],[0,0,1]])
Q=Q_z*Q_y_1*Q_z_2
pprint(Q)
二、证明
1、det(Q)=1
from sympy import pi,sin,cos,pprint,Symbol,simplify
x=Symbol('x')
Q=sym.Matrix([