こんにちは、まえけんです。
今週は手先の姿勢を制御してみたいと思います。
手先の姿勢制御
姿勢のフィードバッグ
手先の姿勢を真下(例：ピッチ90度、ロール0度)に向ける条件であれば、ある程度簡単に手先3軸を使って姿勢制御が出来ます。
ヨーのみ姿勢を170°>=ヨー姿勢目標>=-170°の範囲で指定が可能です。(姿勢によって追従出来ないことはあります。)
以前の記事で肘までの姿勢導出を行いました。
肘のロール・ピッチ・ヨーの姿勢それぞれ手先のモータにフィードバックすると手先の姿勢が真下に保てます。
5番目の関節には肘のロール姿勢をフィードバックします。
6番目の関節には肘のピッチ姿勢をフィードバックします
7番目の関節には肘のヨー姿勢をフィードバックします。
実装
実際に実装してみます。
target_angle_のインデックスは先程上げた画像の関節番号と1ずれています。(プログラム上で関節が0番始まりのため)
ピッチ軸に関しては肘が上を向く(ロール軸の値が90度または-90度を超える)と値が180度変わってしまうので条件分岐をしています。
ロール軸・ヨー軸に関してもこのようなポイントがありますが
今回はその範囲でアームを動かさないので設定していません。
while(1){
wait_s(0.01);
cr.updateElbowPosture();
//手先姿勢制御部分
double end_pitch_target = M_PI/2.0;
double end_yow_target = 0;
double end_roll_target = 0;
cr.target_angle_[4] = +(end_roll_target -cr.elbow_posture_[0]);
if(cr.elbow_posture_[0] < M_PI/2.0 && cr.elbow_posture_[0] > -M_PI/2.0){
cr.target_angle_[5] = +(end_pitch_target -cr.elbow_posture_[1]);
}else{
cr.target_angle_[5] = +(end_pitch_target +( M_PI - cr.elbow_posture_[1]));
}
cr.target_angle_[6] = -(end_yow_target -cr.elbow_posture_[2]);
cr.setJointAngle();
}変数、関数の概要
- end_pitch_target
- 手先のピッチ軸目標角度
- end_yow_target
- 手先のヨー軸目標角度
- end_roll_target
- 手先のロール軸目標角度
- elbow_posture_
- 0,1,2の順で肘先のロール・ピッチ・ヨーの姿勢が格納されている
- updateElbowPosture関数で値が更新
- setJointAngle
- target_angle_に格納されて角度でCRANE-X7に角度指令をだす
- wait_s
- 引数*1秒waitします
可動範囲外を指定するとハードウェアの故障の原因になるので
各関節の角度リミットはしっかり設定しておくことが大切です。
動かしてみる
手先だけトルクON
手先の姿勢制御が想定通り働いているか
手首サーボのみ、トルクをONにして試してみます。
無事動作しました！
この動きがとてもおもしろくてついつい遊んでしまいます。
軌道追従と組み合わせる
単体で動作することが確認できたので
次に軌道追従のソフトと組み合わせて動作をさせてみました。
これもしっかり動きました！
CRANE-X7の7軸のうちようやく6軸までは位置、姿勢の制御が出来るようになってきました！
次回
次回はアームの根本から任意の関節までの回転行列をもう少し簡単に求められるように
プログラムを工夫していきたいと思います。
今はまだ肘関節のみの姿勢しか使っていませんが、今後回転行列はたくさん使うことになるので
早いうちにこのあたりを効率化していきます。
