10: 2013-08-01 (木) 20:19:36 yoshida | 現: 2013-08-07 (水) 20:15:33 yoshida | ||
---|---|---|---|
Line 1: | Line 1: | ||
*逆運動で動かす [#o66404ae] | *逆運動で動かす [#o66404ae] | ||
- | **三角関数を使用する [#zdb959c7] | + | **幾何学的に角度を求める [#zdb959c7] |
逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~ | 逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~ | ||
- | 色々な計算手法がありますが、今回のロボットハンドでは三角関数をメインに角度を計算してみたいと思います。~ | + | 色々な計算手法がありますが、本章では余弦定理をメインに角度を計算してみたいと思います。~ |
- | 尚、ハンド部分は角度計算に不要なので取り外します。~ | + | 以下のようにX、Y、Z軸の方向を決め、ハンド部分は角度計算に不要なので取り外して下さい。~ |
#ref(RobotHand3D.png) | #ref(RobotHand3D.png) | ||
- | 又、各アクチュエータと軸間の長さを次のように呼称することにします。 | + | 指定座標をposと称し、各アクチュエータと軸間の長さを次のように呼称することにします。 |
#ref(id_rename.png) | #ref(id_rename.png) | ||
Line 27: | Line 27: | ||
#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] | ||
- | ではプログラムしてみましょう。~ | + | ではプログラムしてみましょう。 |
#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] = { 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へ変換 | ||
+ | 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); | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); | ||
DX_ChangeBaudrate (1000000); | DX_ChangeBaudrate (1000000); | ||
fd_Wait (2000); | fd_Wait (2000); | ||
// 必要な辺の長さを求めます | // 必要な辺の長さを求めます | ||
- | double D2D3 = L[2]; | + | D2D3 = L[2]; |
- | double D3POS = L[3]; | + | D3POS = L[3]; |
- | double 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]))); |
- | double D2j = pos[2] - (L[0] + L[1]); | + | D2j = pos[2] - (L[0] + L[1]); |
- | double jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); | + | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); |
// コサイン値を求めます | // コサイン値を求めます | ||
- | double tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); | + | tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); |
- | double tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); | + | tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); |
- | double cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); | + | cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); |
// 角度を出します | // 角度を出します | ||
radD1 = atan2 (pos[1], pos[0]); | radD1 = atan2 (pos[1], pos[0]); | ||
radD2 = acos(tmpcos2) - acos(tmpcos1); | radD2 = acos(tmpcos2) - acos(tmpcos1); | ||
radD3 = acos(-1) - acos (cosD3); | 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]]へ戻る |