ぱたへね

はてなダイアリーは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のモーメントの和になっています。

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

Modern Robotics: Mechanics, Planning, and Control のTwist

Robotics

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

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

Modern Robotics: Mechanics, Planning, and ControlのM

Robotics

Modern Robotics: Mechanics, Planning, and Control

この本が教科書です。

www.amazon.co.jp

教科書には行列Mが何度も出てきます。

Mは教科書に始めて登場するのは117ページ目で、ロボットがzero positionにいる時の手の先の場所と向きを表す行列です。

簡単な例

f:id:natsutan:20211206214307p:plain
簡単な例

上の図がZero positionだとすると、Mはこうなります。

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}

まず左上の3x3は座標軸の回転を表します。この図では回転していません。 画像処理で言うところの座標同士が歪まないので、一番下の段は(0, 0, 0, 1)で固定。numpy等でMを定義するとき右下の1を忘れがちなので注意。 一番右列が基準となる点からの(x,y,z)の座標。この組み合わせで上記のMになります。

難しい例

f:id:natsutan:20211206215531p:plain
Fig 4.3
教科書p121の図4．3から。

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

一番右の列は、基準となる座標系(\hat x_0,\hat y_0, \hat z_0)から見たエンドエフェクタの位置（座標）

(L_1, 0, -L_2)^T

を入れます。

ややこしいのは左上の3x3の部分です。 ここは基準となる座標系と、手の先の座標系のこの関係から持ってきます。

\begin{bmatrix}\hat  x_3  \\ \hat y_3  \\ \hat z_3   \end{bmatrix} =  \begin{bmatrix} 0 & 0 & 1  \\ 0 & 1 & 0  \\ -1 & 0 & 0   \end{bmatrix}\begin{bmatrix} \hat x_0 \\ \hat y_0  \\ \hat z_0   \end{bmatrix}

下の段は（0，0，0，1）なので、合わせると上の行列Mになります。

Modern Robotics: Mechanics, Planning, and Control

Robotics

CourseraのModern Robotics: Mechanics, Planning, and Controlを受講しています。

www.coursera.org

全部やると４週×６つの結構なボリュームがある講義です。 動画は5分くらいの短いのがいくつかあるだけで、基本的には指定された教科書を読んでQuizに挑戦していきます。ロボットシミュレータをイントールする課題がでましたが、今の所起動して視点を変えるくらいしか使っていないです。

教科書はこれです。

www.amazon.co.jp

最初のModern Robotics, Course 1: Foundations of Robot Motionは無事終了し、Modern Robotics, Course 2: Robot Kinematicsに入りました。

Quizを解くときは分かった気になるのですが、次の講義にいくとすっかり忘れてしまっていて、速度を落として勉強することにしました。今、詰まっている所は運動学と逆運動学で使うヤコビアンの所。

  • Mを確実に導出できるようにする。
  • Space Jacobianの導出
  • Slist とSpace Jacobianの違い
  • BlistとBody Jacobianの違い
  • Body Jacobianを確実に導出できるようにする。
  • Body Jacobianの意味を理解する
  • Space JacobianからBody Jacobianへ変換出来るようにする。
  • Body JacobianからSpace Jacobianに変換出来るようにする。
  • Space Jacobianを使ってFKを解く
  • Body Jacobianを使ってFKを解く
  • Space Jacobianを使ってIKを解く
  • Body Jacobianを使ってIKを解く

これを目標にしたい。

onnxのnamedtupledict

onnx python

Qumicoでいろいろやろうとしていたらエラーがでたので調べたメモ

namedtupledictはonnxの中で定義されている関数。

github.com

namedtupleに辞書形式でのアクセスを追加しています。

簡単な使い方

import collections

def namedtupledict(*a, **kw):
   namedtuple = collections.namedtuple(*a, **kw)
   def getitem(self, key):
      if type(key) == str:
          return getattr(self, key)
      return tuple.__getitem__(self, key)
   namedtuple.__getitem__ = getitem
   return namedtuple


x = namedtupledict('x', ('name', 'age', 'address'))

x1 = x('natu', 17, 'Japan')

print(x1.name)
print(x1[1])
print(x1['address'])

実行結果

natu
17
Japan

見開きのPDFが1ページずれているときの対処方法

購入した書籍がpdfで、見開きのページがずれていて上手く読めなかったときの対処方法です。

職場の同僚に勧められたので、翔泳社の紙1枚に書くだけでうまくいく プロジェクト進行の技術が身につく本を買いました。

www.shoeisha.co.jp

電子版がpdfで入手できて早速開いてみたら、見開きのページが全ページずれていてちょっと読めないレベル。ファイルに保護がかかっていて、1ページ削除したり、2ページ目からpdfに印刷などもできず、ちょっと困ってしまいました。

f:id:natsutan:20210702204631p:plain

翔泳社さんに問い合わせたところ、Acrobatであれば［見開きページ表示で表紙を表示］にチェックを入れると良いとのこと。

f:id:natsutan:20210702204759p:plain

やってみたら上手くいきました。なんでこんな状態で配布するんだろ・・・。