以前、同次変換行列とヤコビ行列（クロス積）を用いて逆運動学を求めてみましたが、今回はそのコードに角度制限を追加してみました。
環境：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度で表現しておきます。
ヤコビ行列を用いた逆運動学アルゴリズムの場合、最後に各ジョイントにおける回転角を求めるため、その角度が制限角度以上（あるいは以下）になった場合に、最高角度（あるいは最低角度）に更新するだけなので、それほど大きな変更点はありません。
全体のコード：
0 件のコメント:
コメントを投稿