7: 2013-07-31 (水) 15:36:19 yoshida  |
現: 2013-08-07 (水) 20:15:33 yoshida |
| | *逆運動で動かす [#o66404ae] | | *逆運動で動かす [#o66404ae] |
| | | | |
| - | **三角関数を使用する [#zdb959c7] | + | **幾何学的に角度を求める [#zdb959c7] |
| | 逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~ | | 逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~ |
| - | 色々な計算手法がありますが、今回のロボットハンドでは三角関数をメインに角度を計算してみたいと思います。 | + | 色々な計算手法がありますが、本章では余弦定理をメインに角度を計算してみたいと思います。~ |
| | + | 以下のようにX、Y、Z軸の方向を決め、ハンド部分は角度計算に不要なので取り外して下さい。~ |
| | + | #ref(RobotHand3D.png) |
| | + | 指定座標をposと称し、各アクチュエータと軸間の長さを次のように呼称することにします。 |
| | + | #ref(id_rename.png) |
| | | | |
| | **2次元での角度算出 [#d90f6f7a] | | **2次元での角度算出 [#d90f6f7a] |
| - | 以降、各AX-12Aと軸の長さを次のように呼称することにします。~ | |
| - | #ref(id_rename.png) | |
| | では三角形を見てみましょう。~ | | では三角形を見てみましょう。~ |
| | 三角形を作るのはD2、D3、posとなっています。~ | | 三角形を作るのはD2、D3、posとなっています。~ |
| | 三辺の長さが決まったので余弦定理を使ってD3の角度を算出してみましょう。~ | | 三辺の長さが決まったので余弦定理を使ってD3の角度を算出してみましょう。~ |
| | #ref(cosineD3.png) | | #ref(cosineD3.png) |
| - | コサインの値が取得できたので、これをアークコサインで角度に直します。~ | + | コサインの値が取得できたらアークコサインで角度にすることができます。~ |
| - | ここでの角度はラジアンなので180を掛けてπ(円周率)で割れば度(デグリー)に変換できます。~ | + | ここでの角度はラジアンなので180を掛けてπ(円周率)で割れば度(デグリー)に変換することもできます。~ |
| | | | |
| | **3次元での角度計算 [#x04dbfff] | | **3次元での角度計算 [#x04dbfff] |
| - | X軸とY軸、Z軸を決めます。~ | |
| - | #ref(RobotHand3D.png) | |
| | 3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。~ | | 3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。~ |
| | #ref(cosineD2.png) | | #ref(cosineD2.png) |
| | 同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。D2への指令角度とは異なりますので、注意が必要です。 | | 同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。D2への指令角度とは異なりますので、注意が必要です。 |
| | #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] | |
| - | 本ロボットハンドの構成ではD4の角度を変更することはありません。何故なら傾きを再現するには軸が足りていないからです。~ | |
| - | 軸を追加すると角度を求める為に回転行列や平面、ベクトル計算等が必要となり本章の主旨とは異なってしまうため、ここでは触れません。~ | |
| - | 又、回転方向は軸を正から見て左回りになる方向がプラスとなります。 | |
| | | | |
| | **プログラミング [#bdefef6a] | | **プログラミング [#bdefef6a] |
| - | ではプログラムしてみましょう。~ | + | ではプログラムしてみましょう。 |
| | #html{{ | | #html{{ |
| | <pre class="brush:c;toolbar:false"> | | <pre class="brush:c;toolbar:false"> |
| | #include <math.h> | | #include <math.h> |
| | #include <fd.h> | | #include <fd.h> |
| - | double pos[3] = { 100.0, 150.0, 250.0 }; // 座標(x, y, z) | + | double pos[3] = { 70.0, 70.0, 130.0 }; // 座標(x, y, z) |
| - | const int L[7] = { 30, 38, 82, 44, 50 }; // 軸間の長さ | + | const int L[4] = { 42, 27, 83, 55 }; // 軸間の長さ |
| | + | |
| | + | // RadianからDegreeへ変換 |
| | + | double Degree (double rad) { |
| | + | return (rad / (atan(1) * 4)) * 180.0; |
| | + | } |
| | + | // RadianからDynamixelの位置へ変換 |
| | + | uint16_t Position (double rad) { |
| | + | return 512 + Degree(rad) * 3.41; |
| | + | } |
| | + | uint16_t Position2 (double rad) { |
| | + | return 512 - Degree(rad) * 3.41; |
| | + | } |
| | | | |
| | void main (void) { | | void main (void) { |
| | + | double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3; |
| | + | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); |
| | + | DX_ChangeBaudrate (1000000); |
| | + | fd_Wait (2000); |
| | // 必要な辺の長さを求めます | | // 必要な辺の長さを求めます |
| - | double D2D3 = L[2]; | + | D2D3 = L[2]; |
| - | double D3POS = L[3] + L[4]; | + | D3POS = L[3]; |
| - | double D2POS = sqrt (pow(pos[0], 2) + pow(pos[1], 2) + pow(pos[2], 2)); | + | D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1]))); |
| - | double D2k = sqrt (pow(pos[0],2) + pow(pos[1],2) + pow(L[0]+L[1],2)); | + | D2j = pos[2] - (L[0] + L[1]); |
| - | double kPOS = sqrt (pow(pos[3]-(L[0]+L[1]),2)); | + | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); |
| | // コサイン値を求めます | | // コサイン値を求めます |
| - | double cosD3 = (pow(D2D3,2) + pow(D3POS,2) - pow(D2POS,2)) / (2*D2D3*D3POS); | + | tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); |
| - | double cosD2 = (pow(D2D3,2) + pow(D2POS,2) - pow(D3POS,2)) / (2*D2D3*D2POS); | + | tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); |
| | + | cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); |
| | // 角度を出します | | // 角度を出します |
| - | Theta1 = atan2 (pos[2], pos[1]); | + | radD1 = atan2 (pos[1], pos[0]); |
| - | Theta2 = | + | radD2 = acos(tmpcos2) - acos(tmpcos1); |
| | + | radD3 = acos(-1) - acos (cosD3); |
| | + | // AX-12+の出力軸の位置指定 |
| | + | fd_DXSetPosition (1, Position(radD1)); |
| | + | fd_DXSetPosition (2, Position(radD2)); |
| | + | fd_DXSetPosition (3, Position2(radD3)); |
| | + | } |
| | </pre> | | </pre> |
| | }} | | }} |
| | + | |
| | + | 座標から角度を出すことができるようになったので、これをモーションへ組み込んでみましょう。 |
| | + | |
| | + | [[次のチャプターへ>FDIIICHAPTER10.5]] |
| | + | |
| | + | [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る |
ページへ戻る
