Rigid-Body Motion(旋转变换\Screw motion\Spatial velocities\Force)

\[SE(3)=T=\begin{bmatrix}R&p\\0&1\end{bmatrix} \]

SE(3)是一个４X4的矩阵．它具有的性质如下：

1. 两个SE(3)的乘积仍是一个SE(3)

2. SE(3)的逆：　$$T^{{-1}=\begin{bmatrix}R}T&-R^Tp\0&1\end{bmatrix}$$

3. \(T=(R,p)\in R^3, x,y\in R^3\),则有：

   • \(||Tx-Ty||=||x-y||,||．||\)指标准欧式距离，如\(||x||=\sqrt{x^Tx}\)
   • \(<Tx-Ty,Ty-Tz>=<x-y,y-z>\),\(<.>\)指内积，如\(<x,y>=x^Ty\)
     上述性质中，T是某一点的一个transformation，如T将一点x转换至Tx．３中第一个性质保持距离不变，第二个性质保持角度不变．

在上图中，有$$R_{ab}R_{bc}=R_{ac}$$ $$\vec u+\vec v=\vec w \qquad u_a=\vec u \qquad v_b=\vec v$$
现在直接将\(\vec u\)与\(\vec v\)想加是没有意义的，因为他们是基于不同的坐标系的．所以：

\[v_a=R_{ab}\to u_a+R_{ab}v_b=W_a \]

\[\begin{bmatrix}R_{ab}&u_a\\0&1\end{bmatrix}\begin{bmatrix}R_{bc}&v_b\\0&1\end{bmatrix}=\begin{bmatrix}R_{ab}R_{bc}&u_a+R_{ab}v_b\\0&1\end{bmatrix} \]

因此：

\[T_{ab}T_{bc}=T_{ac} \]

5. \(T_{aa}=I\qquad T_{ab}^{-1}=T_{ab}\)

Screw Motion##

这是在Exponential Coordinate下同时表示旋转和平移的一种表示．
给定\(w,v\in R^3\):

\[S=\begin{bmatrix}w\\v\end{bmatrix}\in R^6 \]

S被称为twist，定义\([S]\in R^{4X4}\)是如下的矩阵：

\[[S]=\begin{bmatrix}[w]&v\\0&0\end{bmatrix} , [w]=\begin{bmatrix}0&-w_3&w_2\\w_3&0&-w_1\\-w_2&w_1&0\end{bmatrix} \]

根据以上的定义，我们需要推到出\(e^{[S]\theta}\).
现在给定一个旋转和平移的transform，如何求得对应在Exponential Coordinate下的\(e^{[S]\theta}\)?

以下给出算法：
已知\((R,p)\in SE(3)\),求\(e^{[S]\theta}\)：

1. 如果\(R=I\),那么\(w=0, v=p/||p||, \theta=||p||\)
2. 如果\(tr R=-1\),那么\(\theta=\pi , [w]=logR\)
3. 否则：

\[\theta=cos^{-1}(\frac{trR-1}{2})\in [0,\pi) \]

\[[w]=\frac{1}{2sin\theta}(R-R^T)$$ $$v=G^{-1}(\theta)p \]

其中，\(G^{-1}(\theta)=\frac{1}{\theta}I+\frac{1}{2}[w]+(\frac{1}{\theta}-\frac{1}{2}cot\frac{\theta}{2})[w]^2\)

给定\(S=(w,v)\in R^6,S'=(w',v')\in R^6,T=(R,p)\in SE(3)\),Adjoint map \(S'=Ad_T(S)\)定义：

\[\begin{align*}S&=\begin{bmatrix} w' \\ v' \end{bmatrix} \\ &= \begin{bmatrix} R&0 \\ [p]R&R \end{bmatrix}\end{align*}\]

\([Ad_T]\)是个６X6矩阵．
使用以上的定义，我们可以有：

