5: 2013-07-29 (月) 13:54:26 yoshida  |
6: 2013-07-29 (月) 19:56:41 yoshida |
| | X軸とY軸、Z軸を決めます。~ | | X軸とY軸、Z軸を決めます。~ |
| | #ref(RobotHand3D.png) | | #ref(RobotHand3D.png) |
| - | 3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。同様にD2の角度を求めます。 | + | 3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。~ |
| | #ref(cosineD2.png) | | #ref(cosineD2.png) |
| | + | 同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。 |
| | + | #ref(triangle3D.png) |
| | + | 指令値を出すにはもう一つ三角形を作らなければなりません。それが点kになります。posから垂直に下ろした線と |
| | + | |
| | 最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。 | | 最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。 |
| | | | |
| - | **ID4について [#b1298fca] | + | **条件について [#b1298fca] |
| | 本ロボットハンドの構成ではD4の角度を変更することはありません。何故なら傾きを再現するには軸が足りていないからです。~ | | 本ロボットハンドの構成では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 }; // 座標 | + | double pos[3] = { 100.0, 150.0, 250.0 }; // 座標(x, y, z) |
| | const int L[7] = { 30, 38, 82, 44, 50 }; // 軸間の長さ | | const int L[7] = { 30, 38, 82, 44, 50 }; // 軸間の長さ |
| | | | |
| | void main (void) { | | void main (void) { |
| | + | // 必要な辺の長さを求めます |
| | double D2D3 = L[2]; | | double D2D3 = L[2]; |
| | double D3POS = L[3] + L[4]; | | double D3POS = L[3] + L[4]; |
| - | double D2POS = sqrt (pow (pos[0], 2) + pow(pos[1], 2) + pow(pos[2], 2)); | + | double D2POS = sqrt (pow(pos[0], 2) + pow(pos[1], 2) + pow(pos[2], 2)); |
| - | double cosD3 = (pow (D2D3,2) + pow(D3 | + | double D2k = sqrt (pow(pos[0],2) + pow(pos[1],2) + pow(L[0]+L[1],2)); |
| | + | double kPOS = sqrt (pow(pos[3]-(L[0]+L[1]),2)); |
| | + | // コサイン値を求めます |
| | + | double cosD3 = (pow(D2D3,2) + pow(D3POS,2) - pow(D2POS,2)) / (2*D2D3*D3POS); |
| | + | double cosD2 = (pow(D2D3,2) + pow(D2POS,2) - pow(D3POS,2)) / (2*D2D3*D2POS); |
| | + | |
| | + | // 角度を出します |
| | + | Theta1 = atan2 (pos[2], pos[1]); |
| | + | Theta2 = |
| | | | |
| | </pre> | | </pre> |
| | }} | | }} |
ページへ戻る
