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

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



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


2021年5月10日月曜日

IK(逆運動学):同次変換行列/ヤコビ行列(クロス積)/角度制限

以前、同次変換行列とヤコビ行列(クロス積)を用いて逆運動学を求めてみましたが、今回はそのコードに角度制限を追加してみました。

環境:Python 3.8.5、Jupyter Notebook


この逆運動学アルゴリズムの特長:

  • 同次変換行列を用いる
  • ヤコビ行列(クロス積)を用いる
  • 角度制限を設ける

上図(4リンクの場合):

  • 原点(0,0)がベース
  • 赤い×が目標座標
  • リンク1角度制限:-180〜180度
  • リンク2〜4角度制限:-90〜90度

角度制限のため各リンクは手前のリンクに対して90度以上回転しないようにしています。各ジョイントにおいて任意のminAngleとmaxAngleを設定することができます。仕組みとしては前回のFABRIKとCCDの角度制限と同じです。


追加した角度制限のコード(以下):

def angleLimit(TH, MinA, MaxA):
    for i in range(N):
        TH[i] = TH[i] % tau
        if TH[i] < pi:
            TH[i] -= tau
        if TH[i] < MinA[i]:
            TH[i] = MinA[i]
        if TH[i] > MaxA[i]:
            TH[i] = MaxA[i]
    return TH

引数において、THは全ジョイントの角度リスト、MinAは角度制限の最低角度リスト、MaxAは最高角度リスト。角度は-180〜180度で表現しておきます。


ヤコビ行列を用いた逆運動学アルゴリズムの場合、最後に各ジョイントにおける回転角を求めるため、その角度が制限角度以上(あるいは以下)になった場合に、最高角度(あるいは最低角度)に更新するだけなので、それほど大きな変更点はありません。


全体のコード:



関連:
実践 ロボット制御: 基礎から動力学まで
実践 ロボット制御: 基礎から動力学まで
Posted with Buyer
prime
オーム社
売上ランキング: 77028

0 件のコメント:

コメントを投稿