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

 

最 近はレーザーラモンHGさんも小学生に人気があるそうですが、原ゆたか先生の怪傑ゾロリは圧倒的に小学生に人気があります。近くの図書館ではあまりの人気 のため貸し出し禁止になり、館内閲覧だけになってしまいました。私の子供も大好きでオヤジギャグをかましてくれます(例えば「校長先生ぜっこうちょう」文 献 [1]より引用)。

さて、ODE(Open Dynamics Engine)講座の第８回目です。今回は前回作った腰関節（ヒップジョイント）を動かしましょう。

 ジョイントは以下の手順で動かします。

1. ジョイントの可動域を設定する
   dJointSetHingeParam(dJointID, dParamLoStop, 可動域の下限）;
   dJointSetHingeParam(dJointID, dParamHiStop, 可動域の上限）;
2. ジョイントの目標角速度とそれを実現するための最大トルクを設定する
   dJointSetHingeParam(dJointID, dParamVel,  目標角速度);
   dJointSetHingeParam(dJointID, dParamFMax, 最大トルク)

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

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

参考文献
1.　原ゆたか著、 ぐんぐん身につくゾロリ式おやじギャグドリル、かいけつゾロリの大どろぼうの付録、ポプラ者、2005

 

 // サンプル　フォー　by でむ

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;

  p =  dJointGetHingeAngle(joint[HIP]);　// 現在の関節角度の取得
  dReal z = angle  –  p;　　　　　　　　　　　// angleは目標角度
  if (p > 0.24 * M_PI) angle = – 0.25 * M_PI;
  else {
    if (p < – 0.24 * M_PI) angle = 0.25 * M_PI;
  }
  dJointSetHingeParam(joint[HIP], dParamVel,  velCoeff*z);　// 目標角速度の設定
  dJointSetHingeParam(joint[HIP], dParamFMax, 100);　// 最大トルクの設定
}

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

  time++;
  control();  // ジョイントの制御
  dSpaceCollide(space,0,&nearCallback);
  dWorldStep(world,0.01);
  dJointGroupEmpty(contactgroup);
  dsSetColor(1.0,0.0,0.0);

  // draw a torso   胴体を描画                                                                     
  pos1 = dBodyGetPosition(torso.body);
  R1   = dBodyGetRotation(torso.body);
  dsDrawSphereD(pos1,R1,torso.radius);

  // draw legs    脚を描画                                                                       
  for (int i = 0; i< 2; i++) {
    pos2 = dBodyGetPosition(leg[i].body);
    R2   = dBodyGetRotation(leg[i].body);
    dsDrawCappedCylinderD(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;　  torso.mass   = 10.0;
  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);
    dMassSetCylinderTotal(&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 = dCreateCCylinder(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;
}

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

 ここからサンプル４（フォーーー）のソースコードをダウンロードして遊んでください。では、また次回。