以前、CCD、FABRIK、ヤコビ行列などを用いて逆運動学を求めましたが、今回は複数の円を用いて、それらの交点から2D逆運動学を求めてみます。
環境：Python 3.8.5、Jupyter Notebook
これまでの方法：
- Inverse Kinematics 逆運動学：Backward Shift、FABRIK、CCD
- Jacobian Inverse Kinematics :ヤコビ行列を用いた逆運動学（その１）
- IK（逆運動学）：同次変換行列、クロス積によるヤコビ行列（その２）
- IK（逆運動学）：アーム可動域制限（角度制限）CCDとFABRIKの場合
- IK（逆運動学）：同次変換行列/ヤコビ行列(クロス積)/角度制限
- IK（逆運動学）3Dアーム/同次変換行列/ヤコビ行列/角度制限
今回の特長：
- 2D逆運動学
- 行列式、ヤコビ行列は使わない
- 繰り返し計算で目標値に近似する
- ガイドとなる円弧に沿ってアーム全体が配置される
- アームが交差しない
- ジョイント角が均等になる（各リンク長が等しい場合）
- 各リンク長が異なっても計算可能
- N個のリンクに対応
上図（アーム先端がターゲットに到達した状態）：
- 座標(0,0)がベース、LV[4]がアーム先端、TV（赤い×）がターゲット。
- 青破線の円は各リンクの可動域、各ジョイント位置が円の中心。
- 赤破線の円は各リンクを円弧状に配置するためのガイド、CPが円の中心。
- 複数のリンクは赤破線の円に沿って配置されるため均等な角度となる（各リンク長が等しい場合）。
上図（初期状態：未到達時）：計算手順
- ベース（0,0）からターゲットTV（赤い×）まで線を引く（|TV|>0）
- ベクトルTVの中点から垂直方向にCPを配置する（暫定的な配置）
- (0,0)とTVを通り、CPを中心とした円を用意する（赤破線の円）、半径CR=|CP|
- 各リンクのジョイント座標を赤破線の円上に配置する（青破線の円と赤破線の円の交点利用）
- (0,0)、CP、TVの３点からなる角度Aを求める（下図）
- (0,0)、CP、アーム先端（LV[4]）の３点からなる角度Bを求める（下図）
- 角度A+角度B=360度になるときアーム先端（LV[4]）とTVが一致する
- 角度A+角度Bが360度になるように、その差分Errorに応じてCPの座標を調節する計算式を用意する
Error = Thetas - tau scaler += Error * 0.1 CP = np.array([-TV[1], TV[0]]) * scaler + TV/2
上記がCPの座標を調節する計算式。
Thetasは角度Aと角度Bの合計角、tauは2π=360度（目標値）、Errorはその差分。
CPの座標が変わることで赤破線の円の半径が変わる。常にCPはTV中点からの垂線上にある。
- scaler=0のとき、TV/2がCPとなり、(0,0)とターゲット（TV）を直径とする円になる
- scaler>0のとき、CPの位置はTV/2より上側になる
- scaler<0のとき、CPの位置はTV/2より下側になる
つまり、目標値360度と角度A+角度Bの合計角の差分によってscaler値を増減させ、繰り返し計算によってアーム先端をターゲットに近似させていきます。
上図：
ターゲット（TV：赤い×）がベース(0,0)に近い場合でもアームが交差しない。ただし、|TV|>0の場合。
上図：
ターゲット（TV：赤い×）が遠く到達不可能な場合、ターゲット（TV）に向けてアームが伸びる。この場合、1000ループで計算終了。
上図：
リンク数10の場合。変数Nにリンク数を代入することで任意のリンク数に対応。
上図：
円の交点と角度の計算だけなので、各リンクが異なる長さの場合も計算可能。
コード：
- Nはリンク数。TVはターゲットベクトル。LRは各リンク長（デフォルト：1.0）。
- While文で繰り返し計算を行い、アーム先端（LV[4]）とターゲットTVとの差分（Error）が0.0001以下、あるいは1000ループを超えるとループ解除。
- intersects()関数は２つの円の中心座標と半径を引数にして、２つの交点を返しますが、今回は一方の交点（上側の交点座標）を利用。
0 件のコメント:
コメントを投稿