ODE講座８：関節（ジョイント）を動かそー！

一昨年，レーザーラモンHGさんは小学生に人気があったそうですが今はそのブームを過ぎテレビでもあまり姿を見かけなくなりした．原ゆたか先生の怪傑ゾロリも一昨年までの勢いは少し失速し，オヤジギャグをかます小学生はすっかり影を潜め，私のようなオヤジ達が愛好する本来の姿に戻りました．このように流行は廃れるのも早いですが，ODEはますますユーザを拡大しています．

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

今回は関節（ジョイント）の動かし方を学びましょう。

ジョイントは以下の手順とAPIを使って動かします。

1. ジョイントの可動域を設定する
   dJointSetHingeParam(dJointID　joint_no, dParamLoStop）;
   dJointSetHingeParam(dJointID  joint_no,  dParamHiStop);
   joint_noにはジョイントのID番号、dParamLoStopは関節の可動域の下限[rad]、dParamHiStopは可動域の上限[rad]。
2. ジョイントの目標角速度とそれを実現するための最大トルクを設定する
   dJointSetHingeParam(dJointID, dParamVel);
   dJointSetHingeParam(dJointID, dParamFMax)
   dParamVelは関節の目標角速度[rad]/s]（直動式関節の場合は速度[m/s]）、dParamFMaxはその速度を達成するために発揮するトルク[Nm]（直動式関節の場合は力[N])。

な お，ODEでは関節の目標角度を直接指定するAPIがありませんので，目標角度に関節角をもっていくためには，現在の関節角を dJointGetHingeAngleで取得し，その角度になるまでdJointSetHingeParamを使って角速度を関節に与え，目標角度に なったら角速度を０にするという方法を使います．

では，ソー スコードを以下に示します。前回との違いはオブジェクトを胴体(torso)、上腿、下腿をもつ１本足ロボットMonoBotに変更しています。自由度は 腰関節(hip joint）が１、膝関節(knee joint)が１の計２自由度となっていますが、今回は腰関節だけ動かします。

[code]
//　sample4.c by by でむ

#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD // 単精度と倍精度の描画関数に対応するおまじない
#define dsDrawCapsule dsDrawCapsuleD
#endif

myLink torso,leg[2];
dJointID joint[2];

enum jointNo {
HIP,
KNEE
};

// ジョイントの制御
void control()
{
double velCoeff = 1.0;　// 比例定数
double p; 現在の関節角度
static double angle = 0.5 * M_PI; // M_PIは円周率

p = dJointGetHingeAngle(joint[HIP]);　// 現在の関節角度の取得
dReal z = angle - p;　　　　　　　　　　　 // angleは目標角度[rad]
if (p > 0.25 * M_PI) angle = - 0.25 * M_PI; // 目標角度の下限
if (p < - 0.25 * M_PI) angle = 0.25 * M_PI;　// 目標角度の上限

dJointSetHingeParam(joint[HIP], dParamVel, velCoeff*z);　// 目標角速度[rad/s]の設定
dJointSetHingeParam(joint[HIP], dParamFMax, 100);　 // 最大トルク[Nm]の設定
}

static void simLoop (int pause)
{
const dReal *pos1,*R1,*pos2,*R2;

time++;
control(); 　　　　　　　　　　　　　　　　　　// ジョイントの制御
dSpaceCollide(space,0,&nearCallback); // 衝突検出計算
dWorldStep(world,0.01);　　　　　　　　　// 動力学計算
dJointGroupEmpty(contactgroup);　　　// 衝突検出計算の後始末（接触点グループの解放）

// draw a torso 胴体を描画
dsSetColor(1.0,0.0,0.0);　　　　　　　　　　　　// 色の設定(r, g, b) ここでは赤
pos1 = dBodyGetPosition(torso.body);　　// 位置の取得
R1 = dBodyGetRotation(torso.body);　　 // 回転行列の取得
dsDrawSphere(pos1,R1,torso.radius); // 球の描画

// draw legs 脚を描画
for (int i = 0; i< 2; i++) {
pos2 = dBodyGetPosition(leg[i].body);
R2 = dBodyGetRotation(leg[i].body);
dsDrawCapsule(pos2,R2,leg[i].length,leg[i].radius);　　// カプセルの描画
}
}

// 　Create a monobot 一本足ロボットの生成
void createMonoBot()
{
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;

// torso 胴体
torso.radius = 0.2;　　　　// 半径[m]
torso.mass = 10.0; // 質量[kg]
torso.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1,torso.mass,torso.radius);
dBodySetMass(torso.body,&m1);
dBodySetPosition(torso.body, x0, y0, z0);
torso.geom = dCreateSphere(space,torso.radius);
dGeomSetBody(torso.geom,torso.body);

// legs 上腿、下腿
for (int i = 0; i < 2; i++) {
leg[i].radius = 0.025; leg[i].length = 0.5;　 leg[i].mass = 1.0;
leg[i].body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsuleTotal(&m1,leg[i].mass,3,leg[i].radius,leg[i].length);
dBodySetMass(leg[i].body,&m1);
dBodySetPosition(leg[i].body, x0, y0, z0 -torso.radius
- 0.5 * (2 * i + 1) *leg[i].length);
leg[i].geom = dCreateCapsule(space,leg[i].radius,leg[i].length);
dGeomSetBody(leg[i].geom,leg[i].body);
}

// hip joint 腰関節
joint[HIP] = dJointCreateHinge(world, 0);
dJointAttach(joint[HIP], torso.body,leg[0].body);
dJointSetHingeAnchor(joint[HIP], x0, y0, z0 - torso.radius);
dJointSetHingeAxis(joint[HIP], 1, 0, 0);
dJointSetHingeParam(joint[HIP], dParamLoStop, -0.25 * M_PI);　// 可動域設定 下限
dJointSetHingeParam(joint[HIP], dParamHiStop, 0.25 * M_PI);　// 上限　単位はradian

// knee joint 膝関節
joint[KNEE] = dJointCreateHinge(world, 0);
dJointAttach(joint[KNEE],leg[0].body,leg[1].body);
dJointSetHingeAnchor(joint[KNEE], x0, y0, z0 - torso.radius - leg[0].length);
dJointSetHingeAxis(joint[KNEE], 1, 0, 0);
dJointSetHingeParam(joint[KNEE], dParamLoStop, 0.0 * M_PI);　// 可動域
dJointSetHingeParam(joint[KNEE], dParamHiStop, 0.0 * M_PI);　// M_PIは円周率π（パイ）

}

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);
createMonoBot();　// 一本足ロボットの生成
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy(world);
return 0;
}

[/code]

ジョ イントを動かしている関数controlは目標値と現在値の差に比例して制御するＰ制御と なっています。ジョイントのパラメータdParamVelは目標角速度で、dParamFMaxはその角速度を実現するためにジョイントが発揮できる最大 トルクです。この値を０に設定すると目標角速度を設定してもトルクが０なのでジョイントとは動いてくれません。

ここからsample4.cのソースコードをダウンロードして遊んでください。では、また次回。

更新履歴

• 2008-1-13: コードを整形した．
• 2007-1-14: サンプルプログラムのコメントをより詳しくした。