ロボットアーム研修(まえけん編)

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

ロボットアーム研修(まえけん編)

こんにちは、まえけんです。
回転行列を使って、3軸、4軸のロボットアーム
姿勢を計算するソフトウェアを作っていみたいと思います。
今回は回転行列を作る前の下準備を行います。

姿勢計算をする前の準備

以前の記事で出力角度を反転するということを行いました。
この時のようにリードライト時に計算値とサーボに書き込む値を
いちいち気にするのは軸数が増えてくるととても大変です。
そこで姿勢計算をする前に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]);
  • join_pos
    • Dynamixel Classでリードした各軸のサーボの位置情報
    • 値 0~4096
  • value2deg
    • 0~4096の値を-180~180度に変換する関数
      • join_pos * 360 / 4096 – 180
  • CX_JOINT_OFFSET
    • CRANE-X7の角度オフセット配列(詳細後述)
  • deg2rad
    • 角度をラジアンに変換する関数
  • CX_JOINT_DIRECTION
    • 回転方向を変更する配列(詳細後述)
    • 値 1 or -1
  • joint_angle
    • 各軸の角度を格納する配列

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

ホームポジションの設定

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

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

Link1の座標系Link1の座標系

Link2の座標系Link2の座標系

Link3の座標系Link3の座標系

Link4の座標系Link4の座標系

Link5の座標系Link5の座標系

Link6の座標系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ずれているので注意してください。

タイトルとURLをコピーしました