こんにちは。まえけんです。
今回は根本関節から任意の関節までの回転行列を以前より簡単に求められるようにしたいと思います。
現在の回転行列の求め方
以前の記事でn番目からn+1番目の関節の回転行列を定義しました。
定義した回転行列を根本から姿勢を求めたい関節まで順番にかけることで肘や手先の関節を求めていました。
R_elbow = rot_R01(rad[0]) * rot_R12(rad[1])*rot_R23(rad[2])*rot_R34(rad[3])*rot_X(M_PI/2.0);
しかし、このように毎回根本から順番に回転行列をかけて求めると
後に様々な関節の回転行列を求める時に面倒です。
そこで根本から任意の関節までの回転行列を求める関数を作ってみようと思います。
根本から任意の関節への回転行列を求める
回転行列関数を配列化
for文を使って根本から回転行列をかけたいので回転行列の関数を配列化します。
まずは関数ポインタを配列に格納します。
//回転行列のテーブル化
Eigen::Matrix3d (CR7::* rotation_table[7])(double) = {
&CR7::rot_R01,
&CR7::rot_R12,
&CR7::rot_R23,
&CR7::rot_R34,
&CR7::rot_R45,
&CR7::rot_R56,
&CR7::rot_R67,
};
次にこの回転行列配列を実行するための関数を定義します。
//テーブル化された関数行列の実行関数
Eigen::Matrix3d rotation(int num,double rad){
return (Eigen::Matrix3d)(this->*rotation_table[num])(rad);
};
これでrotation関数を実行することで任意のn番目からn+1番目への回転行列を実行することが出来ます。
引数は2つでそれぞれ以下の通りです。
- num:呼び出す回転行列を指定
- 例:0なら0番目から1番目のリンクへの回転行列を呼び出す
- rad:回転行列計算する際に代入する関節角度
配列化した回転行列関数をfor文で呼び出す
rotation関数を根本から任意の関節までかける関数を作ります。
/**
* @brief CRANE-7の根本からn番目までの回転行列を計算
*/
Eigen::Matrix3d CR7::calcRotation0toX(int num){
Eigen::Matrix3d rot_matrix;
rot_matrix = rotation(0,joint_angle_[0]);
for(int cnt=1; cnt<num+1; cnt++){
rot_matrix = rot_matrix *rotation(cnt,joint_angle_[cnt]);
}
return rot_matrix;
}
calcRotation0toXを実行することで根本から任意の関節までの回転行列を求められるようになりました。
関数内で使用されている変数、引数は以下の通りです。
- num:根本からnum番目までの回転行列を返す用に指定出来る引数
- rot_matrix:返される回転行列
- joint_angle_:CRANE-X7関数のメンバ変数。関節の角度が格納されている
実際の動作
実際の動作は以前の記事と同じですが求めるためのソフトウェアがかなりスッキリしました!
以前のソフトだとこのように求めていたのが…
R_elbow = rot_R01(rad[0]) * rot_R12(rad[1])*rot_R23(rad[2])*rot_R34(rad[3])*offset;
このような形になりました!
R_elbow = calcRotation0toX(4) * offset;
※動画は以前の記事と同じものです。
おわりに
過去のブログその8で行った回転行列を用いた姿勢計算を
関数ポインタを使って回転行列の関数を配列化することで
根本からX番目の関節までの回転行列をある程度簡単に求められるようになりました!
次回は手首、手先位置を求めてみたいと思います!