ODEのテンプレートプログラム

ODEのテンプレートプログラムで流れを確認しておく.

ヘッダや宣言

dobuleとfloatの描画ルーチンを自動でコンパチにする宣言を入れておく.

#include <iostream>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

// Doubleで扱う場合は描画ルーチンをD型にする.
#ifdef dDOUBLE
#define dsDrawBox      dsDrawBoxD
#define dsDrawSphere   dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule  dsDrawCapsuleD
#define dsDrawLine     dsDrawLineD
#endif

// Drawstuffで描画する際のテクスチャの場所を指定する.
#define TEXTURES_PATH ("/usr/local/share/drawstuff/textures")
#define DENSITY (5.0)

オブジェクトの構造体

剛体計算はBodyとMassで,衝突計算はGeomを使う.両方を使う場合はそれで一つのオブジェクトになるように構造体を宣言しておくと便利.

各種オブジェクトの描画ルーチンセット

これは問題関係なく共通で使えると思う.GeomからPosition/Rotationとかの情報は取れるので,GeomIDを渡すようにする. でも,実は下のはちゃんと動かない...シリンダーとカプセルのIDがどっちも3でうまくスイッチできない...なぜだろう?

// Both of the Cylinder and Capsule class id is 3 ??
void drawObject(dGeomID gID,float red,float green,float blue) {
  dsSetColor(red,green,blue);
  //std::cout << dGeomGetClass(gID) << std::endl;
  switch (dGeomGetClass(gID)) {
  case dSphereClass:
    {
      dsDrawSphere(dGeomGetPosition(gID),dGeomGetRotation(gID),dGeomSphereGetRadius(gID));
      break;
    }
  case dBoxClass:
    {
      dVector3 length;
      dGeomBoxGetLengths(gID, length);
      dsDrawBox(dGeomGetPosition(gID),dGeomGetRotation(gID),length);
      break;
    }
  case dCylinderClass:
    {
      dReal r,length;
      dGeomCylinderGetParams(gID, &r,&length);
      dsDrawCylinder(dGeomGetPosition(gID),dGeomGetRotation(gID),length,r);
      break;
    }
    case dCapsuleClass:
    {
      dReal r,length;
      dGeomCapsuleGetParams(gID, &r,&length);
      dsDrawCapsule(dGeomGetPosition(gID),dGeomGetRotation(gID),length,r);
      break;
    }
  }
}

Drawstuffのセットアップ

Drawstuffを管理するオブジェクトdsfFunction(ここではfnとしている)を定義しておいて,下記でセットアップする.

void  setDrawStuff() {
  fn.version = DS_VERSION;
  fn.start   = &initDs;     //初期化関数へのポインタ
  fn.step    = &simLoop;   // 無限ループで毎回実行する関数ポインタ
  fn.command = command;    // キーボード入力を処理する関数ポインタ
  fn.stop    = NULL;       // 停止時の処理の関数ポインタ
  fn.path_to_textures = TEXTURES_PATH;
}

dsFuntion.startにセットする関数ではカメラ視点の設定などを実行する.xyzはカメラの絶対座標,hprはヨー,ピッチ,ロール.右手系にHPRのそれぞれの回転角度があるとして,それぞれの方向にどれだけ回すかを与える.注意点として,DrawStuffの角度は度(degree)で与えられる.他は一般的にラジアンで統一されている場合があるので注意. 下の例だとX軸方向に180度,つまり画面の奥側を向いて,Y,Z軸には回転しない.

static void start() {
  static float xyz[3] = {10, 0, 1.0};
  static float hpr[3] = {-180, 0, 0};
  dsSetViewpoint (xyz,hpr);          
}

キーボード入力を処理する関数は例えば下記.

void command(int cmd) {
  switch (cmd) {
  case 'r': v += 0.5; break;
  case 'l': v -= 0.5;break;
  default: std::cout << "WARNING: unknown key input" << std::endl;
  }
}

fn.stepはDrawstuffの中の無限ループ内で実行される関数をセットする.逆に,Drawstuffを使わないで(描画しないで)ODEのシミュレーションをする場合は,この関数を別途無限ループの中に入れるんだ. ジョイントを制御をする場合のcontrolに関しては,その項目を参照.

static void simLoop (int pause) {
  if (!pause) {
    dSpaceCollide(space,0,&nearCallback); // 衝突計算
    dWorldStep(world,0.1);                // タイムステップを1時刻すすめる(動力学計算)
    dJointGroupEmpty(contactgroup);       // 衝突計算のジョイントグループを初期化
    control();                            // ジョイントを制御する場合はその関数を呼び出す.
  }
  // 結果を描画する(Drawstuffを使わない場合は飛ばす)
  drawObject(base.geom, 0,0,05);

  // STDOUTに情報を出力する場合
  std::cout << "angle:" << dJointGetHingeAngle(joint);
}

衝突計算をする場合

衝突計算自体はdSpaceCollideで実行される.これは各オブジェクトが衝突するかどうかを判定してくれる.この関数にはnearCallBack関数を登録する.これは,2つの物体が衝突する(かも知れない)と判定された時に呼び出される関数.このコールバック関数の中で接触点をジョイントとして登録する.ジョイントはすなわち拘束条件だ.摩擦係数や反発係数は別途PRAM_BOUNCEなどでマクロ定義しておく.

