ぱたへね

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

Modern Robotics: Mechanics, Planning, and Control Specialization

去年から仕事でロボティクスに関わるようになり、いつまでも素人では駄目だと思いCourseraのコースで勉強することにしました。

www.coursera.org

全部で６コースあり、難易度が中級で結構ヘビーです。

動画は短く、勉強の大半は先生が書いた教科書を読んで勉強します。教科書もほぼ飛ばすこと無く、前から順番に全ページ読んでいます。quizの設定値が教科書に載っていることもあり教科書は必須です。

勉強しながら良く分からないことばかりで日本語の教科書を買いあさって補完するつもりでしたが、ロボティクスには流派がある気がしていてこの先生の教え方を日本語で説明している本はありませんでした。今までの経験から言うと、理解してから読むとああ同じ事言ってるなって感じるパターンだと思います。 ロボティクス自体が、表現の次元を増やして冗長性を持つテクニックを常用しており、その冗長性の持ち方が複数ある感じです。分かりやすいところでは、角度（向き）の表現がクオータニオンかロールピッチヨーの複数の表現があって、そういうのが角度以外の所でもある雰囲気です。いったん全コース終わった後、また別の本で勉強し直さないといけない感じがしています。

年末やっていて感じたことをまとめます。

教科書は紙の本が良い

数式を見直す事が多く行ったり来たりします。少し前の章のロボットの設定が出てくることもありKindleだと厳しかったです。地味に紙の本の方が版が新しいので、その意味でも紙をお勧めします。

章末をチェック

章末にサマリーがあってこれが良くまとまっています。Kindleで順番にquizに挑戦していると忘れがちですが、サマリーを見てからquizに望むと結構簡単に解けるときがあります。また、練習問題を解くために先生が用意してくれた便利関数の説明もあります。quizによっては、これ自力で実装するの難しい・・・って問題もその関数を使うだけでクリアー出来るのが多いです。一回は眼を通しましょう。

分からなくなったら戻ろう

1２月頭にヤコビアンの所で完全に詰まってしまい、分かるところまで戻ってブログにまとめていったのが効きました。順を追って数学から説明する本なので、初見では何のためにやっているのか分からないことが多いです。いったん進められるところまで進めて、駄目だと感じてから遡って勉強するのも良いと思います。

実績

６コース中２コースまで完了しました。昨年の間に３コース終わらせたかったのですが、予想以上に難しかったです。

coursera.org

coursera.org

Modern Robotics: Mechanics, Planning, and ControlのBodyヤコビアン

Robotics

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

教科書はこの本です。

www.amazon.co.jp

今回はBody Jacobian

この記事のSpaceヤコビアンを置き換えるだけ。

natsutan.hatenablog.com

 [ \nu_{s} ] =\dot T T^{-1}

に対応するのがこの式になります。

 [ \nu_{b} ] =T^{-1}\dot T

ここから

 T(\theta)  = Me^{[B_1] \theta_1}e^{[B_2] \theta_2} \cdots e^{[B_n] \theta_n}

を使って最後はこう。

  \nu_{b}  =  \begin{bmatrix} J_{b1} & J_{b2} & \cdots & J_{sn} \end{bmatrix}  \begin{bmatrix} \dot \theta_1 \\ \vdots \\ \dot \theta_n \end{bmatrix}  = J_b(\theta) \dot \theta

エンドエフェクタの座標系におけるツイストはbodyヤコビアンと各θの速度で表現できるを示しているで良いのかな。 上の式に出てくるB1,B2を並べた物がBLIST。これとθからボディヤコビアンを計算する関数が、JacobianBody(Blist,thetalist) 。

最初にかかげた目標の半分くらいはできた。

natsutan.hatenablog.com

Modern Robotics: Mechanics, Planning, and ControlのSpaceヤコビアン

Robotics

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

教科書はこの本です。

www.amazon.co.jp

今回はSpace Jacobian

Forward Kinematics おさらい

natsutan.hatenablog.com

FKをPoE形式で表現するとこのようになります。

 T = e^{[S_1]\theta_1}e^{[S_2]\theta_2} \cdots e^{[S_n]\theta_n}M

Space Jacobian

Twistの式

 [ \nu_{s} ] =\dot T T^{-1}

に上のTを代入すると

 [ \nu_{s} ] = [S_1]  \dot \theta_1 + e^{[S_1] \theta_1} [S_2] e^{-[S_1] \theta_1} \dot \theta_2 \cdots

