ODE/スプリングヒンジ - bambooflow Note

スプリング・ヒンジ（バネ蝶番）

バネ付きの蝶番を作る方法。
ここでは、ヒンジのパラメータ設定でスプリングモデルを作ります。

計算的に足りていないところはあると思いますが、シンプルに。
メモ書きです。

ボールを当てて、板を振動させています。
ヒンジに対して角度に比例したバネ力と速度に比例した減衰力を与えて振幅を繰り返しながら停止するようなシミュレーションです。

ソースコード

#ifdef WIN32
#include <windows.h>
#endif
 
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <vector>
 
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawConvex dsDrawConvexD
#define dsDrawLine dsDrawLineD
#define dsDrawTriangle dsDrawTriangleD
#endif
 
 
 
struct ST_Object
{
    dGeomID geom;
    dBodyID body;
    float color[4];
 
    ST_Object() : geom(NULL), body(NULL) {
        color[0] = 1.0; color[1] = 1.0; color[2] = 1.0; color[3] = 1.0;
    }
 
    void setColor( float r, float g, float b, float a=1.0 ) {
        color[0] = r; color[1] = g; color[2] = b; color[3] = a;
    }
};
 
//////////////////////////////////////////////////////////////////////
static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;
 
 
ST_Object* fixed_obj = NULL; // 固定物体
ST_Object* board = NULL; // 扉
ST_Object* ball = NULL;
std::vector<ST_Object*> drawObjList;
 
dJointID hinge; // 蝶番
 
//////////////////////////////////////////////////////////////////////
 
 
void drawObj( const ST_Object *obj )
{
    if (!obj->geom) return;
 
    const dReal *pos = dGeomGetPosition( obj->geom );
    const dReal *R = dGeomGetRotation (obj->geom);
 
    dsSetColorAlpha( obj->color[0], obj->color[1], obj->color[2], obj->color[3] );
 
    int type = dGeomGetClass (obj->geom);
    if (type == dBoxClass) {
        dVector3 sides;
        dGeomBoxGetLengths (obj->geom,sides);
        dsDrawBox (pos,R,sides);
    }
    else if (type == dSphereClass) {
        dsDrawSphere (pos,R,dGeomSphereGetRadius(obj->geom));
    }
    else if (type == dCapsuleClass) {
        dReal radius,length;
        dGeomCapsuleGetParams (obj->geom,&radius,&length);
        dsDrawCapsule (pos,R,length,radius);
    }
    else if (type == dCylinderClass) {
        dReal radius,length;
        dGeomCylinderGetParams (obj->geom,&radius,&length);
        dsDrawCylinder (pos,R,length,radius);
    }
    else if (type == dRayClass) {
        dVector3 Origin, Direction;
        dGeomRayGet(obj->geom, Origin, Direction);
        dReal Length = dGeomRayGetLength(obj->geom);
 
        dVector3 End;
        End[0] = Origin[0] + (Direction[0] * Length);
        End[1] = Origin[1] + (Direction[1] * Length);
        End[2] = Origin[2] + (Direction[2] * Length);
        End[3] = Origin[3] + (Direction[3] * Length);
 
        dsDrawLine(Origin, End);
    }
    //else if (type == dTriMeshClass) {
    //    int triCount = dGeomTriMeshGetTriangleCount( obj->geom );
    //    dReal pos_[3] = {0.0, 0.0, 0.0};
    //    for (int ii = 0; ii < triCount; ii++) {
    //        dVector3 v0, v1, v2;
    //        dGeomTriMeshGetTriangle( obj->geom, ii, &v0, &v1, &v2 );
    //        dsDrawTriangle( pos_, R, v0, v1, v2, 1 );
    //    }
    //}
}
 
 
// 衝突検出用関数
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
    static const int MAX_CONTACTS = 4; // 最大の衝突検出可能数
 
    dBodyID b1 = dGeomGetBody( o1 ); // 物体1
    dBodyID b2 = dGeomGetBody( o2 ); // 物体2
 
    if ( b1 && b2 && dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
    return; // 衝突対象でない物体の衝突ははずす
 
    dContact contact[MAX_CONTACTS];
    for ( int i=0; i<MAX_CONTACTS; i++ )
    {
        // 物体同士の接触時のパラメータ設定
        contact[i].surface.mode
            = dContactBounce
            | dContactSoftERP
            | dContactSoftCFM
            | dContactMu2
            | dContactSlip1
            | dContactSlip2;
        contact[i].surface.mu  = 300.0;        // 摩擦係数
        contact[i].surface.mu2 = 300.0;        // 摩擦係数2
        contact[i].surface.bounce = 0.5;       // 反発係数
        contact[i].surface.soft_erp = 0.3;     // ERP設定
        contact[i].surface.soft_cfm = 0.0001;  // CFM設定
        contact[i].surface.slip1 = 0.01;
        contact[i].surface.slip2 = 0.01;
    }
 
    // 衝突検出
    int numc = dCollide( o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof( dContact ) ); 
    if ( numc > 0 )
    {
        for ( int i=0; i<numc; i++ )
        {
            dJointID c = dJointCreateContact( world, contactgroup, contact+i );
            dJointAttach( c, b1, b2 );
        }
    }
}
 
 
FILE *fp;
 