static void nearCallback(void *data, dGeomID o1, dGeomID o2) {
  const int N = 10;
  dContact contact[N];

  int isGround = ((ground == o1) || (ground == o2));

  int n =  dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
  if (isGround) {
    for (int i = 0; i < n; i++) {
      contact[i].surface.mode = dContactBounce|dContactSoftERP|dContactSoftCFM;
      contact[i].surface.bounce = PRAM_BOUNCE;
      contact[i].surface.mu = PRAM_MU;
      contact[i].surface.soft_erp = PARAM_SOFT_ERP;
      contact[i].surface.soft_cfm = PRAM_SOFT_CFM;
      dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
      dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
    }
  }
}

ロボット定義

シミュレーション対象となるロボットの定義は,(1)剛体(Body)の定義,(2)それに質量(Mass)を付けて,(3)幾何(Geom)を付ける.Geomは衝突計算をしない場合は不要.これを愚直に書いて行くんだ. 例えば,直方体に円柱の車輪をつける例.ほとんどそのままだけど,円柱は回転させる必要があるので,回転行列rotを定義して,回転させたい軸と回転角に対する回転行列をdRFromAxisAndAngle([0,1,0]の軸の周りに,[0,1,pi/2]回す)で作っておいて,dBodySetRotationで回転する.  また,車輪を回したいのでジョイントを付ける.ODEのジョイントはモータを内蔵しているので,それを回すことで車輪を回せる.Axisがジョイントの軸方向,Anchorが軸の場所.

void makeRobot() {
  dMass m;     
  dMassSetZero (&m);

  //basement
  base.body = dBodyCreate (world);
  dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
  dBodySetMass (base.body,&m);
  dBodySetPosition (base.body,0,0,radius+sides[2]/2);
  base.geom = dCreateBox(space, sides[0], sides[1], sides[2]);
  dGeomSetBody(base.geom,base.body);

  dMatrix3 rot;
  // wheel
  dRFromAxisAndAngle(rot,0,1,0, M_PI/2.0);
  wheel.body = dBodyCreate (world);
  dMassSetCylinder(&m,DENSITY,1,radius,width);
  dBodySetMass (wheel.body,&m);
  dBodySetPosition (wheel.body,0.0,0,0.3);
  wheel.geom = dCreateCylinder(space, radius, width);
  dGeomSetBody(wheel.geom,wheel.body);     
  dBodySetRotation(wheel.body,rot);

  joint = dJointCreateHinge(world, 0);
  dJointAttach(joint, base.body, wheel.body);
  dJointSetHingeAxis(joint, 1, 0, 0);
  dJointSetHingeAnchor(joint, 0.0, 0, 0.3);
}

Main関数

ここまで出来たら,最後にメイン関数を書いて終わりだ.world,spaceなどをグローバル変数で渡すような例が多いようなので,グローバル変数として扱うなら適切な場所で定義しておく.

// Global variables
static dWorldID      world;
static dSpaceID      space;
static dGeomID       ground;
static dJointGroupID contactgroup;
dsFunctions fn;

int main (int argc, char **argv)
{
  setDrawStuff();
  dInitODE();            
  world = dWorldCreate();
  dWorldSetGravity(world,0,0,-3.0);
  dWorldSetERP(world, 0.5);
  dWorldSetCFM(world, 1e-05);
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);
  ground = dCreatePlane(space,0,0,1,0);

  makeRobot();
  // initial noise
  dsSimulationLoop (argc,argv,600,400,&fn);
  dWorldDestroy (world);
  dJointGroupDestroy(contactgroup);
  dSpaceDestroy(space);
  dCloseODE();          

  return 0;
}

制御する

制御をするには力やトルクを与える.ここで重要なのは,力やトルクはシミュレーションの更新ごとに0に初期化されるので,継続的に力を加える場合は継続的に力を加える.一回与えても,その一回で消えちゃうよ. 力やトルクをdBodyAddForceのようにBodyに直接力を加える場合の他に,ODEのジョイントはモータになっていて,そのモータにトルクを与えることができる.実際にはジョイントにはトルクではなく速度を与える.

シミュレーション上のTips

描画をしない

デバッグが終わったら描画をせずにシミュレーションだけをしたい場合もある.その場合はsimLoopの描画の箇所をスキップするようにして,Drawstuffのループではなくて自分のWhile(0)ループの中でsimLoopを回す. 他にも,ctrl-tでテクスチャをOffにできる.ウィンドウサイズを変えたりでもシミュレーション速度が変わる.

制御コマンド

デフォルトでいくつかの制御キーコマンドが定義されている. - ctrl-t:テクスチャをOn/Off - ctrl-s:影のOn/Off - ctrl-p:シミュレーションの一時停止 - ctrl-o:1ステップ実行

リスタート

強化学習のシミュレーションなどをやる場合には何度もシミュレーションを繰り返す.やり方は,一旦Body, Geom, Joint,JointGroupを全部消して,再度作りなおす.

void makeRobot() {
... // 上のと同じでいい
}

void deleteRobot() {
  dJointDestory(joint); // jointを削除
  dBodyDestory(body);   // bodyを削除
  dGeomDestory(geom);   // geomを削除
  dJointGroupDestory(contactgroup);
}

void restart() {
  deleteRobot();
  contactgroup = dJointGroupCreate(0);
  makeRobot();
}