PythonによるOpen Dynamics Engine講座 その11
ここでは、3自由度マニピュレータを逆運動学で制御します。サンプルプログラムはここ。
図1：3自由度マニピュレータ
図1の3自由度マニピュレータの場合、手先の座標x、yと手先の姿勢Θを指定して、各関節の角度を求めます。順運動学によって以下の式でx、y、Θが求まります。
そこでこれを逆に解きます。リンクを根元からリンク1、リンク2、リンク3とすると、リンク2の先端座標x'、y'は以下の式で導出できます。
図2：リンク2までのマニピュレータ
これから、β、α、φは以下の式で導出できます。
βは余弦定理により導出できます。
よって、関節角度は以下の式で導出できます。
inverseKinematics関数でやってることは、上記の式そのまんまです。引数は目標のx座標、y座標、先端リンクの姿勢、第1リンクの長さ、第2リンクの長さ、第3リンクの長さの順です。acos()、atan2()等を使うためにはmathをimportする必要があります。
あと、視点を変えてます。gluLookAt (0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 0, 1, 0)とz軸方向5.0から0の地点を見るようにし、y軸正方向が上側に来るようにしています。z軸方向負の方向から見ない限り、x軸が逆になってますが、誰かなんとかしてください。丸投げします。
あとは、_idlefunc関数で
dtheta = inverseKinematics(1.0, 1.0, PI / 2, self.bodies[1].lx, self.bodies[2].lx, self.bodies[3].lx)
として、先端リンクの目標座標を(1.0,1.0)、姿勢を90度としてdthetaに目標関節角度を格納している。
self.joints[1].theta = dtheta[0]
self.joints[2].theta = dtheta[1]
self.joints[3].theta = dtheta[2]
として、目標角度を設定している。
次回は円柱、カプセルの作成をやります。
PythonによるOpen Dynamics Engine講座に戻る