// start simulation - set viewpoint
static void start()
{
    static float xyz[3] = { 0.f, -5.0f, 3.f };
    static float hpr[3] = { 60.f, -15.f, 0.f };
 
    dsSetViewpoint( xyz, hpr );
 
    //fp = fopen( "a.log", "w" );
 
}
 
// simulation loop
static void simLoop( int pause )
{
    // Ctl+p が押されたらifに入らない
    if (!pause)
    {
        dSpaceCollide( space, 0, &nearCallback ); // 衝突検出
 
 
        ////////////////////////////////////////////////////////////////////////
        // 蝶番設定
        // my + cy + ky = 0　：減衰振動の方程式
 
        const dReal k = 500.0;   // ばね定数
        const dReal c = 50.0;    // 減衰係数
        dReal angle = dJointGetHingeAngle(hinge);
        dReal rate = dJointGetHingeAngleRate(hinge);
        dReal f = -( c*rate + k*angle );
        dReal vel = 10.0;
 
        //dJointSetHingeParam( hinge, dParamLoStop, 10.0 );
        //dJointSetHingeParam( hinge, dParamHiStop, 10.0 );
        if (f < 0.0) {
            dJointSetHingeParam( hinge, dParamFMax, -f );
            dJointSetHingeParam( hinge, dParamVel, -vel );
        } else {
            dJointSetHingeParam( hinge, dParamFMax, f );
            dJointSetHingeParam( hinge, dParamVel, vel );
        }
        //dJointSetHingeParam( hinge, dParamFudgeFactor, 1.0 );
        //dJointSetHingeParam( hinge, dParamBounce, 1.0 );
        //dJointSetHingeParam( hinge, dParamCFM, 0.0001 );
        //dJointSetHingeParam( hinge, dParamStopERP, 0.3 );
        //dJointSetHingeParam( hinge, dParamStopCFM, 0.01 );
 
        printf( "Angle = %f, Rate = %f, f=%f\n", angle, rate, f );
        //fprintf( fp, "%f\t%f\t%f\n", angle, rate, f );
        ////////////////////////////////////////////////////////////////////////
 
        dWorldStep( world, 0.01 );
 
        dJointGroupEmpty( contactgroup );
    }
 
    // draw object
    {
        std::vector<ST_Object*>::iterator it = drawObjList.begin();
        while (it != drawObjList.end())
        {
            ST_Object *obj = (*it);
            drawObj( obj );
            ++it;
        }
    }
}
 
 
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 = 0;
    fn.stop    = 0;
    fn.path_to_textures = "../drawstuff/textures";
 
    world = dWorldCreate();
    dWorldSetGravity( world, 0.0, 0.0, -9.8 );
 
    space = dHashSpaceCreate( 0 ); 
    contactgroup = dJointGroupCreate( 0 );
 
    dCreatePlane( space, 0, 0, 1, 0 );
 
 
    // 固定物体（軸）
    {
        fixed_obj = new ST_Object;
 
        dReal pos[3] = { 0.0, 0.0, 1.0 };
        dReal size[3] = { 0.2, 0.2, 1.0 };
 
        // body setting
        fixed_obj->body = dBodyCreate( world );
        dBodySetPosition( fixed_obj->body, pos[0], pos[1], pos[2] );
        // geom setting
        fixed_obj->geom = dCreateBox( space, size[0], size[1], size[2] );
        dGeomSetBody( fixed_obj->geom, fixed_obj->body );
        // mass setting
        dMass mass;
        dMassSetBoxTotal( &mass, 1.0, size[0], size[1], size[2] ); // 10kg
        dBodySetMass( fixed_obj->body, &mass );
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 0.0*M_PI/180 );
        dBodySetRotation( fixed_obj->body, R );
 
        fixed_obj->setColor( 0.0, 1.0, 1.0, 0.5 );
 
        // 物体の固定
        dJointID fixed_joint;
        fixed_joint = dJointCreateFixed( world, 0 );
        dJointAttach( fixed_joint, NULL, fixed_obj->body );
        dJointSetFixed( fixed_joint );
 
        drawObjList.push_back( fixed_obj );
    }
 
 
    // 板
    {
        board = new ST_Object;
 
        dReal pos[3] = { 2.0, 0.0, 1.0 };
        dReal size[3] = { 3.0, 0.2, 1.0 };
 
        // body setting
        board->body = dBodyCreate( world );
        dBodySetPosition( board->body, pos[0], pos[1], pos[2] );
        // geom setting
        board->geom = dCreateBox( space, size[0], size[1], size[2] );
        dGeomSetBody( board->geom, board->body );
        // mass setting
        dMass mass;
        dMassSetBoxTotal( &mass, 10.0, size[0], size[1], size[2] ); // 10kg
        dBodySetMass( board->body, &mass );
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 0.0*M_PI/180 );
        dBodySetRotation( board->body, R );
 
        board->setColor( 0.0, 1.0, 1.0, 0.5 );
        drawObjList.push_back( board );
 
        ////////////////////////////////////////////////////////////////////////
        // 蝶番
        hinge = dJointCreateHinge( world, 0 );
        dJointAttach( hinge, fixed_obj->body,board->body );
        dJointSetHingeAnchor( hinge, 0.0, 0.0, 1.0 ); // 位置設定
        dJointSetHingeAxis( hinge, 0.0, 0.0, 1.0 );   // 軸設定
 
        ////////////////////////////////////////////////////////////////////////
    }
 
 
    // ボール生成
    {
        ball = new ST_Object;
 
        dReal pos[3] = { 2.5, -4.0, 0.5 };
        dReal radius = 0.5;
 
        // body setting
        ball->body = dBodyCreate( world );
        dBodySetPosition( ball->body, pos[0], pos[1], pos[2] );
        // geom setting
        ball->geom = dCreateSphere( space, radius );
        dGeomSetBody( ball->geom, ball->body );
        // mass setting
        dMass mass;
        dMassSetSphereTotal( &mass, 1.0, radius ); // 10kg
        dBodySetMass( ball->body, &mass );
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 0.0*M_PI/180 );
        dBodySetRotation( ball->body, R );
 
        ball->setColor( 1.0, 1.0, 0.0 );
        drawObjList.push_back( ball );
    }
 
 
    dBodySetForce( ball->body, 0.0, 2000.0, 0.0 ); // ボールを転がす
 
 
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    dWorldDestroy( world );
    dCloseODE();
    return 0;
}
 
 

