【Python】2リンクマニピュレータの逆運動学(収束計算・リアルタイム描画版)

Python言語を用いて、2リンクマニピュレータ（2自由度アーム）の逆運動学を収束計算で求め、リアルタイム描画してシミュレーションする方法をソースコード付きで解説します。

逆運動学の計算(収束計算・リアルタイム描画)

ロボットアームの構造が複雑になると数式で逆運動学が解けなくなります。
そのような場合は、収束計算を用いて解を求めます。
【収束計算の流れ】
①仮の解(初期関節角度)を設定する
②仮の解から順運動学を用いて手先位置を求める
③目標位置と手先位置の誤差から仮の解を微修正する
④位置誤差が十分小さくなるまで1～3を繰り返す

逆運動学の計算プログラム(数値計算)

Pythonを用いて収束計算で逆運動学を解き、姿勢をアニメーションで描画するプログラムを作成しました。

【プログラムの処理手順】
①2リンクアームのパラメータ（リンクの長さ、初期関節角度）を設定する
②手先の目標位置を設定する
③逆運動学を収束演算で計算し，目標位置を達成するための関節角度を導出する
④計算結果のアームの姿勢を逐次表示する

ソースコード

サンプルプログラムのソースコードは下記の通りです。

※ NumPy、Matplolibライブラリが必要です。
・【NumPy】インストール方法
・【Matplolib】インストール方法

実行結果

サンプルプログラムの実行結果は下記の通りです。