読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

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

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

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

5.マウスで関節を動かせるようにする

マウスでロボットの関節を操作できたら格好いいのですが、どうすればできるのでしょうか?なんと便利なことにopenFrameworksのexampleの中にpointPickerExampleがあり、参考にできます。場所は、C:\workspace\of_v0.8.0_vs_release\examples\3d\pointPickerExample

ですので、興味があれば試してみてください。

5.1 ロボットの関節を摘まむ

サンプルプログラムでは、スクリーン座標系で、マウスカーソルの位置と、世界座標系からスクリーン座標系へ変換されたメッシュの頂点位置との距離を調べることで、最近傍の頂点を選択していました。これを参考に、首や腰の関節をマウスで動かすルーチンを書いてみましょう。マウスの中ボタンを押したときに一番近い関節を操作するように設計します。

まず、マウスの中ボタンが押されたときのスクリーン座標系におけるマウス座標を調べます。マウスのボタンが押されると、mousePressed(int x, int y, int button)という関数が呼ばれます。引数にあるx,yがマウスカーソルの座標を表し、buttonがどのボタンを押したかを表します。

次に、スクリーン座標系でのロボットの関節の座標を次のように求めます。

ofMatrix4x4m=model.getMeshHelper(i).matrix*model.getModelMatrix();

ofVec3f cur = cam.worldToScreen(m.getTranslation());

スクリーン座標系での2点の座標が求まりましたから、距離を計算するのはsqrt((x1-x0)^2+(y1-y0)^2)なので簡単ですね。ここでは更に便利な関数distanceで直接求めています。すべてのノードの座標を調べて距離が最短のものを保存します。また、関節から20ピクセル以上離れたところでボタンが押されても感知しないようにします。その場合、-1が何も選択されていないことを表すというローカルルールを作って、nearestIndexに与えることにします。

f:id:cvl-robot:20131005000507j:plainfig.9 首を摘まんだ結果

 Index 7という答えが得られました。ところで7って何でしょうか。少し謎を残したまま先に進みます。

5.2 マウスのドラッグで関節角度を調整する

nearestIndexの値を他の関数からも見られるように、testApp.hで宣言するように変更します。また、関節の角度をパラメータで持てるようにchest, pan, tiltの3つの変数を導入しておきます。

int nearestIndex;

float chest, pan, tilt;

マウスのボタンを押しながら移動させることをドラッグと言います。そのような操作をしたときにopenFrameworksではmouseDragged(int x, int y, int button)という関数が呼ばれます。マウスの位置と画面の幅と高さの比から適当に回転量を決めるようにしてみましょう。画面の端から端まで移動したら360度動くことにします。

首を押したらIndex番号が7と表示されましたが、これはofxAssimpMeshHelperがメッシュを順番に番号付けしている数字です。ちょっと扱いづらいので、メッシュではなくノードを、ノードの名前で呼び出せるようにしたいところですが、ちょっと置いておきます。mesh番号を表示したり消したり、クリックしたりいろいろ調べていくと、0-3が左指、6が腰、7,8が首、9-14が右腕、15-20が左腕であることがわかりました。そこでnearestIndexが腰6、首7,8のときに角度の変更が反映されるようにします。

また、ドラッグの最中にカメラが動いてしまうと操作しづらいので、ofEasyCamのマウス操作を無効にしておきます。マウスのボタンを離したときに再び有効になるようにしています。

void testApp::update()関数の中で、chest, pan, tiltのパラメータで関節角度が更新されるようにします。

f:id:cvl-robot:20131006025844j:plainfig.10 ドラッグで関節角度調整

実行してみると、腰と首のチルトは動くようになりましたが、どんなに頑張ってもパン方向に動いてくれません。首の同じ位置に座標系が表示されていることからわかるように、パンとチルトは同じ位置に関節があることが原因のようです。

5.3 同じ位置に重なっている関節を摘めるようにする

同じ位置にある関節のindexを何個も保存できるように、vectorを使うようにnearestIndexの宣言を書き換えます。

 std::vector<int> nearestIndex;

同じ距離に関節が見つかった場合にnearestIndexに追加していきます。updateではfor文を回してすべてのindexに対して関節角度の更新が実行されるようにします。

f:id:cvl-robot:20131006033516j:plainfig.11 首のパン方向も動くようになったけど、、、

首のパン方向も操作できるようになりました。でも、ちょっと動かしにくいし、首が体にめり込んでしまうのが格好悪いですね。次回、もう少し便利に使うように直していくことにしましょう。