cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

ロボットのGUIコントローラを作る(その3)

初稿: 2013/10/4, 最終更新: 

3.ロボットの関節を連続的に動かす

前回は、ロボットの首を回転させてみましたが、一度だけ、決めた角度に動いただけでした。今回は、これを連続的に変化させられるようにしていきます。

3.1 各ノードの位置と姿勢を表示してみる

まず、関節の位置と姿勢を図示して確認してみましょう。

ロボットの関節はツリー構造で管理されていて、各関節の位置姿勢はノードの中のmTransformationという4×4行列に格納されています。この4×4行列は、あるノード自身から一つ上の親ノードへの位置姿勢の変位を表しています。座標系をロボットに重ねて表示したいので、各関節の座標系がロボットの原点からどのような位置姿勢にあるかがわかった方が扱いが簡単になります。

ロボットの原点を基準とした各関節の位置姿勢を求めたいときは、ルートからツリーをたどって順に位置姿勢行列mTransformationを掛け合わせていきます。

 M_ルート基準の求めたいノード=M_root×M_node1×M_node2×・・・×M_求めたいノード

プログラムでは、少し手抜きをして予め用意されている関数を使って求めます。次のソースコードをvoid testApp::draw()関数の中のmodel.drawFaces();と書かれた行の下にコピー&ペーストしてください。

draw()関数は描画処理を行っています。*1関節の座標を実際に計算しているのは、次の関数の中です。

ofxAssimpMeshHelper & meshHelper = model.getMeshHelper(i);

ofMultMatrix(meshHelper.matrix);

ofxAssimpMeshHelperというクラスは各ノードの幾何形状を表示するための補助ツールで、原点基準の位置姿勢行列を計算してくれます。i番目のメッシュの位置姿勢行列を求めて、openGLによる描画の行列に掛ています。*2

f:id:cvl-robot:20131004153922j:plainfig.6 関節の位置姿勢を表示

 ビルドしなおして実行すると、このようにロボットに重ねて軸が表示されています。RGBの色の順序で軸の方向を表しており、赤がx軸、緑がy軸、青がz軸です。一見正しいように思えますが、ロボットのマニュアルを持ってきて座標原点を確認すると、変なことに気がつきます。右手座標系で、ロボットの体柱の中心かつ台座部分の一番下の点が原点で、前面がx軸方向の+、ロボットの左手側がy軸方向の+、ロボット頭部側がz方向の+であるはずです。openGLも右手座標系なので、データ間の齟齬はないはずなので、きっと表示設定がおかしいのでしょうと思い当たります。

確認しておきましょう。頭のパン軸を回転させる代わりに、右肩をまわしてみます。

model.getAssimpScene()->mRootNode->FindNode("RARM_JOINT0_Link")->mTransformation *= aiMatrix4x4().

FromEulerAnglesXYZ(0,0,45*3.14/180.0);

 f:id:cvl-robot:20131004165633j:plainfig.7 右肩回転

予想通り、左のはずの肩が回ってしまっていますね。表示がおかしくて、左右が逆になってしまっているのだとわかりました。原因がわかってしまえば、怖いことはありませんので後で直すことにして進んでいきましょう。

3.2 初期姿勢の保存

ロボットの姿勢は、関節ノードのmTransformationに回転行列を掛けると変更できます。ただし、一度回転させた姿勢に更に何度も回転行列を掛けていくと変更に変更を加えていることになりますのでおかしなことが起こります。初期姿勢である0度の姿勢を何度回転させなさい、という命令にしたほうがスマートです。そのために、初期姿勢を保存しておきましょう。

 testApp.hにSTLのmapを使えるように、次の一行を加えてください。

#include <map>

また、class testAppの定義の中に初期姿勢を保存しておくためのコンテナを定義します。

 std::map<std::string, aiMatrix4x4> TransformationOrigin;

 

testApp.cppのsetup()関数の前に次の関数をコピーペーストしてください。ツリーを辿って、各ノードのmTransformationをTransformationOriginにコピーしていきます。mapを使っているので、探索キーをノードの名前にすることができます。

setup関数内のloadModel関数の呼び出し直後で初期姿勢を登録するようにします。

model.loadModel("kawada-hironx-parallelfingers.dae", false); searchRegistOrigin(model.getAssimpScene()->mRootNode, TransformationOrigin); searchPrintNode(model.getAssimpScene()->mRootNode);

3.3 関節の連続的な変化

テストしてみましょう。testApp::update()関数のmodel.update()の前に、次のソースコードをコピー&ペーストして実行してみてください。

static float pan = 0;

pan += 10.0f;

if(pan>180.0f) pan-=360.0f;

model.getAssimpScene()->mRootNode->FindNode("HEAD_JOINT0_Link")->mTransformation = TransformationOrigin["HEAD_JOINT0_Link"]*aiMatrix4x4().FromEulerAnglesXYZ(0,0,pan*3.14/180.0);

 首がぐるぐる回り始めました。シュールですね。このテストコードはもう使いませんので、試したら消しておいてください。

*1:ofPushMatrix()やofMultMatrix()のように関数の頭にofと付いていますが、名前がラッピングされているだけで中身はopenGLそのものです。一つ一つの関数で何をしているか興味を持ったら、openGLの解説を探してみてください。

*2:ノードの話をしていたはずなのにメッシュと呼んでいるのがおかしいな?と感じたら、とても勘が良いです。ofxAssimpMeshHelperにはその辺の誤解があるためにバグの原因となっていて、表示されたロボットの左手をよく見

ると指が表示されていません。これはモデルが壊れているのでも、Assimpによる読み込みが上手くいっていないのでもなく、ofxAssimpMeshHelperがノードとメッシュを混同して扱っているためです。後で修正します。