ゲーム開発やロボットの研究者にも使われているオープンソースの物理計算エンジン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]
コメント