空间位姿描述

二维空间位姿描述

二维空间位姿表示方法

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