« 今後の予定 | トップページ | スキーにいってきました »

C言語 逆運動学

前程条件
壱   ヒューマノイド、またはマニピュレータであり
     ヒザ、またはヒジが一自由度であること

弐    腰と足首、 または肩と手首が直行軸であること

参    位置のみを求めるものであり、姿勢は指定できない

四   double型で計算しないと、だいぶズレる

     サーボによってはハード的に誤差があるため、少しでもよい値を渡したい

     浮動小数点を扱えるSHマイコンでないと実装は難しいかも

伍    数値解析的に、オイラー法などを使っても良いが、

     値が変なところにとんでしまって僕にはむりです。

 

 

 

欲しい座標・・・ 位置3 姿勢3
持っている自由度 KHRなど、標準的なヒューマノイドだと5自由度

姿勢は犠牲にしなければいけないと感じますが、
足裏なんて、「常に地面に水平」として拘束してしまいましょう。

すると
欲しい座標・・・ 位置3
持っている自由度5

簡単に解けます。

 

 

以下 ソース
(動作未確認、理論はしっかり通ってます。)

#include<math.h>    //必須

#difine L1 60    //リンク間距離 膝
#difine L2 80    //リンク間距離 脛
#difine H 120    //腰高さ
#difine PAI  3.1415

double X;    //足先前後移動量 正方向で前へ
double Y;    //足先上下移動量 正方向で上げる
double Z;    //足先左右移動量

int gyaku(double X, double Y, double Z){

double theta1;
double theta2;
double theta3;
double theta4;
double L;    //腰と足先との距離

L    = sqrt( pow(H-Y,2.0) + pow(X,2.0) ) / H * sprt(pow(Z,2.0) + pow(H,2.0) )
theta1    = atan(X/(H-Y)) / PAI*180
theta2    = acos((pow(L1,2.0) + pow(L,2.0) - pow(L2,2.0)/2/L1/L)*180/PAI    //余弦定理

theta1    = theta1+theta2;    //変数の節約
theta3    = 180 - acos( pow(L1,2.0) + pow(L2,2.0) - pow(L,2.0)/2/L1/L2) )*180/PAI;    //余弦定理
theta2    = theta3 - theta1;    //変数の節約
theta4    = atan(Z/H) /PAI*180;

return theta1,theta2,theta3,theta4;
}

//おしまい

テキストにまとめたもの 「gyaku_undo.txt」をダウンロード

関数の使い方が、かなーーーり自信ないです。リターンはこんなに返せない。

ポインタ渡しが必須なはず。

ソースが間違ってる気がしてならない・・・
難しいなぁ.

|

« 今後の予定 | トップページ | スキーにいってきました »

コメント

この記事へのコメントは終了しました。

トラックバック

この記事のトラックバックURL:
http://app.f.cocolog-nifty.com/t/trackback/430008/28058773

この記事へのトラックバック一覧です: C言語 逆運動学:

« 今後の予定 | トップページ | スキーにいってきました »