ODE/振り子 - bambooflow Note

振り子

ここでは、ボールジョイントを用いた振り子を紹介します。
メモ書きです。

サンプル

test_furiko.zip

振り子は球体ではなくあえて箱型にしました。（方向がわかるので）
ジョイントに対して特になにも制約を与えていないので同じ高さまで振れます。

ソースコード

物体の固定２に振り子なるboxとボールジョイントを追加しています。

物体に質量設定してないので、違和感ありありですがサンプルということで。

#ifdef WIN32
#include <windows.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
 
static dWorldID world;
 
static dBodyID body_box1;
static dBodyID body_box2;
dReal  box_sizes[3] = { 1.0, 1.0, 1.0 };
 
 
#ifdef dDOUBLE
  #define dsDrawBox dsDrawBoxD
#endif
 
 
// start simulation - set viewpoint
static void start()
{
  static float xyz[3] = { 0.f, -10.f, 10.f };
  static float hpr[3] = { 90.f, -15.f, 0.f };
 
  dsSetViewpoint( xyz, hpr );
}
 
// simulation loop
static void simLoop( int pause )
{
    // Ctl+p が押されたらifに入らない
    if (!pause)
    {
        dWorldStep( world, 0.01 );
    }
 
    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 = 0;
    fn.stop    = 0;
    fn.path_to_textures = "../drawstuff/textures";
 
    world = dWorldCreate();
    dWorldSetGravity( world, 0.0, 0.0, -9.8 );
 
 
    // box1 creating
    {
        body_box1 = dBodyCreate( world );
        dReal pos[3] = { 0.0, 0.0, 10.0 };
        dBodySetPosition( body_box1, pos[0], pos[1], pos[2] );
    }
 
    // box2 creating
    {
        body_box2 = dBodyCreate( world );
        dReal pos[3] = { 5.0, 0.0, 10.0 };
        dBodySetPosition( body_box2, pos[0], pos[1], pos[2] );
    }
 
    // box1の固定
    {
        dJointID fixed;
        fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, NULL, body_box1 );
        dJointSetFixed( fixed );
    }
 
    // ボールジョイント
    {
        dJointID joint_ball;
        joint_ball = dJointCreateBall( world, 0 );
        dJointSetBallAnchor( joint_ball, 0.0, 0.0, 10.0 ); // box1と同じ座標
        dJointAttach( joint_ball, body_box1, body_box2 );
 
    }
 
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    dWorldDestroy( world );
    dCloseODE();
    return 0;
}
 

フィードバック

test_furiko_feedback.zip

ボールジョイントのフィードバックを得る方法をメモします。

• box2の座標

const dReal* pos = dBodyGetPosition( body_box2 );
fprintf( fp, "%.3f\t%.3f\t%.3f\t", pos[0], pos[1], pos[2] );

• ボールジョイントの力とトルク

   // ボールジョイントからのフィードバック
   dJointFeedback *fb_ball = dJointGetFeedback( joint_ball );
   fprintf( fp, "%.3f\t%.3f\t%.3f\t", fb_ball->f1[0], fb_ball->f1[1], fb_ball->f1[2] );
   fprintf( fp, "%.3f\t%.3f\t%.3f\t", fb_ball->t1[0], fb_ball->t1[1], fb_ball->t1[2] );
   fprintf( fp, "%.3f\t%.3f\t%.3f\t", fb_ball->f2[0], fb_ball->f2[1], fb_ball->f2[2] );
   fprintf( fp, "%.3f\t%.3f\t%.3f\n", fb_ball->t2[0], fb_ball->t2[1], fb_ball->t2[2] );

まとめ

ボールジョイントの使い方を理解するためにメモとして用意しました。
今回のサンプルは振り子が永遠に振れます。
空気抵抗とか摩擦とかないので運動法則から言えば当然といえば当然です。

でも、自然的に止まるシミュレーションもほしい。
ただ、探してもボールジョイントに対して制約を与える関数が存在せず。
ジョイントに対して制約を与えるにはほかのジョイントで代用するしかなさそうです。（たぶん）
あとは、振り子自体に抵抗となる力を与えるとか。