この複雑な式が、adjoint mappingの意味からこうなります。 natsutan.hatenablog.com

  \nu_{s}  = S_1 \dot \theta_1 + Ad_{e^[S_1]\theta_1}(S_2)\dot \theta_2 \cdots 
  \nu_{s}  = J_{s1} + J_{s2}(\theta)\dot \theta_1 + \cdots + J_{sn}(\theta)\dot \theta_n

これを行列表現にしたときのヤコビアンがSpaceヤコビアンです。

  \nu_{s}  =  \begin{bmatrix} J_{s1} & J_{s2} & \cdots & J_{sn} \end{bmatrix}  \begin{bmatrix} \dot \theta_1 \\ \vdots \\ \dot \theta_n \end{bmatrix}  = J_s(\theta) \dot \theta

この後、教科書にいろいろ書いてあるのですが、結局の所こういうことだと思います。

  J_s(\theta)  =  \begin{bmatrix}  \omega_{s1} &  \omega_{s2 } & \cdots  & \omega_{sn} \\ - \omega_{s1} \times q_1 & - \omega_{s2} \times q_2 & \cdots & - \omega_{sn} \times q_n \end{bmatrix}

実際の例

教科書Example 5.3から

f:id:natsutan:20211219000652p:plain
Figure 5.8

RRPRRRのチェインです。

上の式にあてはめると、

  • q1がSの原点なのでq1=0、ωs1×q1も0
  • θ3は回転しないからω3は0, twistのリニアな速度はv3
  • q4, q5, q6の座標は同じqw

これらを合わせてこうなります。

  J_s(\theta)  =  \begin{bmatrix}  \omega_{s1} &  \omega_{s2 } & 0  & \omega_{s4 } & \omega_{s5} & \omega_{s6} \\ 0 & - \omega_{s2} \times q_2 & v_{s3} & - \omega_{s4} \times q_w  & - \omega_{s5} \times q_w  & - \omega_{s6} \times q_w \end{bmatrix}

FKを微分をせずに直接ヤコビアンが計算出来るのがメリットです。

Spaceヤコビアンを計算するために出てくるS1,S2が分かればSpaceヤコビアンは計算で出せるはず。 Spaceヤコビアンを計算するためのSのリストがSLIST、関数JacobianSpace(Slist, thetalist)にSとθを与えるとSpaceヤコビアンが返ってくる。

分かってきた。

Modern Robotics: Mechanics, Planning, and Controlのヤコビアン

Robotics

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

教科書はこの本です。

www.amazon.co.jp

今回はヤコビアン

Forward Kinematicsのヤコビアン

あるFKの式を考えます。xの座標の次元数をm、θの次元数（ジョイントの数）をnとするときの x(t)=f(\theta(t)), \theta \in \mathbb{R}^n を時間で微分します。

 \dot x = \frac {\partial f(\theta)}{ \partial \theta} \dot \theta = J(\theta)\dot \theta

この J(\theta) \in \mathbb{R}^{m \times n} をヤコビアンと呼びます。このヤコビアン行列は、ジョイントの速度 \dot \theta が変化したときにエンドエフェクタの座標の速度 \dot xがどれくらい変化するかを表しています。

実際の計算

教科書の図5.1から

f:id:natsutan:20211217233653p:plain
Figure 5.1

この場合xの座標は二次元、ジョイントの数（θの数）は二個なので、ヤコビアンは2x2の行列になります。

FKはこう。

x_1 = L_1 \cos \theta_1 + L_2 \cos(\theta_1 + \theta_2)
x_2 = L_1 \sin \theta_1 + L_2 \sin(\theta_1 + \theta_2)

時間で微分して行列にすると2x2のヤコビアン行列が出来ます。

 \begin{bmatrix} \dot x_1 \\ \dot x_2   \end{bmatrix} =  \begin{bmatrix} -L_1 \sin \theta_1 - L_2 \sin(\theta_1 + \theta_2) & -L_2 \sin(\theta_1 + \theta_2) \\ L_1 \cos \theta_1 + L_2 \cos (\theta_1 + \theta_2) & L_2 \cos (\theta_1 + \theta_2)  \end{bmatrix} \begin{bmatrix} \dot \theta_1 \\ \dot \theta_2   \end{bmatrix}

