grbl1.1+Arduino CNCシールドV3.5+bCNCを使用中。
BluetoothモジュールおよびbCNCのPendant機能でスマホからもワイヤレス操作可能。
その他、電子工作・プログラミング、機械学習などもやっています。
MacとUbuntuを使用。

CNCマシン全般について:
国内レーザー加工機と中国製レーザー加工機の比較
中国製レーザーダイオードについて
CNCミリングマシンとCNCルーターマシンいろいろ
その他:
利用例や付加機能など:
CNCルーター関係:



*CNCマシンの制作記録は2016/04/10〜の投稿に書いてあります。


2021年6月3日木曜日

IK(逆運動学):複数の円の交点から求める

 以前、CCD、FABRIK、ヤコビ行列などを用いて逆運動学を求めましたが、今回は複数の円を用いて、それらの交点から2D逆運動学を求めてみます。

環境:Python 3.8.5、Jupyter Notebook


これまでの方法:



今回の特長:

  1. 2D逆運動学
  2. 行列式、ヤコビ行列は使わない
  3. 繰り返し計算で目標値に近似する
  4. ガイドとなる円弧に沿ってアーム全体が配置される
  5. アームが交差しない
  6. ジョイント角が均等になる(各リンク長が等しい場合)
  7. 各リンク長が異なっても計算可能
  8. N個のリンクに対応

上図(アーム先端がターゲットに到達した状態):

  • 座標(0,0)がベース、LV[4]がアーム先端、TV(赤い×)がターゲット。
  • 青破線の円は各リンクの可動域、各ジョイント位置が円の中心。
  • 赤破線の円は各リンクを円弧状に配置するためのガイド、CPが円の中心。
  • 複数のリンクは赤破線の円に沿って配置されるため均等な角度となる(各リンク長が等しい場合)。


上図(初期状態:未到達時):計算手順

  • ベース(0,0)からターゲットTV(赤い×)まで線を引く(|TV|>0)
  • ベクトルTVの中点から垂直方向にCPを配置する(暫定的な配置)
  • (0,0)とTVを通り、CPを中心とした円を用意する(赤破線の円)、半径CR=|CP|
  • 各リンクのジョイント座標を赤破線の円上に配置する(青破線の円と赤破線の円の交点利用)
  • (0,0)、CP、TVの3点からなる角度Aを求める(下図)
  • (0,0)、CP、アーム先端(LV[4])の3点からなる角度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()関数は2つの円の中心座標と半径を引数にして、2つの交点を返しますが、今回は一方の交点(上側の交点座標)を利用。



人気の投稿