空間位姿描述

二維空間位姿描述

二維空間位姿表示方法

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