11: 2013-08-02 (金) 13:14:30 yoshida  |
現: 2013-08-07 (水) 20:15:33 yoshida |
| | *逆運動で動かす [#o66404ae] | | *逆運動で動かす [#o66404ae] |
| | | | |
| - | **三角関数を使用する [#zdb959c7] | + | **幾何学的に角度を求める [#zdb959c7] |
| | 逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~ | | 逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~ |
| - | 色々な計算手法がありますが、今回のロボットハンドでは三角関数をメインに角度を計算してみたいと思います。~ | + | 色々な計算手法がありますが、本章では余弦定理をメインに角度を計算してみたいと思います。~ |
| - | 尚、ハンド部分は角度計算に不要なので取り外します。~ | + | 以下のようにX、Y、Z軸の方向を決め、ハンド部分は角度計算に不要なので取り外して下さい。~ |
| | #ref(RobotHand3D.png) | | #ref(RobotHand3D.png) |
| - | 又、各アクチュエータと軸間の長さを次のように呼称することにします。 | + | 指定座標をposと称し、各アクチュエータと軸間の長さを次のように呼称することにします。 |
| | #ref(id_rename.png) | | #ref(id_rename.png) |
| | | | |
| | #ref(triangle3D.png) | | #ref(triangle3D.png) |
| | 指令値を出すため、もう一つ三角形を作ります。D2からZ軸に沿ってposの位置まで移動した点をjとし、D2、j、posを頂点とした直角三角形を作成します。D2の指令角度はこの三角形のD2の角度から先ほど求めたD2の角度を差し引きした角度になります。~ | | 指令値を出すため、もう一つ三角形を作ります。D2からZ軸に沿ってposの位置まで移動した点をjとし、D2、j、posを頂点とした直角三角形を作成します。D2の指令角度はこの三角形のD2の角度から先ほど求めたD2の角度を差し引きした角度になります。~ |
| | + | 但し、D2とD3のAX-12+は逆向きに結合しているので、位置を指定する際には注意が必要です。~ |
| | | | |
| | 最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。 | | 最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。 |
| - | | |
| - | **回転方向について [#b1298fca] | |
| - | 回転方向は軸を正から見て左回りになる方向をプラスとします。 | |
| - | #ref(roll_pitch_yaw.png) | |
| | | | |
| | **プログラミング [#bdefef6a] | | **プログラミング [#bdefef6a] |
| | #include <math.h> | | #include <math.h> |
| | #include <fd.h> | | #include <fd.h> |
| - | double pos[3] = { 80.0, 0, 50.0 }; // 座標(x, y, z) | + | double pos[3] = { 70.0, 70.0, 130.0 }; // 座標(x, y, z) |
| - | const int L[4] = { 42, 27, 68, 40 }; // 軸間の長さ | + | const int L[4] = { 42, 27, 83, 55 }; // 軸間の長さ |
| | | | |
| | // RadianからDegreeへ変換 | | // RadianからDegreeへ変換 |
| | uint16_t Position (double rad) { | | uint16_t Position (double rad) { |
| | return 512 + Degree(rad) * 3.41; | | return 512 + Degree(rad) * 3.41; |
| | + | } |
| | + | uint16_t Position2 (double rad) { |
| | + | return 512 - Degree(rad) * 3.41; |
| | } | | } |
| | | | |
| | D2D3 = L[2]; | | D2D3 = L[2]; |
| | D3POS = L[3]; | | D3POS = L[3]; |
| - | D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]); | + | D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1]))); |
| | D2j = pos[2] - (L[0] + L[1]); | | D2j = pos[2] - (L[0] + L[1]); |
| | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); | | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); |
| | fd_DXSetPosition (1, Position(radD1)); | | fd_DXSetPosition (1, Position(radD1)); |
| | fd_DXSetPosition (2, Position(radD2)); | | fd_DXSetPosition (2, Position(radD2)); |
| - | fd_DXSetPosition (3, Position(radD3)); | + | fd_DXSetPosition (3, Position2(radD3)); |
| | } | | } |
| | </pre> | | </pre> |
| | }} | | }} |
| - | 座標から角度を算出してアーム部分を動かすことができるようになりましたが、稼働部分、及び軸間の短さから指定できる座標は非常に狭い範囲になります。~ | |
| - | 範囲外の座標が指示された場合は途中で計算が破綻をきたしますので、acosの前に値のチェックを入れることで回避することができます。~ | |
| - | 又、アームを構成しているパーツによっては、指示されても移動できない出力軸の位置ができてしまいます。この場合はAX-12+への位置指定の前にチェックを入れて位置を指定しないようにしましょう。位置を指定してしまった場合にはFDIII-HCのスイッチをOFFして下さい。過度の負荷は故障の原因となります。~ | |
| | | | |
| | + | 座標から角度を出すことができるようになったので、これをモーションへ組み込んでみましょう。 |
| | | | |
| | [[次のチャプターへ>FDIIICHAPTER10.5]] | | [[次のチャプターへ>FDIIICHAPTER10.5]] |
| | | | |
| | [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る | | [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る |
ページへ戻る
