ロボットアーム（マニピュレータ）における逆運動学のアルゴリズム。
今回は以下の方法について試してみます。
- Forward + Shift
- FABRIK（Foward and Backward Reaching Inverse Kinematics）
- CCD（Cyclic Coordinate Descent）
環境：
- Python3.6
- Jupyter Notebook
運動学の場合、ロボットアームの各関節の角度を入力するとアーム先端（End-Effector）の座標値を求めることができます。
逆運動学の場合、End-Effector（アーム先端部）の座標値を入力すると各関節の角度が求められます。
上記の方法以外にヤコビ行列を用いた方法がありますが、それは次回へ。
Backward Shift：
この方法は、解析的に各関節の角度を求めるというより、図形的に回転と移動を繰り返すことで徐々に目標の座標に近似させていきます。Baseに近いほうからLink1、Link2、Link3という順番にしておき、Backwardの名前の通り、Link3から回転移動処理を始め、次にLink2、そしてLink1と操作を続けていきます。
Chart1：
アーム先端（End-Effector）の根元の関節Joint3から目標座標Target3に線を引いて角度θ3を得る。
Joint3を軸にLink3をθ3回転させる。
Chart2：
回転させたのち、End-EffectorとTarget3が一致するようにLink3をd3だけ平行移動する。
Chart3：
Link2とLink3の間に隙間ができるが、これでLink3の操作は一旦終了。
次はLink2の回転移動操作。目標座標はLink3下端（Target2）。
Chart4：
Joint2からTarget2へ向けて線を引き角度θ2を得る。
Joint2を軸にLink2をθ2回転させる。
Chart5：
回転させたのち、Link2上端とTarget2が一致するようにLink2をd2だけ平行移動する。
Chart6：
Link1とLink2の間に隙間ができるが、これでLink2の操作は一旦終了。
次のLink1についても同様の手順で動かす。目標座標はLink2の下端（Target1）。
Chart7：
Joint1からTarget1へ向けて線を引き角度θ1を得る。
Joint1を軸にLink1をθ1回転させる。
Chart8：
回転させたのち、Link1上端とTarget1が一致するようにLink1をd1だけ平行移動する。
Chart9：
Link1下端とBaseの間には隙間d0ができるので、Link1下端がBaseと一致するようにd0だけ全体的に平行移動する。
Chart10：
全体的に平行移動すると、End-Effectorと目標座標Target3との間に隙間ができる。
Joint3からTarget3へ線を引き、これ以降はChart1に戻って同じ処理を繰り返す。
最終的にはEnd-Effectorが目標座標Target3に限りなく近づく。
プログラム上では、この繰り返し処理に回数制限を設けるか、End-Effectorと目標座標との誤差が0.00001以下になったらループ処理を抜け出すかなどの設定にします。
コード：
- リンク（Arm）のclassを用意して、長さ、両端座標、角度などを定義しておきます。
- ならびに回転移動処理に必要なbackwardとshiftというメソッドも定義しておきます。
- 角度はnp.arctan2()で二点の座標から求めています。
- 変数Nを変えれば、Link数を増やすことができます。
- Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
- コード後半のmotion()関数で、ロボットアームがマウス座標を追従します（Jupyter上では動きはやや遅い）。
FABRIK：
この方法は、「Forward and Backward Reaching Kinematics」の略で、ForwardとBackwardの回転移動操作を繰り返して徐々に目標座標に近づいていきます。
前述したBackward ShiftのShiftさせた部分をForward（前方からの）の回転移動操作に置き換えたようなものです。
まず、End-Effectorのほうから目標座標に対して回転移動操作（Backward）し、Link1まで操作したら、今度はLink1、Link2、Link3という順番で回転移動操作（Forward）していきます。向きが変わるだけで基本的な手順は前述のChart1〜Chart8までの操作と同じです。
コード：
- Linkのclassを用意して、長さ、両端座標、角度などを定義しておきます。
- 回転移動処理に必要なbackwardとforwardのメソッドも定義しておきます。
- 角度はnp.arctan2()で二点の座標から求めています。
- 変数Nを変えれば、Link数を増やすことができます。
- Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
- コード後半のmotion()関数で、ロボットアームがマウス座標を追従します（Jupyter上では動きはやや遅い）。
CCD:
この方法は、２つのベクトル（回転軸から目標座標までのベクトル、回転軸からEnd-Effectorまでのベクトル）の角度の差分だけ回転させますが、Linkを回転させるというよりも、回転軸以降にあるJointの座標を回転移動させると言ったほうがいいでしょう。回転軸はJoint3、Joint2、Joint1という順番で移行し、それを繰り返します。
Chart1：
まずEnd-Effectorに近いJoint3を回転軸とする。
Joint3からTargetまでのベクトル、Joint3からEnd-Effectorまでのベクトル間の角度θ3を求める。
回転軸となるJoint3以降にあるJoint（この場合End-Effectorのみ）をθ3回転させる。
Chart2：
次の回転軸はJoint2。
Joint2からTargetまでのベクトルとJoint2からEnd-Effectorまでのベクトル間の角度θ2を求める。
Joint2以降のJoint（この場合、Joint3とEnd-Effector）をJoint2を中心にθ2回転させる。
Chart3：
次の回転軸はJoint1。
Joint1からTargetまでのベクトルとJoint1からEnd-Effectorまでのベクトルの角度θ1を求める。
Joint1以降のJoint（この場合、Joint2、Joint3、End-Effector）をJoint1を中心にθ1回転させる。
Chart4：
再度、回転軸はJoint3に戻り、Chart1同様の手順で回転移動させる。
あとはこの繰り返しで徐々にEnd-EffectorはTargetに近づいていく。
コード：
- 各Linkごとに動かすというよりも、回転軸となるJointを中心にそれ以降にあるJointの座標を回転移動させています。
- CCD()ファンクション内の二重のforループは、最初のforループが回転軸用、次のforループが回転移動させるJoint座標用になります。
- Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
３つの比較：
どれも比較的簡単なアルゴリズムなので軽快に動きますが、Forward & Backward系とCCDでは少し動きに違いが出てきます。
以下は、マウス（赤x印）の動きに追従するインタラクティブなプログラム。３つの方法を重ねて同時に動かしています。
FABRIKは全体的に張りのある動きをしています。Backwardのほうはやや柔らかめ。
CCDはやや反応が遅く、先端に近いほうの動きとBaseに近いほうの動きに差が出て蛇行することがあります。
リンク数を多くすると、その動き方の違いも顕著になってきます。
マウスの動かし方にもよりますが、CCDは２つに比べるとやや不自然な形になっています。
これらのアルゴリズムは、CGのキャラなどを動かすときに向いているようです。角度制限がないため実際のロボットアームに使うアルゴリズムには向いていないようです。
結果、BackwardとFABRIKが反応も早く、動きも自然という感じです。それぞれの回転移動方法やコードも比較的簡単。
3つ同時のコード：
- Backward Shift、 FABRIK、CCDの３つ方法をマウスに追従させることでインタラクティブに試すことができます。
- 変数Nを変えればリンク数を増やすことができます。
- Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
0 件のコメント:
コメントを投稿