いろいろとあって、逆運動学するソフトがひつようになりました。
そこでフリーソフトのmaximaをチョイス！
いろいろをまず解説すると、
６軸マニの逆運動学を行う、、、には
ヤコビアンで数値計算がひつようぽい、、、ので
同次変換行列を計算して、、、
６軸分のヤコビアンを求める、、、となり
関節ごとの同次変換行列から、回転軸の単位ベクトルと関節位置を使いヤコビアンを求める、、、つまり
回転軸が分かって角速度を与えれば、後は腕のながさで次の関節の位置の速度と角速度が出ると、、、そういうわけで
腕の長さベクトルと回転軸周りの角速度ベクトルを外積しないといけません！
つまり同次変換行列から、列ベクトルを抜き出して外積してその結果から新たにヤコビアンの行列を生成します。
で、maximaには標準で外積の機能がりません←どうしてこうなってる？
調べるとべクトル用のパッケージがあってそれを使えばできるとかなんとか・・・
load(vect);u:[a,b,c];v:[x1,x2,x3];express(u~v);って書くとこうなる↓
[b*x3-c*x2,c*x1-a*x3,a*x2-b*x1]いいじゃんねー。
だがしかし、これは行ベクトルというより、単なるリストなのです。
ちなみに1行しかないmatrixで宣言すると
load(vect);u:matrix([a,b,c]);v:matrix([x1,x2,x3]);express(u~v);って書くとこうなる↓
~ used with improper arguments: matrix([a,b,c])matrix([x1,x2,x3])#0: express1(expn=matrix([a,b,c]) ~ matrix([x1,x2,x3]))(vect.mac line 166) -- an error. To debug this try: debugmode(true);くそだ、なんだこれ
そんなわけで、関数を定義します。
cros(a,x):=matrix(a[2]*x[3]-a[3]*x[2],a[3]*x[1]-a[1]*x[3],a[1]*x[2]-a[2]*x[1]);コレ、テストに出ル。
結果
あ、間違ってる、画像治すのめんどいけどプログラムはなおす。
u1v3-u3v2×
u2v3-u3v2○
はい、できました。
寝ましょう。
[1回]
PR
