ODE講座16: 圧力センサを作ろう！

haltiさんからの御質問にあったように関節にかかるトルクや力を調べるコマンドはわかりづらくなっています．

 ODEでは関節にかかるトルクなどをすぐ取得できません．取得するためにはdJointSetFeedbackで関節を指定してからdJointGetFeedbackで情報を取得します．これはパフォーマンスを向上させるためです．常に全ての関節にかかる力とトルクが必要なわけではありませんよね．

• void dJointSetFeedback (dJointID, dJointFeedback *);
  力とトルクの情報を取得する関節をdJointIDで指定する．dJointFeedback構造体は次のとおりです．
  typedef struct dJointFeedback { dVector3 f1; // 関節がボディ1に及ぼしている力 dVector3 t1; // 関節がボディ1に及ぼしているトルク dVector3 f2; // 関節がボディ2に及ぼしている力 dVector3 t2; // 関節がボディ2に及ぼしているトルク} dJointFeedback;

• dJointFeedback *dJointGetFeedback (dJointID);
  dJointIDで指定している関節の力とトルクの情報を取得します． 

次に，このAPIを使ったサンプルプログラムを紹介します．２つのボックスを固定ジョイント(Fixed Joint)でくっつけ，そこにかかる力を表示するプログラムです．ボックスの重さが各1kgなので，z軸方向（上方向）には9.8Nの力がかかっていれば理論どおりです．私の環境では理論どおり9.8の値をたたき出していました．

なお，プログラムでは２つのボックスのうち，下を圧力センサとみなしています．

// sensor.cpp  by Kosei Demura  2006-7-26
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

static dWorldID world;
static dSpaceID space; 
static dGeomID  ground;
static dJointID fixed;
static dJointGroupID contactgroup;
dJointFeedback *feedback = new dJointFeedback;
dsFunctions fn;

typedef struct {   
  dBodyID body;    
  dGeomID geom;    
  dReal   radius,length,width,height,mass;
} myLink;
myLink box,sensor;     

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  static int MAX_CONTACTS = 10;
  int i;

  // 2つのボディがジョイントで結合されていたら衝突検出しない
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnected (b1,b2)) return;

  dContact contact[MAX_CONTACTS];   //
  int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,
                       sizeof(dContact));
  if (numc > 0) {
    for (i=0; i<numc; i++) {
      contact[i].surface.mode  =  dContactSoftCFM | dContactSoftERP;
      contact[i].surface.mu       = dInfinity;　　// 摩擦係数
      contact[i].surface.soft_cfm = 1e-8;
      contact[i].surface.soft_erp = 1.0;
      dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
      dJointAttach (c,dGeomGetBody(contact[i].geom.g1),
                      dGeomGetBody(contact[i].geom.g2));
    }
  }
}

static void simLoop (int pause)
{
  static int steps = 0;

  dSpaceCollide(space,0,&nearCallback);
  dWorldStep(world,0.01);
  dJointGroupEmpty(contactgroup);

  feedback = dJointGetFeedback(fixed);　// 力とトルク情報の取得
  printf(“%5d Force fx=%6.2f “,steps++,feedback->f1[0]);　// x座標成分
  printf(“fy=%6.2f “,feedback->f1[1]);　　　// y座標成分
  printf(“fz=%6.2f \n”,feedback->f1[2]);　// z座標成分

  // ボックスの描画
  dsSetColor(1.0,0.0,0.0);
  dReal sides1[] = {box.length,box.width,box.height};
  dsDrawBoxD(dBodyGetPosition(box.body),
             dBodyGetRotation(box.body),sides1);

  // センサの描画
  dsSetColor(0.0,0.0,1.0);
  dReal sides2[] = {sensor.length,sensor.width,sensor.height};
  dsDrawBoxD(dBodyGetPosition(sensor.body),
             dBodyGetRotation(sensor.body),sides2);
}

void start()
{
  static float xyz[3] = {0.0,-3.0,1.0};
  static float hpr[3] = {90.0,0.0,0.0};
  dsSetViewpoint (xyz,hpr);
}

void  setDrawStuff() {
  fn.version = DS_VERSION;
  fn.start   = &start;
  fn.step    = &simLoop;
  fn.command = NULL;
  fn.stop    = NULL;
  fn.path_to_textures = “../../drawstuff/textures”;
}

int main (int argc, char **argv)
{
  setDrawStuff();

  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);
  dWorldSetGravity(world,0,0,-9.8);

  ground = dCreatePlane(space,0,0,1,0);
 
  dMass m1;
  dReal x0 = 0.0, y0 = 0.0, z0 = 0.0;

  // センサ（下のボックス）
  sensor.length = 0.2;  sensor.width  = 0.2;
  sensor.height = 0.2;  sensor.mass   = 1.0;
  sensor.body   = dBodyCreate(world);  
  dMassSetZero(&m1);
  dMassSetBoxTotal(&m1,sensor.mass,sensor.length,sensor.width,sensor.height);
  dBodySetMass(sensor.body,&m1);           
  dBodySetPosition(sensor.body, x0, y0, 0.5 * sensor.height + z0);

  sensor.geom = dCreateBox(space,sensor.length,sensor.width,sensor.height);
  dGeomSetBody(sensor.geom,sensor.body);      

  // ボックス（上のボックス）
  box.length = 0.2;  box.width  = 0.2;
  box.height = 0.2;  box.mass   = 1.0;
  box.body   = dBodyCreate(world);  
  dMassSetZero(&m1);
  dMassSetBoxTotal(&m1,box.mass,box.length,box.width,box.height);
  dBodySetMass(box.body,&m1);           
  dBodySetPosition(box.body, x0, y0, sensor.height + 0.5 * box.height + z0);
 
  box.geom = dCreateBox(space,box.length,box.width,box.height);
  dGeomSetBody(box.geom,box.body);      

  // 固定ジョイント
  fixed = dJointCreateFixed(world,0);
  dJointAttach(fixed,box.body,sensor.body);
  dJointSetFixed(fixed);

  // 力とトルク情報を取得する関節を指定
  dJointSetFeedback(fixed,feedback);

  dsSimulationLoop(argc,argv,352,288,&fn);

  dWorldDestroy(world);

  return 0;
}