ODE (Open Dynamics Engine) 初級講座の第11回目です。
今回は 力とトルクについて勉強しましょう。
1. ボディに関する力とトルク
ボディにかかる力、トルクを取得するAPIはdBodyGetForce(), dBodyGetTorqueです。ボディに力、トルクを設定するAPIはdBodySetForce, dBodySetTorqueとなっています。座標系は絶対座標系です。
- const dReal *dBodyGetForce (dBodyID body);
- const dReal *dBodyGetTorque (dBodyID body);
ボディbodyの力とトルクベクトルを返す。戻り値はdReal型の要素数3個の配列へのポインタ。 - void dBodySetForce(dBodyID b, dReal x, dReal y, dReal z);
- void dBodySetTorque(dBodyID b, dReal x, dReal y, dReal z);
力とトルクベクトルをボディbodyに設定する。
2.ジョイント(関節)に関する力とトルク
(1) 力とトルクの取得
次にジョイントに関する力とトルクについて学びましょう。ODEでは関節に かかるトルクなどをすぐ取得できません.取得するためにはdJointSetFeedback()で関節を指定してからdJointGetFeedback()で 情報を取得します.これはパフォーマンスを向上させるためです.常に全ての関節にかかる力とトルクが必要なわけではありませんよね.
- 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で指定している関節の力とトルクの情報を取得します.
(2) 力とトルクの設定
ジョイントの種類によって力またはトルクを設定することになります。つまり、ヒンジジョイントのような回転式ジョイントにはトルクを与えれば良いですし、スライダジョイントのような直動式ジョイントには力を与えれば良いわけです。
- dJointAddHingeTorque(dJointID joint, dReal torque)
ヒンジジョイントjointに対してトルクtorqueをかけます。ジョイントに結合されているボディ1とボディ2に対して同じトルクを反対方向にかけています。これはdBodyAddTorqueを元に作られています。 - dJointAddSliderForce(dJointID joint, dReal force)
スライダジョイントの軸に対して力forceをかけます。ジョイントに結合されているボディ1とボディ2に対して同じ力を反対方向にかけています。これはdBodyAddForceを元に作られています。
次 に,このAPIを使ったサンプルプログラムを紹介します.2つのボックスを固定ジョイント(Fixed Joint)でくっつけ,そこにかかる力を表示するプログラムです.ボックスの重さが各1kgなので,z軸方向(上方向)には9.8Nの力がかかっていれ ば理論どおりです.私の環境では理論どおり9.8の値をたたき出していました.
なお,プログラムでは2つのボックスのうち,下を圧力センサとみなしています.ヒューマノイド足裏の圧力センサをシミュレートする場合は,下のボックスのサイズを小さくし,数を増やせばよいわけです.
次からサンプルプログラムをダウンロードできます。
// sample11.cpp by Kosei Demura 2006-2008 #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(); dInitODE(); 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); dCloseODE(); return 0; }