ODE講座8:腰関節(ジョイント)を動かそー!

 サンプルフォー

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

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

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

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

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

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

参考文献
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は目標値と現在値の差に比例して制御するP制御と なっています。ジョイントのパラメータdParamVelは目標角速度で、dParamFMaxはその角速度を実現するためにジョイントが発揮できる最大 トルクです。この値を0に設定すると目標角速度を設定してもトルクが0なのでジョイントとは動いてくれません。

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

3 Comments
  1. ども。ちょっと前にコメントを投稿した生粋のスキーヤーです。(^^)

    じょーじさんと同じく、僕もプログラムの初心者です。

    ODEの書籍などを探してもなかったのでどうしよう、と思ってましたが、
    この連載によりかなりのところまでわかるようになってきました!!

    本当に助かっています。(^^)/
    これからもお体に気をつけて頑張ってください!

    質問ではないのですが、以上です!

  2. じょーじさん投稿ありがとうございます。連載の励みになります。

    では回答します。ヒンジジョイントをボディに結合させたときの角度は0 (radian)となります。ヒンジジョイントの回転軸ベクトル方向に対して右に回転させると正、左に回転させると負の角度となります。

    つまり、初期状態(基準姿勢)が重要ですので、ボディとジョイントを希望するように配置してください。

  3. はじめまして。
    大学の研究でODEを使ってシミュレーションを作ることになったのですが、
    いかんせんプログラムはほぼ初心者の状態から模索だったので
    こちらの講座は色々と参考にさせて頂いてます。
    ありがとうございます!

    ところで、一つ質問が有るのですが、
    初期状態のヒンジジョイントでの二物体の接続角度は
    指定することは可能ですか?
    マニュアルは目を通してみたつもりなのですが、見当たらず・・・
    もしよろしければ、マニュアルのどこに書いてあるかだけでも
    教えて頂けると幸いです。
    不躾な質問ですみませんが、よろしくお願いします。

    これからも、更新頑張って下さいね!

コメントを残す

メールアドレスが公開されることはありません。