補助平面で逆運動学のターゲットを操作

 ロボットをマウスで直感的に操作するために、補助平面の導入をしました。その実行サンプルです。遅くなりましたが、ソースコードをGitHubにアップロードしました。

HIRONXGUIControler.hに3行足します。新しく定義したPlanePickerクラスのインスタンスと、マウスのスクリーン座標を保存しておくための変数です。

#include "PlanePicker.h"

ofVec3f p1, p2;

PlanePicker picker;

 HIRONXGUIControler.cppには、setup,update,mouseDraggedに修正を加えます。まずsetupで世界座標系からロボット座標系への変換行列の登録と、平面モデルの初期化を行います。ロボット自身が移動する場合、update内で変換行列を随時更新してください。

picker.setWorldToRobotMatrix(model.getModelMatrix().getInverse()*model.getMeshHelper(0).matrix.getInverse());

picker.setupModel();

 updateの中では、平面の定義を与えて、マウスカーソルに重なるスクリーンを前後につながるレイとの交点を求めています。

ofVec3f p0(0.5,0.0,0.3);

ofVec3f n(-0.3,0.1,0.4);

if(coords.get()){

 if(picker.update(picker.worldToRobot(p1),picker.worldToRobot(p2),p0,n.getNormalized())){

    xyz = picker.pf;

}

else {

  aiMatrix4x4 ai;

  ai.FromEulerAnglesXYZ(0,0,-chest.get()*M_PI/180.0);

  ofMatrix4x4  m(ai.a1,ai.a2,ai.a3,ai.a4,

    ai.b1,ai.b2,ai.b3,ai.b4,

    ai.c1,ai.c2,ai.c3,ai.c4,

    ai.d1,ai.d2,ai.d3,ai.d4);

  ofMatrix4x4 mt = m.getTransposedOf(m);

  if(picker.update(mt*picker.worldToRobot(p1),mt*picker.worldToRobot(p2),p0,n.getNormalized())){

    xyz = picker.pf;

  }

}

 drawで表示。

ofPushMatrix();

ofMultMatrix(model.getModelMatrix());

if(coords.get()) ofMultMatrix(model.getMeshHelper(0).matrix); // HIRONX

else ofMultMatrix(model.getMeshHelper(1).matrix); // chest

picker.draw();

ofPopMatrix();

mouseDraggedの中ではマウスカーソルの位置を保存します。

else if(button == 2 && use_picker.get() && coords.get()){

  cam.disableMouseInput();

  p1 = cam.screenToWorld(ofVec3f(x,y,-1));

  p2 = cam.screenToWorld(ofVec3f(x,y,1));

}

ビジョンで取得した情報を元にp0とnを指定してやれば、ビジョンベースで補助面と決めてロボットを少し知的に動かすことができるよになります。