ODE/スライダーでスプリングダンパー - bambooflow Note

スライダーでスプリングダンパーモデル

スライダージョイントを用いたスプリング・ダンパーモデルを作ってみました。
ここでは、ERPとCFMを利用しています。

メモ書きです。

"m"キーを押すとおもり位置を変えます。

• 減衰振動

安定位置は、初期位置(0.0)よりおもりの質量だけ下に位置します。

ソースコード

#ifdef WIN32
#include <windows.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
 
#ifdef dDOUBLE
  #define dsDrawBox dsDrawBoxD
#endif
 
const dReal step_size = 0.01;  // シミュレーションステップサイズ
static dWorldID world;
 
static dBodyID body_box1;  //固定
static dBodyID body_box2;  //おもり
dReal  box_sizes[3] = { 1.0, 1.0, 1.0 };
 
dJointID joint_spring;
 
 
 
// start simulation - set viewpoint
static void start()
{
  static float xyz[3] = { 0.f, -8.f, 8.f };
  static float hpr[3] = { 90.f, -15.f, 0.f };
 
  dsSetViewpoint( xyz, hpr );
}
 
static void command( int cmd )
{
    if (cmd == 'm') {
        dBodySetPosition( body_box2, 0.0, 0.0, 1.0 ); // 錘移動
    }
}
 
// simulation loop
static void simLoop( int pause )
{
    // Ctl+p が押されたらifに入らない
    if (!pause)
    {
        dWorldStep( world, step_size );
 
        // スプリング情報表示:debug
        {
            dReal spPos = dJointGetSliderPosition( joint_spring );
            dReal spPosRate = dJointGetSliderPositionRate( joint_spring ); 
            printf( "pos = %f, speed = %f\n", spPos, spPosRate );
        }
    }
 
    dsSetColor( 1.0f, 1.0f, 1.0f );
    dsDrawBox( dBodyGetPosition( body_box1 ), dBodyGetRotation( body_box1 ), box_sizes );
 
    dsSetColor( 0.0f, 1.0f, 0.0f );
    dsDrawBox( dBodyGetPosition( body_box2 ), dBodyGetRotation( body_box2 ), box_sizes );
 
}
 
 
int main( int argc, char* argv[] )
{
    dInitODE();
 
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start   = &start;
    fn.step    = &simLoop;
    fn.command = &command;
    fn.stop    = 0;
    fn.path_to_textures = "../drawstuff/textures";
 
    world = dWorldCreate();
    dWorldSetGravity( world, 0.0, 0.0, -9.8 );
 
 
    // 固定box
    {
        body_box1 = dBodyCreate( world );
        dReal pos[3] = { 0.0, 0.0, 10.0 };
        dBodySetPosition( body_box1, pos[0], pos[1], pos[2] );
 
        // box1の固定
        dJointID fixed;
        fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, NULL, body_box1 );
        dJointSetFixed( fixed );
    }
 
    // おもり : box1の下
    {
        body_box2 = dBodyCreate( world );
        dReal pos[3] = { 0.0, 0.0, 5.0 };
        dBodySetPosition( body_box2, pos[0], pos[1], pos[2] );
        // mass setting
        dMass mass;
        dMassSetBoxTotal( &mass, 10.0, box_sizes[0], box_sizes[1], box_sizes[2] ); // 10kg
        dBodySetMass( body_box2, &mass );
    }
 
 
    // スライダージョイント
    {
        joint_spring = dJointCreateSlider( world, 0 );
        dJointAttach( joint_spring, body_box1, body_box2 );
        dJointSetSliderAxis( joint_spring, 0.0, 0.0, -1.0 );
 
 
        /////////////////////////////////////////////////////////////////////////
        // スプリング設定
 
        dReal h = step_size;
        dReal kp = 500;   // ばね定数 (spring constant)
        dReal kd = 1.0;   // 減衰定数 (damping constant)
        dReal erp = h*kp / (h*kp + kd );
        dReal cfm = 1.0 / (h*kp + kd);
 
        dJointSetSliderParam( joint_spring, dParamLoStop, 0.0 ); // ばねの自然長位置
        dJointSetSliderParam( joint_spring, dParamHiStop, 0.0 ); // ばねの自然長位置
 
        dJointSetSliderParam( joint_spring, dParamStopERP, erp );
        dJointSetSliderParam( joint_spring, dParamStopCFM, cfm );
 
        printf( "erp = %f, cfm = %f\n", erp, cfm );
 
        // ↓今回は使用ないパラメータ
        //dJointSetSliderParam( joint_spring, dParamVel, 0.0 );
        //dJointSetSliderParam( joint_spring, dParamFMax, 100.0 );
        //dJointSetSliderParam( joint_spring, dParamCFM, 0.05 );
 
        /////////////////////////////////////////////////////////////////////////
    }
 
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    dWorldDestroy( world );
    dCloseODE();
    return 0;
}
 

説明

バネとなるスライダージョイントは次のように設定しました。

       dReal h = step_size;
       dReal kp = 500;   // ばね定数 (spring constant)
       dReal kd = 1.0;   // 減衰定数 (damping constant)
       dReal erp = h*kp / (h*kp + kd );
       dReal cfm = 1.0 / (h*kp + kd);

       dJointSetSliderParam( joint_spring, dParamLoStop, 0.0 ); // ばねの自然長位置
       dJointSetSliderParam( joint_spring, dParamHiStop, 0.0 ); // ばねの自然長位置

       dJointSetSliderParam( joint_spring, dParamStopERP, erp );
       dJointSetSliderParam( joint_spring, dParamStopCFM, cfm );

ERPとCFMは数値シミュレーションにおいて、スプリング-ダンパーシステムを擬似的に再現できるものです。
ERPとCFMの計算方法はODEマニュアルに書いてある物をそのまま使っています。

ポイントとしては、

• ｄParamLoStopとｄParamHiStopで固定している点
• ERPとCFMを使ったスプリング-ダンパーの再現

まとめ

ERPとCFMを用いることで、シミュレーションステップ中にユーザが演算をしなくて済むのが楽です。

今回、減衰器が入らない自由振動も再現出来るかいろいろ試してみたのですが、どうやっても振動が減衰してしまう結果となってしまいました。