Open Dynamics Engine 入門
【2日目】オブジェクトのジョイント

ODE（Open Dynamics Engine）の入門編の2日目です。 といっても、自分自身の勉強がてらつくっています。 「【1日目】球の描画と衝突判定」では、ODEを利用するモチベーションと基本となるプログラムを作成いたしました。

2つの剛体をつなぐ

ODEでは、2つ以上のオブジェクト間に拘束を設定することができます。 今回は、球体と長細いカプセルをヒンジでつないだ物体を、空から大量に投下することをシミュレーションします。

プログラムソース

以下のプログラムソースは、demura.net: ロボットの開発と教育にて公開されているソースを利用させていただいております。

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

int WindowWidth  = 352; //ウィンドウの幅
int WindowHeight = 288; //ウィンドウの高さ


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

const int N = 100;//マッチ棒の数

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

typedef struct {
  dBodyID body;
  dGeomID geom;
  dReal   radius;
  dReal   length;
  dReal   mass;
} myLink;
myLink ball[N], pole[N];      // add

dJointID joint[N];


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

  int n =  dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
  for (int i = 0; i < n; i++) {
    contact[i].surface.mu    = 10.0;//dInfinity;
    contact[i].surface.mu2   = 10.0;//dInfinity;
    contact[i].surface.mode = dContactBounce;
    contact[i].surface.bounce     = 0.1; // (0.0~1.0)
    contact[i].surface.bounce_vel = 0.01;
    dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
    dJointAttach (c,dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
  }
}

static void simLoop (int pause)
{
  dSpaceCollide(space,0,&nearCallback);
  //dWorldStep(world,0.002);
  dWorldQuickStep(world,0.005);
  dJointGroupEmpty(contactgroup);
  
  for(int i=0; i<N; i++){
    dsSetColor(1.0,0.0,0.0);
    dsDrawSphereD( dBodyGetPosition(ball[i].body),dBodyGetRotation(ball[i].body),ball[i].radius);
    dsDrawCapsuleD(dBodyGetPosition(pole[i].body),dBodyGetRotation(pole[i].body),pole[i].length,pole[i].radius);
  }
}

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  prepDrawStuff() {
  fn.version = DS_VERSION;
  fn.start   = &start;
  fn.step    = &simLoop;
  fn.command = NULL;
  fn.stop    = NULL;
  fn.path_to_textures = "C:/ode-0.11.1/drawstuff/textures";
}

void createBallandPole() {
  dMass m1;

  for(int i=0; i<N; i++){
    dReal x0 = 0.0, y0 = 0.0, z0 = 1.2 * (i+1) +2.0;
    dReal rx = double(rand()-rand())/double(RAND_MAX);
    dReal ry = double(rand()-rand())/double(RAND_MAX);

    ball[i].radius = 0.1;
    ball[i].mass   = 0.1;
    ball[i].body = dBodyCreate(world);
    dMassSetZero(&m1);
    dMassSetSphereTotal(&m1,ball[i].mass,ball[i].radius);
    dBodySetMass(ball[i].body,&m1);
    dBodySetPosition(ball[i].body, x0 + rx, y0+ ry, z0);
    ball[i].geom = dCreateSphere(space,ball[i].radius);
    dGeomSetBody(ball[i].geom,ball[i].body);

    pole[i].radius = 0.025;
    pole[i].length = 1.0;
    pole[i].mass   = 2.0;
    pole[i].body = dBodyCreate(world);
    dMassSetZero(&m1);
    dMassSetCapsule(&m1,pole[i].mass,3,pole[i].radius,pole[i].length);
    dBodySetMass(pole[i].body,&m1);
    dBodySetPosition(pole[i].body, x0 + rx, y0 + ry, z0 - ball[i].radius - 0.5 * pole[i].length - pole[i].radius );

    pole[i].geom = dCreateCapsule(space, pole[i].radius, pole[i].length);
    dGeomSetBody(pole[i].geom, pole[i].body);

    // Hinge joint
    joint[i] = dJointCreateHinge(world, 0);
    dJointAttach(joint[i], ball[i].body,pole[i].body);
    dJointSetHingeAnchor(joint[i], x0 + rx, y0 + ry, z0 - ball[i].radius - pole[i].radius/2 );
    dJointSetHingeAxis(joint[i], 0, 0, 1);
  }
}

void  setDrawStuff() {
  fn.version = DS_VERSION;
  fn.start   = &start;
  fn.step    = &simLoop;
  fn.command = NULL;
  fn.stop    = NULL;
  fn.path_to_textures = "C:/ode-0.11.1/drawstuff/textures";
}

int main (int argc, char **argv)
{
   srand(int(time(NULL))); //rand()の種

  setDrawStuff();
  dInitODE();
  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);

  dWorldSetGravity(world,0,0,-9.8);
  dWorldSetERP(world, 0.2);
  dWorldSetCFM(world, 10E-5);
  dWorldSetLinearDamping(world,0.001);

  ground = dCreatePlane(space,0,0,1,0);
  createBallandPole();
  dsSimulationLoop (argc,argv,WindowWidth, WindowHeight,&fn); //シミュレーション用の無限ループ

  dWorldDestroy (world);
  dCloseODE();

  return 0;
}

メモ

■Q．dSolveLCP関数にエラー
■A．スタック領域を使い果たすとエラーになる

コンパイル時に「 /F [バイト数] 」オプションをつけることで、スタック領域を明示的に指定することができる。 （[]の中は整数を入れる。デフォルトで、1000000 (1MB) 。）

参考

・ODE debugging dSolveLCP（英語）
・/F (スタック サイズの設定)