ロボットアーム研修(まえけん編) その7 ~座標系の設定~

こんにちは、まえけんです。
回転行列を使って、3軸、4軸のロボットアームの
姿勢を計算するソフトウェアを作っていみたいと思います。
今回は回転行列を作る前の下準備を行います。
(2020/08/31 Link2,Link4画像修正)

姿勢計算をする前の準備

以前の記事で出力角度を反転するということを行いました。
この時のようにリードライト時に計算値とサーボに書き込む値を
いちいち気にするのは軸数が増えてくるととても大変です。
そこで姿勢計算をする前にCRANE-X7クラスのリード関数を少し工夫して
軸のホームポジションと回転方向を好きな位置に変えられるようにします。

ソフトウェアの構成

Dynamixel Classはサーボからサーボホーンの位置情報(0~4096)をリードする。
Cranxe-X7 ClassはDynamixel Classでリードした位置情報を以下の式で角度に変換します。

joint_angle_[cnt] = 
　　　CX_JOINT_DIRECTION[cnt]*deg2rad(value2deg(joint_pos[cnt])-CX_JOINT_OFFSET[cnt]);

以上の式を使ってCRANE-X7の関節角度を計算する。
CX_JOINT_OFFSETとCX_JOINT_DIRECTIONを設定することで好きな姿勢をホームポジションに設定出来る。

ホームポジションの設定

今回は以下の姿勢をホームポジションとした。
手に持っているものにはX,Y,Zが書いてあり、棒が伸びている方向が正方向です。ホームポジション

各軸の回転方向は以下の写真の通りに取ります。
Z軸の右ねじ方向を正回転とします。
また、最初の軸はワールド座標と等しいものとして取りました。
ワールド座標兼Link0の座標系

Link1の座標系

Link2の座標系(2020/08/31画像修正)

Link3の座標系

Link4の座標系(2020/08/31画像修正)

Link5の座標系

Link6の座標系

エンドエフェクタは閉じてる状態を0度として、開く方向を正方向として設定しました。

ホームポジションに合わせたオフセットの設定

各軸の回転方向と軸のオフセットは以下のように設定します。

//JOINT FIX
static int	CX_JOINT_DIRECTION[CX_JOINT_NUM]= {1,-1,1,-1,1,-1,1,1};
static double	CX_JOINT_OFFSET[CX_JOINT_NUM]	= {0,-90.0,0,0,0,0,0,0};

このように設定することで、写真の姿勢をホームポジションとして扱えるようになりました。
以降は特に断りがなければこの姿勢をホームポジションとして扱います。

おわりに

このソフトウェアを実装することでDynamixelの配置を意識せずにプログラム出来るようになります。
次回は回転行列を用いた姿勢計算を実際に行います。

注意

私が設定したリンクナンバリングはプログラミングしやすいように公開されているリンクナンバリングと値が1ずれているので注意してください。