机器人与3D视觉 Robotics Toolbox Python 二 空间位姿描述
空间位姿描述
二维空间位姿描述
二维空间位姿表示方法
from spatialmath.base import *
from spatialmath import *
T1 = SE2(x=3,y=3,theta=30,unit="deg")
trplot2(T1.A,frame="T1",dims=[0, 5, 0, 5])
T2=transl2(3, 4)
trplot2(T2,frame="T2",dims=[0, 5, 0, 5],color='red')
三维空间姿态描述
R1=SO3.Rx(60,"deg")*SO3.Ry(30,"deg")*SO3.Rz(50,"deg") #绕x轴旋转60°，再绕y轴旋转30°，再绕z轴旋转50°
trplot(R1.A,frame="A", color= "b")#画出旋转矩阵R1
tranimate(R1.A,fram="A", color= "b")#将R1的变换做成动画
R2=SO3.Rz(50,"deg")*SO3.Ry(30,"deg")*SO3.Rx(60,"deg");#绕z轴旋转50°，再绕y轴旋转30°，再绕x轴旋转60°
trplot(R2.A,frame="B", color="r")#画出旋转矩阵R2
tranimate(R2.A,frame="B",color= "r")#将R2的变换做成动画
角度表示法
欧拉角表示方法
>>> R3=SO3.Rz(0.1)*SO3.Ry(0.2)*SO3.Rz(0.3)#构造旋转矩阵R3
>>> print(R3)
0.9021 -0.3836 0.1977
0.3875 0.9216 0.01983
-0.1898 0.05871 0.9801
>>> R4=eul2r(0.1,0.2,0.3)# 欧拉角转化为旋转矩阵
>>> print(R4)
[[ 0.902113 -0.38355704 0.19767681]
[ 0.3875172 0.92164909 0.01983384]
[-0.18979606 0.0587108 0.98006658]]
>>> eul=tr2eul(R3.A)# 旋转矩阵转化为欧拉角
>>> print(eul)
[0.1 0.2 0.3]
>>>
RPY角表示方法
>>> R5=SO3.Rz(0.3)*SO3.Ry(0.2)*SO3.Rx(0.1)# 构造旋转矩阵R5
>>> print(R5)
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.03696
-0.1987 0.09784 0.9752
>>> R6=rpy2r(0.3,0.2,0.1)# rpy角转化为旋转矩阵
>>> print(R6)
[[ 0.97517033 -0.03695701 0.21835066]
[ 0.0978434 0.95642509 -0.27509585]
[-0.19866933 0.28962948 0.93629336]]
>>> rpy=tr2rpy(R5.A)# 旋转矩阵转化为rpy角
>>> print(rpy)
[0.1 0.2 0.3]
双向量表示法
a=[1, 0, 0]
o=[0, 1, 0]
R7=SE3.OA(o,a) #将双向量o,a转化为旋转矩阵R7
trplot(R7.A,frame="A", color= "b")#画出旋转矩阵R7
向量旋转角表示法
vec=[1,0,0]
R8=angvec2tr(theta=20,v=vec,unit='deg')
trplot(R8,frame="A", color= "b")#画出旋转矩阵R8
四元素表示法
s=0.98335
v=[0.034271, 0.10602, 0.14357]
Q=UnitQuaternion(s,v) # 组成四元数
R9=q2r(Q.A) #四元数转为旋转矩阵
Q1=r2q(R9) #旋转矩阵转为四元数
rul=Q.eul(unit='deg') #四元数转为欧拉角
rpy=Q.rpy(unit='deg',order='zyx') #四元数转为rpy角
trplot(R9,frame="R9", color= "b")#画出旋转矩阵R9
