ぱたへね

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

Modern Robotics: Mechanics, Planning, and ControlのWrench

Robotics

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

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

www.amazon.co.jp

Wrenchはモーメントと力を並べた1x6の行列です。教科書の初出は92ページ。

m_a = r_a \times f_a \in \mathbb{R}^3

mはモーメント、もしくはトルクと呼びます。

このmとfを並べた物がWrenchです。教科書によって並び順が違うときがありますが、この教科書はこの順番です。

F =  \begin{bmatrix} m_a \\  f_a \end{bmatrix} \in \mathbb{R}^{6}

一つの剛体に複数のWrenchが存在する場合は、単純に足し算になります。

複数のフレーム缶のWrenchには以下の関係式があります。これはTwistと同じですね。

F_a = [Ad_{T_{ab}} ] F_b
F_b = [Ad_{T_{ab}} ]^T F_a

[Adt]は、前回の記事のadjoint representation。

natsutan.hatenablog.com

実際の計算

教科書のExample 3.28から

f:id:natsutan:20211211115958p:plain
Figure 3.22

リンゴを持っているHandの図です。

  • リンゴの重さは0.1kg
  • 重力加速度は g = 10 m/s2
  • Handの重さは0.5kg
  • L1 = 10cm, L2 = 15cm

ここからfのWrenchを計算します。座標系のaが今まではspace寄りだったのですが、今回はAppleのaで一番遠い所にあることに注意すれば大丈夫。 まず、リンゴとHandそれぞれの自分にかかるFを計算します。ここでは相互作用は気にしません。

F_h = (0, 0, 0, 0, -5, 0)
F_a = (0, 0, 0,0, 0,1)

自分自身にかかる重力加速度のみですが、リンゴとHandで座標の向きが違います。

いつもの行列Tを計算します。計算方法はここ。

natsutan.hatenablog.com

T_{hf} =  \begin{bmatrix} 1 & 0 & 0 & -0.1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 
T_{af} =  \begin{bmatrix} 1 & 0 & 0 & -0.25 \\ 0 & 0 & 1 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

これらをまとめて、fのWrenchを計算します。

F_f =  [Ad_{T_{hf}}]^T F_h + [Ad_{T_{af}} ]^T F_a   =   \begin{bmatrix} 0 \\ 0 \\ -0.5 \\ 0 \\ -5 \\ 0  \end{bmatrix} +  \begin{bmatrix} 0 \\ 0 \\ -2.5 \\ 0 \\ -1 \\ 0  \end{bmatrix} =  \begin{bmatrix} 0 \\ 0 \\  -0.75  \\ 0 \\ -6 \\ 0  \end{bmatrix}

結局の所、下向きの力はリンゴとHandの重さ分、モーメントはfからみたリンゴのモーメントとHandのモーメントの和になっています。