ODE/歯車 - bambooflow Note

歯車モデル

ODEで歯車みたいに1つのホイール回転をもう一方のホイールに伝達させるようなモデルを作るにどうすればいいのだろうと常々考えていました。
ようやく1つの解が見つかったので紹介します。
メモ書きです。

＜＜執筆中＞＞

本当の歯車みたいに歯は付いてないですけど、モデルということで。

test_gear_model.zip

左がwheel1で右がwheel2です。
wheel1のみ、トルクを与えることができます。
"a"：トルク加算(+)
"d"：トルク減算(-)

ソースコード

#ifdef WIN32
#include <windows.h>
#endif
 
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
 
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
 
#ifdef dDOUBLE
  #define dsDrawBox dsDrawBoxD
  #define dsDrawSphere dsDrawSphereD
  #define dsDrawCylinder dsDrawCylinderD
#endif
 
static dWorldID world;
static dSpaceID space;
 
static dBodyID body_box1; // fixed
static dBodyID body_box2; // fixed
 
static dBodyID body_wheel1;
static dJointID joint_hinge1;
static dGeomID geom_wheel1;
 
static dBodyID body_wheel2;
static dJointID joint_hinge2;
static dGeomID geom_wheel2;
 
dReal box_size[3] = { 1.0, 1.0, 1.0 };
 
dReal wheel1_r = 1.0;
dReal wheel1_w = 0.1;
 
dReal wheel2_r = 2.0;
dReal wheel2_w = 0.1;
 
#define STEP_SIZE 0.01
double time = 0;
 
double torque = 0.0;
 
 
// start simulation - set viewpoint
static void start()
{
    static float xyz[3] = { 2.f, -7.f, 12.f };
    static float hpr[3] = { 90.f, -20.f, 0.f };
 
    dsSetViewpoint( xyz, hpr );
}
 
// called when a key pressed
static void command (int cmd)
{
    if (cmd == 'a') {
        torque += 1.0;
        printf( "torque = %f\n", torque );
    }
    else if (cmd == 'd') {
        torque -= 1.0;
        printf( "torque = %f\n", torque );
    }
}
 
