ぱたへね

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

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

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

教科書はこの本です。

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が分かるけど、今は興味ないので飛ばします。駄目なら戻ってくる。

ロドリゲスの回転公式

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

教科書はこの本です。

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

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

教科書はこの本です。今回は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

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

教科書はこの本です。今回は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} 

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

Modern Robotics: Mechanics, Planning, and ControlのWrench

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

教科書はこの本です。今回は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のモーメントの和になっています。

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

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

教科書はこの本です。今回は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の値に一致します。 手計算すれば分かりますが、回転のωに関しては座標軸の変換だけ、リニアな速度に関しては位置の変換と座標軸の変換が行われます。

Modern Robotics: Mechanics, Planning, and Control のTwist

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

教科書はこの本です。今回はTwist。 www.amazon.co.jp

Twist

教科書で最初に出てくるのは56ページ目。教科書によるとtwistは角速度と普通の速度の組み合わせを表しています。

Example 3.23からTwistを求めてみます。 車の回転の問題です。車の前輪が傾いているので、ある点を中心にグルグル回ります。回転の中心がr、回転の角速度がωとします。{s}から見たrの座標が(2,-1,0)、前輪がエンドエフェクタに相当してその座標系{b}からみたrの座標は(2, 1.4, 0)です。ωは2 rad/s とします。ここまでが与えられた値です。

f:id:natsutan:20211208205206p:plain
Figure 3.18

まず座標系{s}と{b}の関係を求めます。{s}からみた{b}の座標は(4,0.4,0)、{s}と{b}の座標軸を比べながら、前回の記事を見ながらまとめると、

natsutan.hatenablog.com

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

Twistは、(ωx, ωy, ωz, vx, vy, vz).Tです。教科書によっては並びが違ったり、ωがベクトル表示になって文字数が減ったりしますが、Twistは必ず1x6の行列になります。ωは2 rad/sと与えられているので、角速度ωを使ってリニアな速度vを求めます。vとωは回転の中心rを使って、この関係があります。

v =  \dot p - \omega \times p = \dot p + \omega \times (-p) = \dot p + p \times \omega  

ここでdotのついたpはpの時間微分なので、pが動かない場合は0でよいはず。つまり、ωとpの外積でvが求められます。

Νs

本当はν(ニュー)だけど、vと見分けがつかないので大文字で。 {s}座標系のr、raは(2,-1,0)です。ωは右手系なので(0, 0, 2)です。

v_s = (2, -1, 0) \times (0, 0, 2) = (-2, -4, 0)
\nu_s = (0, 0, 2, -2 ,-4, 0)^T

Nb

{b}座標系でのr、rbは(2, 1.4, 0)、ωは右手系なので(0, 0, -2)です。

 v_b = (2, 1.4, 0) \times (0, 0, -2) = (2.8, 4, 0)
 \nu_b = (0, 0, -2, 2.8 ,4, 0)^T

Twistの行列表現

Twistは1x6の行列ですが、回転行列っぽい表現が使われることもあります。Twist Nsと区別するために[Ns]と表現されます(本当はν)。 関係式がこの3つ。

 \nu_s =  \begin{bmatrix} \omega_s \\ v_s  \end{bmatrix} \in \mathbb{R}^6  

 [\nu_s]  =  \begin{bmatrix}  [\omega_s] & v_s \\ 0 & 0  \end{bmatrix}  

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

これを使うとNsはこうなります。右下が0な所が回転行列と違います。


 [\nu_s ]  =  \begin{bmatrix}  0 & -2 &  0 & -2 \\  2 & 0 & 0 & 4  \\  0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0  \end{bmatrix}