ぱたへね

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

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ヤコビアンが返ってくる。

分かってきた。