説明

• ヒンジの設定

       hinge = dJointCreateHinge( world, 0 );
       dJointAttach( hinge, fixed_obj->body,board->body );
       dJointSetHingeAnchor( hinge, 0.0, 0.0, 1.0 ); // 位置設定
       dJointSetHingeAxis( hinge, 0.0, 0.0, 1.0 );   // 軸設定

• スプリング設定

       const dReal k = 500.0;   // ばね定数
       const dReal c = 50.0;    // 減衰係数
       dReal angle = dJointGetHingeAngle(hinge);
       dReal rate = dJointGetHingeAngleRate(hinge);
       dReal f = -( c*rate + k*angle );
       dReal vel = 10.0;

       if (f < 0.0) {
           dJointSetHingeParam( hinge, dParamFMax, -f );
           dJointSetHingeParam( hinge, dParamVel, -vel );
       } else {
           dJointSetHingeParam( hinge, dParamFMax, f );
           dJointSetHingeParam( hinge, dParamVel, vel );
       }

まとめ

今回は、dParamFMaxでバネ力＋減衰力を与えました。このとき、dParamVel（速度）は一定。

fという力の与え方で、もし減衰力をなくしてしまいバネ力だけにしてしまうと永遠に振幅します。（自由振動）
また、減衰力をある程度大きくすると、振動は起こらずにゆっくりと元の位置に戻ろうとするはずです。

参考

• http://wwwra.meijo-u.ac.jp/labs/ra007/murata/onlinetext/quake/c5-damp.htm

以上