Moveit2工業機器人機械臂
機器人學
https://www.bilibili.com/video/BV1v4411H7ez
https://www.bilibili.com/video/av59243185
理論基礎
DOF(自由度)
簡單來說，自由度(以下統稱為dof)指的是 物體在空間裡面的基本運動方式 ，總共有6種。任何運動都可以拆分成這6種基本運動方式，而這6種基本運動方式又可以分為兩類： 位移 和 旋轉 。
位移：X軸、Y軸、Z軸的平動
旋轉：Roll橫滾角(繞X轉動)、Pitch俯仰角(繞Y轉動)、Yaw航向角(繞Z轉動)
數理基礎
位姿（位置與姿態）的表示
倘若在一個空間裡有一個剛體（frame），我們如何確定剛體在這個空間裡的位姿呢？
首先要建立一個世界座標系（world frame），然後要在剛體上建立剛體座標系（body frame）.
- 位置的描述
描述剛體的質心(一個點)在世界中的位置，就可以用一個3X1向量來表示.
這樣就知道了平動的三個DOF。
- 方位的描述
設世界座標系為A，剛體座標系為B。
上面這個矩陣就叫旋轉矩陣（Rotation Matrix），是一個3*3的正交矩陣，ABR描述的是A為參考座標系，B相對於A的方向。
每一個列向量，都代表B的對應座標軸各自指向的方向。
每一列向量都是B的對應的座標軸相對於A的方向餘弦（Direct Cosines）。
旋轉矩陣的每個元素 r ij代表 B 的第 j 軸與 A 的第 i 軸的方向餘弦.
實在看不懂，先來看下面來看例子：
B的X軸在A中怎麼表示？可以看出來，B的X正好是A的Z軸的負半軸，那就是0，0，-1.
B的Y軸在A中怎麼表示？可以看出來，B的Y正好是A的Y軸的正半軸，那就是0，1，0.
B的Z軸在A中怎麼表示？可以看出來，B的Z正好是A的X軸的正半軸，那就是1，0，0.
這個例子裡，AB的Z重合了，所以我們只看上視圖就可以了。
就是把XB這個單位向量，投影到A的X和Y上看分量即可。
同理YB也一樣。
ZB和ZA重合，比較簡單。
答案是B。
- 位姿的描述
通過BF在WF的狀態，就可以知道剛體在世界中的位姿。
- 運動的描述
紅色是剛體運動的軌跡線，
軌跡（平動DOF）對時間的微分(導數)，就是剛體線速度。
剛體速度再對時間的微分(導數)，就是剛體線加速度。
同理,轉動DOF對時間的微分(導數)，就是剛體的角速度。
角速度再對時間的微分(導數)，就是剛體角加速度。
旋轉矩陣
特性：
由於旋轉矩陣R裡每個元素都是兩個向量內積，內積是可以交換位置且最後結果數值不變的。
所以我們選擇交換位置。
原始的R是每一 列 都是B的某一軸在A系的分量。
交換位置後的R是每一 行 都是A的某一軸在B系的分量。
結論：所以說Rab = Rba的T。
他倆明顯是轉置關係。
RT*R=I3（3*3單位陣）(正交陣orthogonal matrix的性質)
還有個性質就是|R|=1
雖然有9個數字，但是啊，因為正交陣的性質，所以是有約束條件的，這9個數字裡有6個數字是隨著其他數字變化而變化的，所以這9個數字實際上只有3個參數可以任意選擇，也就是旋轉矩陣實際上只有3個自由度。(轉動DOF)
旋轉矩陣的一個功能如下：
比如說另一個座標系B相對於A座標系繞X,Y,Z軸各自逆時針轉動theta度的旋轉矩陣。
在中國大陸教材中，常用R(X,theta)來代替圖中的RXA(theta)。圖中這個A指的是原座標系，得出來的AP`也是原座標下的座標。
這樣的話，AP左乘一個R就得出來了P轉動後在A系的座標。（一定要與下面講的旋轉座標變換分清楚，很容易混淆）
P逆時針轉動30度後，在原座標系中的座標為002.
總結：旋轉矩陣主要是三種用法，如下圖：
座標變換
- 平移座標變換
- 旋轉座標變換
這裡的APX是個數值，XA等是矢量，加法是矢量加法，最後得出來的是AP向量。
重要結論就是AP = ABR*BP.(注意是矩陣的左乘)
AP就是P在A系的座標。
BP就是P在B系的座標。
ABR就是A為參考座標系，B相對於A的旋轉矩陣。
物體的變換及逆變換
物體平動的順序可以互相顛倒,但是物體轉動的順序不能互相顛倒,否則姿態會不一樣.
主要是兩種拆解方式,一個是設一個固定的座標系,一直按這個座標系轉動,
另一個方式是假設物體的座標系.
機械臂描述方式
Link 0一般也叫base_Link
先看好相對關係，Axis i-1的後面才是Link i - 1(當然其他描述也成立)
描述各關節之間的關係
也就是公垂線(唯一解)，其長度為Link Length連桿長度
現在只是限制住了兩個軸的距離，兩個軸還是可以轉動的，所以需要下一個參數，Link Twist連桿扭角。
這個角就是沿中垂線，把後一個軸線沿中垂線往當前軸移動，然後形成的夾角叫Link Twist連桿扭角。
也就是說，針對空間中任意兩個轉軸，我們需要兩個參數來進行描述，也就是Link Length和Link Twist。
如果是多個串起來的轉軸，我們就無法找到對應關係了，比如說
上面這個圖，我沒法表示ai-1與ai在軸線Axis i上的相對關係以及相對姿態是什麼樣子的。
所以還需要其他參數。
首先肯定需要一個長度，兩個公垂線在Axis i上的距離，叫做Link Offset，連桿偏距。
然後還需要一個角，Joint Angle連桿夾角，也叫關節角。
其實發現，這四個參數，只有一個參數是變化的，其他都是固定的。
如果ioint type是revolute joint，那麼thetai變化，其他不變。
如果joint type是prismatic joint，那麼di變化，其他不變。
描述兩個軸需要2個參數，連桿長度與連桿扭角。
描述多個軸串在一起需要4個參數(每兩兩杆件都需要4個參數)，連桿長度Link Length，連桿扭角Link Twist，連桿偏距Link Offset，連桿夾角Joint Angle。
在joint上建立frame
咱們一般把Z方向定義成和轉軸的方向一樣，Z朝上或朝下是看怎麼朝向，這兩個軸的夾角最小，這樣就能夠確定Z的方向了。
Xi的方向是沿著ai的方向。
Xi與Zi+1和Zi都垂直。
右手定則，判斷Y方向。
原點是Z和X的交點。
若是建立base_link(link0)與link1的話，則是特殊情況。（base_link是immobile不動的）
frame0和frame1重疊(重合)。通常，比如說是個旋轉關節，雖然規定theta是arbitrary任意的，但是我把theta固定成0，然後讓frame0和frame1(theta
= 0)重合。如果是平動關節，那麼同理也取d=0的時候的frame1。注意，這裡是重疊重合，並不是形狀相同，而是完全重疊的座標系。
最後一個杆件，也差不多，因為Xn和Xn-1都要垂直於Axis n(Zn)，所以最簡單的方法就是讓Xn與Xn-1方向一致。
Xn取Xn-1的方向。也就是framen和framen-1是延長的。
下面是重點中的重點：(有好幾種方法判斷，如有錯誤請討論後修改)
需要知道的常識：
- 一般連桿扭角按逆時針是正值。
- 我們常說的轉向，是從逆著的方向去看，也就是讓箭頭指向眼睛的方向去看的。
- 從軸的逆方向去看和順著方向去看轉向，是完全相反的方向，但是在平面上容易產生視覺錯覺，難以理解。可以拿支筆或者電機，轉動一下試試。
①alphai-1是正值還是負值，要看Zi-1到Zi的角是順時針還是逆時針，伸出右手，讓拇指沿Xi-1的方向，如果alphai-1順著四指方向則為逆時針，正值，反之為順時針，負值。
所以如圖，alphai-1是逆時針，所以是正值。
②ai-1的長度因為是長度，所以永遠是正值，然後值為Z軸間的相對距離。
③thetai的角度也基本同理，將右手大拇指沿著Zi的方向，若thetai順著四指方向則為逆時針，逆著則為順時針。
④di的大小方向要看從ai-1沿著zi的方向到ai，則是正值，反之為負值，大小即為距離。
Link Transformations
理論
我們兩個關節都有倆軸，一個Axisi-1一個是Axisi，我們也有倆frame，一個是framei-1一個是framei。
我們需要找到兩個frame之間的關係式是什麼，也就是找到變換矩陣Transformation Matrix。
然後將Trans Matrix量化即可。
假設說有一個點P，他在frame i下的表達是Pi，如果我們找到了Ti-1 i的矩陣，那麼就有辦法，獲得P在frame i-1下的表達了。
所以我們現在需要，用剛才找到的四個參數，轉化成我們的Trans Matrix。
這四個參數，也就是ai-1，alphai-1，di和thetai，足以可以表達framei-1到framei了。
①首先在Axis i-1上，
先描述alpha，就把framei-1的Zi-1旋轉到差不多Zi的方向，生成FrameR（只旋轉Z，X不動，然後右手定則判斷Y）
②然後描述a，
就把FrameR沿著ai-1的方向移動到Zi上，
③再描述theta
我們轉動frameR中的ZR，使其與ai方向相同，生成frameP（X動，Z不動，右手定則判斷Y）
這樣搞完之後，Xp的方向與Xi是相同的，Zp的方向也與Zi相同。
④再描述d，也就是把FrameP往上拉，最後會與Framei重合。
也就是從Framei - 1到Frame R，然後再到Frame Q，然後再到Frame P，最後到Frame i。一共四次轉化。
剛才我們演示的是從Pi-1到Pi，現在我們要求的是從我們的Pi要到Pi-1，那麼就是倒著左乘，先乘Tp i接著往下以此類推。
從連桿i到連桿i-1的座標系間的齊次變換矩陣T i-1 i=Rot(X，aplhai-1)Trans(ai-1,0,0)Rot(Z,thetai)Trans(0,0,di)
左上角是3X3的旋轉矩陣，所以只有角度theta和alpha參數，
右上角3X1的矩陣，也就是frame i的原點相對於frame i - 1的原點的向量。然後是從frame i - 1去看。所以他是長度與角度的複合。
最後一行的1X4的矩陣，是0001是固定數不動的。
對於連續的杆件，我們可以從base_link一直算到linkx，x想是幾就是幾。
比如說，現在有三個杆件，我們找到T23（3對2的），T12（1對2的）T01（地對1的），就可以找到T03（地對3的）
Example
平面RRR類型
①先找到關節轉軸Joint Axes
三個轉軸是三個紅點，是點的話，也就是出紙面的方向。（因為，他是個平面的，所以說，每個Z軸之間的角度都是0，所以說Link Twist Alpha是0，所以Z的方向都隨便取，但是咱們這裡，假設關節都是逆時針旋轉，按右手螺旋定則來看，Z就都朝上）
②再找到公垂線Common Perpendiculars
但是由於Axis都是互相平行的，所以說，這個公垂線有無數多條，所以我們就在一個平面內表達即可。
③下一步定義Zi向量（Z方向與轉軸方向相同所以也是向上。）
④然後判斷中間的Xi向量
⑤然後判斷中間的Yi向量
⑥然後判斷頭和尾，也就是frame 0 和frame n
其實，我們可以把frame0和frame1建的完全重合，就是讓alpha和theta都為0，如圖並沒有重合，所以有theta大小。
最後一個杆件也一樣，建議讓X3的方向和X2方向重合，這樣的話，theta和alpha都是0。
因為每個軸都是平行的，所以alpha是0，由於ai都是同平面的，所以d也為0.
因為frame0和frame1重合，所以a0 = 0，然後a1 = L1，a2 = L2
由於全是RRR，所以，theta都是變化的角。
P點末端執行器，也可以算出，沿X3方向走，獲得P在frame n的表達，然後就可以推出P在frame 0中的表達了。
如圖的，P點在Frame3裡的座標是（L3，0，0）
RPR類型
①先找到轉軸
按右手螺旋來。
②找公垂線
因為frame1的Z1和frame2的Z2相交，所以，沒有公垂線。
frame2的Z2和frame3的Z3重合，所以也沒有公垂線。
也就是說a全是0.
③建立Zi向量
Zi方向與轉軸方向相同。
④Xi的向量
當Z1和Z2相交時，我們挑X的方向就挑和Z1和Z2都垂直的，有兩種方案，要麼X往前，要麼往後，如圖是往後的。
X1和X2必須平行，因為是個P類型的關節
⑤Y軸
⑥Frame 0和frame n
frame 0 和frame1重合
讓X3方向與X2方向相同
如圖，驅動的關節參數分別是theta1，d2，theta3
由於frame0和frame1重合，那麼alpha0 = 0，
從Z1到Z2的角，伸右手大拇指指向X1，然後四指與Z1到Z2方向相同，所以是逆時針，為正值，所以是90度。
然後fame2到frame3，Z共線，所以alpha2=0
然後由於Zi有的相交，有的共線，所以a全是0
然後d1是0，因為frame0和frame1重合，
d2是d2，d3是L2（d是X在Axis上的距離）
P點在Frame3上的座標為（0，0，L3）
其實Z方向有倆選擇，X也有倆選擇，一共四種選擇，選擇自己好理解，好計算的方案即可。
中國臺積電晶圓機器人(PRRR類型4個自由度)
SCARA機器人(RRRP類型4個自由度)
該機器人最後一個關節是既可以R又可以P的，所以是個RP關節，既可以先算R也可以先算P。
RP類型
選D，因為倆自由度，所以有倆驅動參數。
執行器關節與笛卡爾空間(Actuator Joint and Cartesian Spaces)
我們這裡的驅動是theta1-3，
當我們知道theta1-3的值之後，我們就會知道P點在世界座標系上的表達。
這被叫做正向運動學（Forward Kinematics）。
由P點世界座標系反算關節角度，那麼叫逆向運動學（Inverse Kinematics）。
Actuator Space就是驅動器空間，比如一個電機怎麼操控能轉joint space下的固定角（經過一系列轉換）。
有轉動有移動的部分（所以需要兩個電機來達到這兩個自由度）
同步帶機構使其旋轉。（第一個電機）
齒輪齒條機構達到上下移動。（第二個電機）
但是，這兩個並不是獨立的，因為是同軸驅動的，所以有些負荷。
兩個轉動變成一個轉動一個移動。
正運動學
定義 ：已知機器人各個關節（或輪子等驅動單元）的運動參數（如角度、位移、速度等），計算末端執行器的位置和姿態。
ads=dv/dt * ds = ds/dt *dv = vdv
牛頓第二定律，能量守恆，衝量與動量
Wp就是P點在世界座標系下的座標
可以根據該公式，得知末端執行器在世界座標系中的座標。
逆運動學
定義 ：已知末端執行器的目標位置和姿態，計算需要讓各個關節（或輪子等驅動單元）運動到什麼角度或速度才能達到該目標。
先知道末端執行器的某個點P在世界座標系中的表達，也就是給出Pw或者末端執行器某個點上的frameH，
通過Pw求出theta。
這樣手臂就有6個未知數
16個數字，轉動的部分佔了9個數字，也就是左上角的3X3的旋轉矩陣，然後右上角3X1的向量表示相對於原點的位移量是什麼。（也就是frame6的原點相對於frame0的原點位移量是什麼）
下面的0001是整數，固定的，不變的。
這個旋轉矩陣裡，有3個長度條件，3個互相垂直條件，所以9個數字裡，就剩3個自由度。（也就是向量長度為1限制3個，向量兩兩垂直限制3個，所以是平移矩陣，3個自由度）
然後右上角3X1的向量中，相對原點的座標X,Y,Z,那麼就是3個自由度。
所以總共有6個自由度。
這12個方程式就是除了低下的0001，上面的參數都可以列一個式子。我們要做的，就是從12個式子中求出6個未知數。
靈活工作空間Dexterous workspace是可達工作空間Reachable workspace的子集。
它的可達工作空間是個圓環。
對於某個點，在這個例子中，只有1種或2種姿態可以達到。這個機器人只有RWS，沒有DWS。
手臂一樣長的話，那樣工作空間就是個圓了，
有一個點就是DWS,就是原點，當手臂內折，那麼可以以360度任意一個角度來達到這個點，所以該點就是DWS。