sympy 符号计算库在机器人旋转矩阵方面的应用（代码）

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([