// simulation loop
static void simLoop( int pause )
{
    // Ctl+p が押されたらifに入らない
    if (!pause)
    {
        dJointAddHingeTorque( joint_hinge1, torque ); // wheel1のみにトルクを与える
 
        dWorldStep( world, STEP_SIZE );
        time += STEP_SIZE;
 
    }
 
    dsSetColor( 1.0f, 1.0f, 1.0f );
    dsDrawBox( dBodyGetPosition( body_box1 ), dBodyGetRotation( body_box1 ), box_size );
 
    dsSetColor( 1.0f, 1.0f, 1.0f );
    dsDrawBox( dBodyGetPosition( body_box2 ), dBodyGetRotation( body_box2 ), box_size );
 
    dsSetColor( 0.0f, 1.0f, 1.0f );
    dsDrawCylinder( dBodyGetPosition( body_wheel1 ), dBodyGetRotation( body_wheel1 ), wheel1_w, wheel1_r );
 
    dsSetColor( 1.0f, 0.0f, 1.0f );
    dsDrawCylinder( dBodyGetPosition( body_wheel2 ), dBodyGetRotation( body_wheel2 ), wheel2_w, wheel2_r );
}
 
 
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 );
 
    space = dHashSpaceCreate( 0 );
 
 
    // box creating
    {
        body_box1 = dBodyCreate( world );
        dReal pos[3] = { 0.0, 0.0, 10.0 };
        dBodySetPosition( body_box1, pos[0], pos[1], pos[2] );
 
        // boxの固定
        dJointID fixed;
        fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, NULL, body_box1 );
        dJointSetFixed( fixed );
    }
 
    // wheel1 creating
    {
        dReal pos[3] = { 0.0, -1.0, 10.0 };
        body_wheel1 = dBodyCreate( world );
        dBodySetPosition( body_wheel1, pos[0], pos[1], pos[2] );
 
        geom_wheel1 = dCreateCylinder( space, wheel1_r, wheel1_w );
        dGeomSetBody( geom_wheel1, body_wheel1 );
 
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, M_PI/180.0*90.0 );
        dGeomSetRotation( geom_wheel1, R );
 
        // ヒンジジョイント(wheel1回転軸)
        joint_hinge1 = dJointCreateHinge( world, 0 );
        dJointAttach( joint_hinge1, body_box1, body_wheel1 );
        dJointSetHingeAnchor( joint_hinge1, pos[0], pos[1], pos[2] );
        dJointSetHingeAxis( joint_hinge1, 0.0, 1.0, 0.0 );
    }
 
 
    // box creating
    {
        body_box2 = dBodyCreate( world );
        dReal pos[3] = { 3.0, 0.0, 10.0 };
        dBodySetPosition( body_box2, pos[0], pos[1], pos[2] );
 
        // boxの固定
        dJointID fixed;
        fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, NULL, body_box2 );
        dJointSetFixed( fixed );
    }
 
    // wheel2 creating
    {
        dReal pos[3] = { 3.0, -1.0, 10.0 };
        body_wheel2 = dBodyCreate( world );
        dBodySetPosition( body_wheel2, pos[0], pos[1], pos[2] );
 
        geom_wheel2 = dCreateCylinder( space, wheel2_r, wheel2_w );
        dGeomSetBody( geom_wheel2, body_wheel2 );
 
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, M_PI/180.0*90.0 );
        dGeomSetRotation( geom_wheel2, R );
 
        // ヒンジジョイント(wheel2回転軸)
        joint_hinge2 = dJointCreateHinge( world, 0 );
        dJointAttach( joint_hinge2, body_box2, body_wheel2 );
        dJointSetHingeAnchor( joint_hinge2, pos[0], pos[1], pos[2] ); // boxと同じ座標
        dJointSetHingeAxis( joint_hinge2, 0.0, 1.0, 0.0 );
    }
 
    // 接触点の生成
    {
        dContact contact;
        contact.geom.depth = 0.0;
        contact.geom.pos[0] = 1.0;
        contact.geom.pos[1] = 0.0;
        contact.geom.pos[2] = 10.0;
        contact.geom.normal[0] = 1.0;
        contact.geom.normal[1] = 0.0;
        contact.geom.normal[2] = 0.0;
        contact.surface.mode = dContactBounce;
        contact.surface.mu = dInfinity;
        contact.surface.bounce = 0.0;
        dJointID c = dJointCreateContact( world, 0, &contact );
        dJointAttach( c, body_wheel1, body_wheel2 ); 
    }
 
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    dWorldDestroy( world );
    dCloseODE();
    return 0;
}
 
 

ポイント

ポイントはdJointCreateContactを静的に作るところです。

   {
       dContact contact;
       contact.geom.depth = 0.0;
       contact.geom.pos[0] = 1.0;
       contact.geom.pos[1] = 0.0;
       contact.geom.pos[2] = 10.0;
       contact.geom.normal[0] = 1.0;
       contact.geom.normal[1] = 0.0;
       contact.geom.normal[2] = 0.0;
       contact.surface.mode = dContactBounce;
       contact.surface.mu = dInfinity;
       contact.surface.bounce = 0.0;
       dJointID c = dJointCreateContact( world, 0, &contact );
       dJointAttach( c, body_wheel1, body_wheel2 ); 
   }

• depth：接触点の深さ（ここではホイールの面と面で接触していると仮定して0を設定）
• pos：接触点の位置
• normal：衝突の法線ベクトル（垂直なベクトル）
• surface：摩擦とかすべりとかを設定

上記の部分をコメントして一方のホイールを回転させてみると、もう片方は回らないと思います。

注意
現状(ODEv0.11.1)ではCylinder同士の衝突はサポートしていないようです。よって、ｄCollideによる衝突検出で自動でposやnormalを求めるといったやり方はできなさそうです。
(かなり残念）
そのうちサポートされることを祈ります。

まとめ

通常は衝突に使うコンタクトジョイントを反則的？にホイール同士の接触に適用することでホイール回転の伝達が可能になりました。

この方法が正攻法であれば、いろいろと応用が利きそうです。

こういうのを発見するのは楽しいですね。