10. 速度と加速度

ODE (Open Dynamics Engine) 初級講座の第１０回目です。

今回は、速度、角速度、加速度、角加速度について学びます。

１．速度、角速度

剛体の速度の設定はdBodySetLinearVel()、角速度の設定はdBodySetAngularVel()、を使います。ただし、この速度は重心の速度、角速度は重心周りの角速度です。また、ボディの速度を設定することは初期状態だけにし、時間ステップ毎に強制的にある速度を設定することは、実際の物理現象とは異なった挙動を生み出してしまいますので避けてください。

もし、物体の速度をコントロールした場合は、関節に付属しているモータを使ってください。

void dBodySetLinearVel(dBodyID body, dReal x, dReal y, dReal z);
bodyの速度を絶対座標系で(x, y, z) [m/s]に設定します。

void dBodySetAngularVel(dBodyID body, dReal x, dReal y, dReal z);
bodyの角速度を絶対座標系の各軸周りに(x, y, z) [rad/s]と設定します。

const dReal *dBodyGetLinearVel(dBodyID body);
bodyの速度を取得します。戻り値は要素数３個の配列を指すコンストポインタ。

const dReal *dBodyGetAngularVel(dBodyID body);
bodyの角速度を取得します。戻り値は要素数３個の配列を指すコンストポインタ。

２．加速度、角加速度

残念ながらODEは加速度、角加速度を取得するAPIはありません。速度変化、あるいは角速度変化を時間ステップサイズで割ってそれらを求めてください。時間ステップとはdWorldStep(dWorldID, dReal stepsize)の２番目の引数stepsize、数値積分の時間ステップサイズのことです。

注意する点としては、dBodyGetLinearVel(), dBodyGetAngularVel()の戻り値が要素数３個の配列を指すポインタです。つまり、戻り値がスカラではなく絶対座標系（x, y, z軸）の３次元ベクトルです。角速度は各軸周りです。

３．サンプルプログラム

サンプルプログラムは第９回目のサンプルプログラムsample9.cppに球の速度、加速度並びに円柱の速度、加速度表示を追加するだけの簡単なプログラムです。

static void simLoop (int pause)
{
  static long steps = 0;
  const dReal stepsize = 0.01;    
  const dReal *linear_vel, *angular_vel;
  static dReal linear_vel_old[3], angular_vel_old[3];
  dReal linear_accel[3], angular_accel[3];

  if (!pause)  {
    dSpaceCollide(space,0,&nearCallback);
    dWorldStep(world,stepsize);
    dJointGroupEmpty(contactgroup);
  }

  // 速度(Linear velocity),  角速度(Angular Velocity)
  linear_vel  = dBodyGetLinearVel(ball.body);
  angular_vel = dBodyGetAngularVel(pillar.body);
  printf("\n%d steps \n", steps++);
  printf("Linear Velocity: x=%.3f y=%.3f z=%.3f \n", linear_vel[0],linear_vel[1],linear_vel[2]);
  printf("Angular Velocity: x=%.3f y=%.3f z=%.3f \n",       angular_vel[0],angular_vel[1],angular_vel[2]);

  // 加速度(Linear acceleration), 角加速度(Angular acceleration)
  for (int i=0; i < 3; i++) {
    linear_accel[i] = (linear_vel[i]-linear_vel_old[i])  /stepsize;
    angular_accel[i] = (angular_vel[i]-angular_vel_old[i])/stepsize;
  }

  printf("Linear Acceleration: x=%.3f y=%.3f z=%.3f \n",       linear_accel[0],linear_accel[1],linear_accel[2]);
  printf("Angular Acceleration: x=%.3f y=%.3f z=%.3f \n",       angular_accel[0],angular_accel[1],angular_accel[2]);

  for (int j=0; j < 3; j++) {
    linear_vel_old[j]  = linear_vel[j];
    angular_vel_old[j] = angular_vel[j];
  }

  drawObject(ball.geom,1.3,0,0);
  drawObject(pillar.geom,0,0,1.3);
}

次からサンプルプログラムをダウンロードできます。

サンプルプログラム１０

おしまい。