てけノート

on the foot of giants

[MATLAB] Robotics Toolbox の使い方:その2、関数など

   


前回に続いて、今回はよく使いそうな関数などの使い方を紹介します。

順運動学

各関節の値から、アームの先端位置の座標を求めたいとき。

htm = robot.fkine([q0 q1 q2]); 
x = htm(1,4);
y = htm(2,4);
z = htm(3,4);

これは簡単。
robot.fkineに各関節の角度を渡してやると、同次変換行列(Homogeneous transformation matrix)が返ってきます。
この中でx,y,zは4*4の4列目1-3行に当たるので、そこを取ってくれば先端位置の絶対座標が取れます。
自由度が増えても、渡す関節数が増えるだけで後は同じはず。

逆運動学

アームの先端位置の座標から、それを実現する関節の値を求めたいとき。複数解存在する。
これがちょっと厄介。

T = transl([x y z]); %x,y,zは目標の絶対座標
M = [1 1 1 0 0 0];
theta = robot.ikine(T, Ikine0, M, 'ilimit', 1000); %Ikine0は[th1 th2 th3]

まず、上述の同次変換行列Tをtranslで求めます。

Mはmask vectorと呼ばれる長さ1*6のベクトル。終端位置のrotationを考慮するときは後ろの3つも1にするが、到達位置だけが問題になる場合は最初の3つを1にして、後ろの3つは0でよい。
今回はrotationなしの3自由度なので後者。

Ikine0とは逆運動学の解を探索するときのスタートの値。今回は3DoFなので長さ1*3。
このikineという関数では、目標位置に到達する関節値のセットを初期値から探索して探すことになります。よってこの初期値は、出来るだけ目標の関節値に近いものを予め渡しておく必要があります(そうじゃないと探索アルゴリズムが局所解に落ち着いてしまって適切な解が返ってこない様子)
だから最初は順運動学を使ってテキトーに関節値を与えて、目標位置に近い関節値のセットを探しておくと○。
長さ計100のアームでやった感じ、距離が10以内にあるとほぼ間違いなく適切な解が返ってくる印象です。
ikinemというのもありますが、印象としては

ikinem「終端位置座標が多少ずれていても、初期関節値から考えて近いところを返す」
ikine「終端位置座標は合ってるが、初期値から全然違う」

という感じ。
ilimitは探索回数を設定するオプション。1000くらいが妥当っぽい。

逆動力学

アームの各関節において必要なトルクを求めたいとき。

q = [th1 th2 th3];
qd = [th1d th2d th3d];
qdd = [th1dd th2dd th3dd];
tau = robot.rne(q,qd,qdd);

各関節における角度と角速度、そして角加速度の3つを渡してやると返ってきます。

軌跡の処理

その1で紹介したplotや、今紹介した順運動学や逆動力学などは系列データ(=軌跡)を渡してあげると系列で返してくれます。
上のrneなら、
thはスカラーでなくて系列長N*1のベクトルにできます。
plotでは、
robot.plot([th1 th2 th3])でそれぞれ系列長がNなら、アームの動く描画ができます。N=200くらいだとすごく変化が小さいので、50くらいが見てて飽きない。

軌跡の録画

robot.plot([th1 th2 th3], 'movie', 'test')

testはディレクトリの名前。これでtestというディレクトリの中に画像が150枚とかできるので、testに移動して

ffmpeg -r 10 -i %04d.png out.avi

とかやってやれば動画になります。windowsはffmpegをダウンロードして、ディレクトリにffmpeg.exeを配置すればok。

まとめ

非常に簡単な3DoFのアームで使ってみるサンプルを紹介してみました。ヤコビアンとか難しいこと分からなくてもこれでこシミュレーションが走ります!すごい!
多自由度アームの動作の計算がmatlabでしたいときにはオススメです。
今回作ったモデルでは粘性や重力項は除外しましたが、それらもモデル作成で追加できます。また、collisionなども計算できるっぽいです。
情報のほとんどはrvctools内のrobot.pdfに記載されているので、詳しくはそちらをご覧下さい。

 - プログラミング, 研究