ぱたへね

はてなダイアリーはrustの色分けができないのでこっちに来た

Modern Robotics: Mechanics, Planning, and Controlのadjoint representation

Robotics

教科書から分かったところだけをまとめています。

教科書はこの本です。今回はadjoint representation。

www.amazon.co.jp

adjoint representationで検索すると随伴表現という難しい言葉が見つかりますが多分分からなくてもOK。

定義はp85にあります。

並進と回転の行列 T=(R,p) \in SE(3) が与えられた時、そのadjoint representation、[Adt]はこうなります。

[Ad_T] =  \begin{bmatrix} R & 0 \\  [p] R & R \end{bmatrix} \in \mathbb{R}^{6 \times 6}

この行列を使うと、twist間の変換ができます。

\nu_s =  [Ad_{T_{sb}}]  \nu_b

多分これだけのことを難しく言ってるんだと思う。

実際の計算

natsutan.hatenablog.com

前回2つのtwist NsとNbを求めたので、2つのadjoint representationが上手くいくのか計算してみます。

登場人物の整理

T_{sb} =  \begin{bmatrix} -1 & 0 & 0 & 4 \\ 0 & 1 & 0 & 0.4 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

と

\nu_s = (0, 0, 2, -2 ,-4, 0)^T
\nu_b = (0, 0, -2, 2.8 ,4, 0)^T

adjoint representation

RがT左上の3x3、[p]が行列の右端（4, 0.4, 0）を下のルールで3x3に展開した行列です。

 [\hat \omega ]  =  \begin{bmatrix}  0 & -\hat \omega_3 &  \hat \omega_2 \\  \hat \omega_3 & 0 & - \hat \omega_1  \\  -\hat \omega_2  & \hat \omega_1 & 0 \end{bmatrix}

[p]Rだけ先に計算すると

[p]R =  \begin{bmatrix} 0 & 0 & 0.4 \\ 0 & 0 & -4 \\ -0.4 & 4 & 0  \end{bmatrix}   \begin{bmatrix} -1 & 0 & 0 \\ 0 & 1 & 0 \\ 0  & 0 & -1  \end{bmatrix} =   \begin{bmatrix} 0 & 0 & -0.4 \\ 0 & 0 & 4 \\ 0.4 & 4 & 0  \end{bmatrix}

あとは6x6に並べるだけ

[Ad_{T_{sb}}] =  \begin{bmatrix} -1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & -1 & 0 & 0 & 0 \\ 0 & 0 & -0.4 & -1 & 0 & 0 \\ 0 & 0 & 4 & 0 & 1 & 0 \\ 0.4 & 4 & 0 & 0 & 0 & -1 \end{bmatrix}

この行列を使って実際に計算すると結果が一致するのが分かります。手計算でもなんとかなります。

[Ad_{T_{sb}}] \nu_b =  \begin{bmatrix} -1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & -1 & 0 & 0 & 0 \\ 0 & 0 & -0.4 & -1 & 0 & 0 \\ 0 & 0 & 4 & 0 & 1 & 0 \\ 0.4 & 4 & 0 & 0 & 0 & -1 \end{bmatrix}  \begin{bmatrix} 0 \\ 0 \\ -2 \\ 2.8 \\ 4 \\ 0  \end{bmatrix} =   \begin{bmatrix} 0 \\ 0 \\ 2 \\ 2 \\ 4.8 \\ 0  \end{bmatrix}

ちゃんとNsの値に一致します。 手計算すれば分かりますが、回転のωに関しては座標軸の変換だけ、リニアな速度に関しては位置の変換と座標軸の変換が行われます。