v_tipをエンドエフェクタの速度として、ヤコビアンJ(θ)を列に分解すると

v_{tip} = J_1(\theta)\dot \theta_1 + J_2(\theta)\dot \theta_2

このヤコビアンを使う事で、singularities、manipulability ellipsoid, force ellipsoidが分かるけど、今は興味ないので飛ばします。駄目なら戻ってくる。

ロドリゲスの回転公式

Robotics

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

教科書はこの本です。

www.amazon.co.jp

前回分からなかった所はここ。

 T_{04} = e^{[S_3]\theta_3}M

eの肩に行列がくる表現は、行列指数関数といってこの動画が分かりやすかったです。

www.youtube.com

肩に3x3の正方行列が乗っている時の

 e^{[S]\theta_3}

は、3x3の行列になります。肩の行列が4x4の場合は、4x4の行列になります。θはスカラーに注意してください。試験に出ます。

回転を行列指数関数で表す。

教科書p71の図3.11より。

f:id:natsutan:20211214234528p:plain
Figure 3.11

点p(0)が、点p(θ)まで、ωを軸にθ回転する時を考えます。

p(\theta) =  e^{[\hat{\omega}]\theta} p(0)

最初見た時はなんだこれはと思いましたが、行列指数関数だと思うとふむふむって思います。

行列指数関数が上手いことsinとcosになります。

Rot(\hat{\omega}, \theta) =  e^{[\hat{\omega}]\theta}  = I + \sin \theta [\hat{\omega}] + (1-\cos \theta)[\hat{\omega} ]^2

上手いことの計算は教科書をみてください。

これがロドリゲスの回転公式（Rodrigues' formula for rotations）です。

実際の計算例

教科書の図3.12から

f:id:natsutan:20211215224932p:plain
Figure 3.12

フレームbが、ω=(0, 0.866, 0.5)を軸にθ=30°回転した時を考えます。ωは単位ベクトルになっています。もし、単位ベクトルでなかったら単位ベクトルにしてから計算します。

R =  e^{[\hat{\omega_1}]\theta_1}  = I + \sin \theta_1 [\hat{\omega_1}] + (1-\cos \theta_1)[\hat{\omega_1} ]^2

sin θ1はsin(30deg)なので0.5, (1-cos θ1)は0.134になります。 [ω1]を行列にするにはこのルールでOK。

 [\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}  

R =  I + 0.5   \begin{bmatrix}  0 & 0.5 &  0.866 \\  0.5 & 0 & 0  \\  -0.866  & 0 & 0 \end{bmatrix} + 0.134   \begin{bmatrix}  0 & 0.5 &  0.866 \\  0.5 & 0 & 0  \\  -0.866  & 0 & 0 \end{bmatrix}^2 =  \begin{bmatrix}  0.866 & -0.250 &  0.433 \\  0.250 & 0.967 & 0.058  \\  -0.433  & 0.058 & 0.899 \end{bmatrix}

手計算は間違えそうなので、この辺からPythonで計算してます。

import numpy as np

I = np.identity(3)
omega = np.array([
  [0, -0.5, 0.866],
  [0.5, 0, 0],
  [-0.866, 0, 0]
])

R = I + np.sin(0.524) * omega + (1-np.cos(0.524)) * np.dot(omega, omega)
print(R)

上で書いた式を展開すると、このような行列になります。

f:id:natsutan:20211215231230p:plain

ちょっとTexで書く根性が無かったので教科書73ページの(3.52)から画像で持ってきました。

Modern Robotics: Mechanics, Planning, and ControlのForward Kinematics その2

Robotics

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

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

www.amazon.co.jp

前回の記事の続きでさらに他の表現を考えます。

前回の記事

natsutan.hatenablog.com

教科書p116の図1より。

f:id:natsutan:20211211235538p:plain
Figure 4.1

まずはzero position時の行列Mを求めます。求め方はここ。

natsutan.hatenablog.com

M =  \begin{bmatrix} 1 & 0 & 0 & L_1+L_2+L_3 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

教科書には、次にθ1、θ2が0の時のθ3が回転したときのscrewを求めると書いてありますが、実際はtwistを求めています。これも良く分かってないのですが、screwには、twistとwrenchの両方が含まれるようです。

twistの求め方はここ。 natsutan.hatenablog.com

