5: 2013-07-29 (月) 13:54:26 yoshida | 6: 2013-07-29 (月) 19:56:41 yoshida | ||
---|---|---|---|
Line 22: | Line 22: | ||
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> | ||
}} | }} |