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

ロボットアーム研修(まえけん編) その13 ~同次変換行列を用いた手首、手先座標の計算 中編~

ロボットアーム研修(まえけん編) その13 ~同次変換行列を用いた手首、手先座標の計算 中編~ CRANE-X7

こんにちは。まえけんです。
前回の記事では手先座標を計算するために
回転行列から平行移動の無い同時変換行列を作る関数を用意しました。
今回は平行移動のある同次変換行列を作っていきます。

手首座標の計算

並行移動のある同次変換行列

並行移動のある同次変換行列を作っていきます。
並行移動が必要なのは以前の記事のリンク番号の付け方だと2linkから3link(根本から肘)への同次変換行列と3linkから4link(肘から手首)への同次変換行列の2箇所です。
エンドエフェクタは最後にオフセットとしてかけるので同次変換行列を作る必要がありません。

2linkから3linkへの同次変換行列を考える上でそれぞれのリンクの向きと距離を整理しておきます。
2linkと3linkの関係2linkと3linkの関係
Y軸方向に-90度回転した後、X軸方向に0.25[m]並行移動したあと、肘のサーボ角度の分回転させたものが2linkから見た3linkの位置と姿勢になります。
それを踏まえて2-3link間の同次変換行列を作っていきます。

Eigen::MatrixXd T23(4, 4);
(中略)
Eigen::MatrixXd x_trans_23 = init_3dto4dmat(rot_X(0));
x_trans_23(0,3) = 0.25;
T23 = init_3dto4dmat(rot_Y(-M_PI/2.0)) * x_trans_23 * init_3dto4dmat(rot_Z(joint_angle_[3]));
T23(0,0) = T23(0,1) = T23(0,3) = T23(2,2) = 0;
  • 変数
    • T23:link2からlink3の同時変換行列を格納する4×4の行列
    • x_trans_23:link2からlink3のx軸方向の並進移動を入れる行列
    • joint_angle:CRANE-X7の各関節角度を格納している配列
  • 関数

手首座標の計算

手首座標計算のためにオフセット用のベクトルを作成します。

手首座標の計算

肘から手首まではx軸方向に0.25[m]並行移動しています。
それを表すために次のような4×1の行列を用意します。
1行目はX軸のオフセット値
2行目はY軸のオフセット値
3行目はZ軸のオフセット値
4行目は固定で1とします。

Eigen::MatrixXd offset(4, 1);
offset << 0.25, 0, 0, 1;

前回の記事で用意した同次変換行列と今回作った同次変換行列、オフセットベクトルを使って手首座標を計算してみます。根本から順番に同時変換行列をかけていくと手首座標が計算出来ます。

wrist_point = ( TW0 * T01 * T12 * T23 * offset);

TW0:ワールド座標からlink0への同次変換行列(link0の回転)
T01:link0からlink1への同次変換行列
T12:link1からlink2への同時変換行列
T23:link2からlink3への同時変換行列
offset:肘から手先への距離オフセット

wrist_pointは4×1の行列で以下のような内容になっています。
1行目は手首のX座標
2行目は手首のY座標
3行目は手首のZ座標
4行目は1で固定です。

これで手首座標の計算が出来ました。

実際に動作させてみる

手首までの4つの関節角度を考慮して手首座標を計算しました。
しっかり計算出来ています!

次回へ

次回は手先の位置を計算してみたいと思います。
同時変換行列のシリーズも次で最後です。

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