S_3 =  \begin{bmatrix} \omega_3 \\ v_3  \end{bmatrix}  = \begin{bmatrix} 0 \\ 0 \\ 1 \\  0 \\ -(L_1+L_2) \\ 0  \end{bmatrix}

ωの方は右手系の正の方向に回転しているので(0,0,1)、vの方は

v_3 = - \omega_3 \times q_3

を使っています。

4x4の行列に変換すると

 [S_3] =  \begin{bmatrix} [\omega] & v \\ 0 & 0 \end{bmatrix}  = \begin{bmatrix} 0  & -1 & 0 & 0 \\ 1 & 0 & 0  &  -(L_1+L_2) \\ 0 & 0 & 0 & 0 \\  0 & 0 & 0 & 0   \end{bmatrix}

ここまでは分かります。 正直、次から良く分かってなくて、教科書そのまま書きます。[S3]を使う事とでθ1＝θ2＝0の時、以下の関係式がでます。

 T_{04} = e^{[S_3]\theta_3}M

θ2、θ1の順で同じ計算をすることで、以下の関係式を得ます。

 T_{04} = e^{[S_1]\theta_1}e^{[S_2]\theta_2}e^{[S_3]\theta_3}M

Exponentialの積の形になっているので、Product of Exponentialsの式（PoE）と呼びます。日本語の教科書だとこの方式を採用していなくて困っています。 PoEについては、次のセクションで詳しく説明しているので、そこで理解したいです。

Modern Robotics: Mechanics, Planning, and ControlのForward Kinematics その1

Robotics

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

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

www.amazon.co.jp

教科書p116の図1より。

f:id:natsutan:20211211235538p:plain
Figure 4.1

それぞれのθから手の先の(x,y)を計算するのが、Forward Kinematics です。

図から頑張って追えばこのような関係が導き出されます。

x=L_1\cos\theta_1+L_2\cos(\theta_1+\theta_2)+L_3 \cos(\theta_1+\theta_2+\theta_3)
y=L_1\sin\theta_1+L_2\sin(\theta_1+\theta_2)+L_3 \sin(\theta_1+\theta_2+\theta_3)
\phi=\theta_1+\theta_2+\theta_3

これをもっとシステマティックに求めたい。

フレームを変換する行列Tを使うと。

T_{04} = T_{01}T_{12}T_{23}T_{34}

それぞれの行列はこうなります。今までと同じく左上の3x3が座標軸の回転、右側の3つが並行移動です。

T_{01} =  \begin{bmatrix} \cos \theta_1 & -\sin \theta_1 & 0 & 0 \\ \sin \theta_1 & \cos \theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 
T_{12} =  \begin{bmatrix} \cos \theta_2 & -\sin \theta_2 & 0 & L_1 \\ \sin \theta_2 & \cos \theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 
T_{23} =  \begin{bmatrix} \cos \theta_3 & -\sin \theta_3 & 0 & L_2 \\ \sin \theta_2 & \cos \theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 
T_{34} =  \begin{bmatrix} 1 & -0 & 0 & L3 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

全部計算する根性が無いので、最初の2つだけ計算してみました。

T_{01}T_{12} =  \begin{bmatrix} \cos \theta_1 & -\sin \theta_1 & 0 & 0 \\ \sin \theta_1 & \cos \theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}   \begin{bmatrix} \cos \theta_2 & -\sin \theta_2 & 0 & L_1 \\ \sin \theta_2 & \cos \theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} =  
 \begin{bmatrix} \cos \theta_1 \cos \theta_2 - \sin \theta_1 \sin \theta_2 & -\cos \theta_1 \sin \theta_2 + \sin \theta_1 \cos \theta_2 & 0 & L_1 \cos \theta_1 \\ \sin \theta_1 \cos \theta_2 - \cos \theta_1 \sin \theta_2 & -\sin \theta_1 \sin \theta_2 + \cos \theta_1 \cos \theta_2  & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

受検で勉強したsinとcosの加算公式（sin(x+y) = sin(x)cos(y)+cos(x)sin(y)）を使うとこうなります。

T_{01}T_{12} =  \begin{bmatrix} \cos (\theta_1 + \theta_2) & -\sin (\theta_1 + \theta_2) & 0 & L_1 \cos \theta_1 \\ \sin (\theta_1 + \theta_2) & \cos (\theta_1 + \theta_2) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

上手く合成されてますね。