周筆記（17/52） - RoboticManipulation - Rigid Body Motion（二）

剛體速度，不是簡單地點可以模擬的~ 要用twist！

---

（2017.5.6更新）又看了幾遍第3節和第4節，趕腳好像終於理出一條線了……
第3節其實講述的是 vector 和 matrix 的變換：
- 旋轉矩陣(SO3) <=> 旋轉軸和角度(so3)
- 剛體變換矩陣(SE3) <=> 旋轉軸和角度和平移(se3)
第4節則是上述變換的速度（導數）方面：
- 剛體變換矩陣速度(SE3導數) <=> se3上的表示
- 速度在不同座標系下的變換（伴隨矩陣）
第4節必須要理解的概念——spatial座標系和body座標系：
- 物體在A位置和B位置之間，本身是可以通過一個rigid motion來到達的，亦即矩陣Rab 。當A位置是變動的時，就有了Rab(t) 這個概念。然而B位置是可以一直保持不動的，其實當於參照系。
第5節講述力和速度的關係，介紹了力（剛體約束）和速度（剛體自由度）維度的關係。進入了正交系統，突然間有線性代數的趕腳了……

我一直懵逼的其實一部分是，這些有啥用？也許可能大概將來會用來快速計算之類？或者還有一些深層次的聯繫？——來自每再讀一遍就覺得一週前的自己是個白癡的我 QwQ 以及沃德天上週我記得這些筆記都是個啥~

---

旋轉速度

先考慮純旋轉運動

1. 曲線Rab(t)∈SO(3) ，初始幀A叫做 spatial coordinate，結束幀B叫做body coordinate。其狀態爲：qa(t)=Rab(t)qb ，其速度爲：
   
   vqa(t)=ddtqa(t)=R˙ab(t)qb(=R˙ab(t)R−1ab(t)Rab(t)qb)
2. 定理：對於R(t)∈SO(3),有
   R˙ab(t)R−1ab(t)∈so(3)
   R˙ab(t)Rab(t)∈so(3)
3. 設ω^sab:=R˙abR−1ab ，ω^bab:=R−1abR˙ab ，那麼
   
   ω^bab=R−1abω^sabRab
   
   
   ωbab=R−1abωsab
4. 於是速度爲：
   
   vqa(t)=ω^sabRab(t)qb=ωsab(t)×qa(t)

剛體運動

計算速度

對於gab(t)∈SE(3) ，

gab(t)=[Rab(t)0pab(t)1]

g˙abg−1ab=[R˙ab0p˙ab0][R˙Tab0−R˙Tabpab1]=[R˙abRTab0−R˙abRTabpab+p˙ab0]

也是一個twist。定義 V^sab∈se(3) ：

V^sab=g˙abg−1ab

Vsab=[vsabωsab]=[−R˙abRTabpab+p˙ab(R˙abRTab)∨]

而速度：

vqa=q˙a=g˙abqb=g˙abg−1abqa

=V^sabqa=ωsab×qa+vsab

從上式可以看出，ωsab 依然是spatial幀的角速度，不過vsab 並不是body幀的原點的速度，它可以看做是t時刻一個站在spatial frame的原點並且從原點走過的人，來測量剛體上某點的瞬時速度（the velocity of a (possibly imaginary) point on the rigid body which is traveling through the origin of the spatial frame at time t。）

伴隨矩陣

剛體運動的spatial和body速度也有着和純旋轉中類似的聯繫：

V^sab=g˙abg−1ab=gab(g−1abg˙ab)g−1ab=gabV^babg−1ab

或者寫作：

ωsab=Rabωbab

vsab=−ωsab×pab+p˙ab=pab×(Rabωbab)+Rabvbab

從而：

Vsab=[vsabωsab]=[Rab0p^abRabRab][vbabωbab]

這個矩陣是把速度和角速度進行了變換，稱作g的伴隨矩陣（今後將經常用到~）：

Adg=[R0p^RR]

定理：設ξ 是se(3)上的一個twist（其座標爲ξ∈R6 ，那麼對於任何SE(3)上的g，gξ^g−1 這個twist的座標爲 Adg

Screw motion 的速度

對於screw而言，gab(θ)=eξ^θgab(0) ，其導數經計算可得：

ddt(eξ^θ)=ξ^θ˙eξ^θ

於是spatial velocity是：

V^sab=g˙ab(θ)g−1ab(θ)=(ξ^θ˙eξ^θgab(0))(g−1ab(0)e−ξ^θ)=ξ^θ˙

V^bab=g−1ab(θ)g˙ab(θ)=(g−1ab(0)e−ξ^θ)(ξ^θ˙eξ^θgab(0))

=(g−1ab(0)ξ^gab(0))θ˙=(Adg−1ab(0)ξ)∧θ˙

座標變換（Coordinate transformation）

對於已知A->B和B->C的情況下，如何計算A->C？
一個直觀認知可以幫助理解即將到來的定理：伴隨矩陣(6x6)可以用作對不同座標系下的速度的變換。
證明利用了

V^sab=g˙ab(θ)g−1ab(θ)

以及伴隨矩陣的性質。定理爲：

Vsac=Vsab+AdgabVsbc

我的直觀上理解，Adgab 把Vsbc 從B座標系轉換到了A座標系，然後再加上Vsab 就是最終的A座標系下的Vsac 了
在body座標系下有：

Vbac=Adg−1bcVbab+Vbbc

依然從直觀上理解，Adg−1bc 把Vbab 從B座標系轉換到C座標系，再加上Vbbc 就是Vbac 了
值得注意的是，許多情況下，其中兩個座標系之間的變換是不變的（速度爲0），亦即上述式子可以簡化。這在後邊將會非常有用。

Wrench

由力（force）和矩（moment）組成
- 作用於剛體上不同位置的Wrench，其產生的效果可能是一樣的，如何定義這個“效果”？使用Vab⋅Fb
- 兩個完全相同的Wrench：其對所有可能的速度所產生的效果都相同
- 作用於兩個位置B和C的力的轉換：（根據定義可計算得到）

Fc=AdTgbcFb

Reciprocal screws

定義1：Wrench F和Twist V的 Reciprocal：F · V = 0
定義2：Screw 1 和 Screw 2的Reciprocal：Screw 1的twist V和沿着Screw 2的wrench是Reciprocal的
定義3：Screw 1 和 Screw 2的Reciprocal：兩個Screw的 Reciprocal Product爲0

Reciprocal Product

• 對於兩個Screw S1和S2（l,h,M），記α爲l1和l2之間的夾角，其Reciprocal Product爲：
  
  S1⊙S2=M1M2((h1+h2)cosα−dsinα)
  
  （事實上V·F經計算就是上邊的式子，亦即定義3和定義2其實是一回事）
• 對於兩個Twist V1和V2（v,w），其Reciprocal Product爲：
  
  V1⊙V2=vT1ω2+vT2ω1
• 這個概念可以用於求解在被wrench約束的情況下的twist
• 在有多個wrench的情況下，一個與之 reciprocal的twist，意味着在該約束下依然可以運動，這個twist可以作爲正交系統的其中一個分量加入其中，這些screw的集合稱爲system of screws。它代表剛體的約束的。
• reciprocal system screws是與之reciprocal的twist的集合，代表剛體運動的可能方向。
• 定理：上述兩個集合的維度之和爲 6