\[S'=[Ad_T]S \]

还可以这样表示：

\[[S']=T[S]T^{-1} \]

\[\begin{bmatrix}[w']&v'\\0&0\end{bmatrix}=\begin{bmatrix}R[w]R^T&[p]Rw+Rv\\0&0\end{bmatrix}?? \]

性质：

1. \(Ad_{T_1}(Ad_{T_2}(S))=Ad_{T_1T_2}(S)\)
2. \(Ad_T^{-1}=Ad_{T^{-1}}\)
3. \(Ad_{T^{-1}}(Ad_T(S))=Ad_{T^{-1}T}(S)=Ad_I(S)=S\)

假定\(S_a=(w_a,v_a)\)是在a坐标系下的screw motion,\(S_b=(w_b,v_b)\)是在b坐标系下的screw motion,则：

\[s_b=Ad_{T_{ba}}(S_a)$$$$s_a=Ad_{T_{ab}}(S_b) \]

Spatial Velocities##

\(\dot RR^{-1}\)或者\(R^{-1}\dot R会得到角速度向量．　类似的我们可以得到：\)

\[\begin{align*}T^{-1}\dot T&=\begin{bmatrix}R^T&-R^Tp\\0&1\end{bmatrix}\begin{bmatrix}\dot R&\dot p\\0&0\end{bmatrix}\\ &=\begin{bmatrix}R^T\dot R&R^T\dot p\\0&0\end{bmatrix}\\ &=\begin{bmatrix}[w_b]&v_b\\0&0\end{bmatrix}\end{align*}\]

S表示固定参参考系，B表示移动参考系（body coordinate），\(T_{sb}\in SE(3)\):

\[T_{sb}(t)=\begin{bmatrix}R(t) & p(t) \\ 0 & 1\end{bmatrix} \]

所以有：

\[T_{sb}^{-1}\dot T_{sb}=[V_b]=\begin{bmatrix}[w_b&v_b\\0&0]\end{bmatrix} \]

以上称之为移动参考系中的Spatial Velocity.

\[\dot T_{sb}T_{sb}^{-1}=[V_s]=\begin{bmatrix}[w_s]&v_s\\0&0\end{bmatrix} \]

以上称之为固定参考系中的Spatial Velocity.
\(V_s\)和\(V_b\)关系如下：

\[V_s=\begin{bmatrix} w_s \\ v_s \end{bmatrix}=\begin{bmatrix} R&0 \\ [p]R&R \end{bmatrix}\begin{bmatrix}w_b\\v_b\end{bmatrix} = Ad_{T_{sb}}(V_b) \]

\[V_b=\begin{bmatrix} w_b \\ v_b \end{bmatrix}=\begin{bmatrix} R^T&0 \\ -R^T[p]&R^T \end{bmatrix}\begin{bmatrix} w_s \\ v_s \end{bmatrix} = Ad_{T_{bs}}(V_s) \]

Spacial Force##

假设一个力f作用在刚体的一点p上．给定一个a参考系，\(f_a \in R^3\)是在a坐标系下的f的表示，产生力矩\(m_a\):

\[m_a=r_a\times f_a \]

其中\(r_a\in R^3\)表示坐标系a中原点到p的向量，将力\(f_a\)和力矩\(m_a\)形成\(F_a=(m_a,f_a)\),这个称之为在a坐标系下的Spatial Force.
现在确定两个坐标系a与b中Spatial Force：　\(F_a={m_a,f_a}\)与\(F_b={m_b,f_b}\)的关系:
\(T_{ab}=\begin{bmatrix}R_{ab} & p_{ab} \\ 0 & 1\end{bmatrix}\)

在上图中，显然$$f_b=R_{ba}f_a$$
\(r_b=R_{ba}(r_a-p_{ab})\),这是因为\(r_a-p_{ab}\)是在a坐标系中表示的，转换到b坐标系下需要诚意R_{ba},注意与之前的在同一个坐标系中表示旋转的向量点的不同:

\[r_b=R_{ab}^T(r_a-p_{ab}) \]

力矩\(m_b=r_b\times f_b\)就可以写成：

\[\begin{align*}m_b &= R_{ab}^T(r_a-p_{ab})\times R_{ab}^Tf_a\\ &=[R_{ab}^Tr_a]R_{ab}^Tf_a-[R_{ab}^Tp_{ab}]R_{ab}^Tf_a\\ &=R_{ab}^T[r_a]f_a-R_{ab}^T[p_{ab}]f_a\\ &=R_{ab}^Tm_a+R_{ab}^T[p_{ab}]^Tf_a\end{align*}\]

因为\([p_{ab}]^T=-[p_{ab}]\),所以：

\[\begin{bmatrix} m_b \\ f_b \end{bmatrix}=\begin{bmatrix} R_{ab} & 0 \\ [p_{ab}]R_{ab} & R_{ab}\end{bmatrix}^T\begin{bmatrix} m_a \\ f_a \end{bmatrix} \]

或者可以表示如下：

\[F_b=Ad_{T_{ab}}^T(F_a)=[Ad_{T_{ab}}]^TF_a \]