ODE講座13：圧力センサを作ろう！

ゲーム開発やロボットの研究者にも使われているオープンソースの物理計算エンジンODE(Open Dynamics  Engine、オープン ダイナミクスエンジン）を学ぶODE講座の第１３回目です。

今回は ジョイント（関節）にかかる力を取得する方法を勉強します．

これを応用するとヒューマノイドロボットの足裏に付け，ZMPの計測に利用する圧力センサをシミュレートできます．

• void dJointSetFeedback (dJointID, dJointFeedback *);
  力とトルクの情報を取得する関節JointIDに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の値をたたき出していました．

なお，プログラムでは２つのボックスのうち，下を圧力センサとみなしています．ヒューマノイド足裏の圧力センサをシミュレートする場合は，下のボックスのサイズを小さくし，数を増やせばよいわけです．

[code]
// sensor.cpp by Kosei Demura 2006-7-26
#include
#include

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
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;
}

[/code]