ODE/アナログ上皿はかり - bambooflow Note

アナログ上皿はかり（実験）

実験的に、秤を作ってみました。
スライダーによるスプリング・ダンパーの応用になります。
ギアの接触点が微妙なんですがとりあえず面白そうだったので作成してみました。

ギアのところは、正攻法とは言えないので今回は実験ものとしました。
メモ書きです。

上に乗っている球体は5Kgのものです。
量るとメーターがおよそ1/4回転します。

設計仕様

質量20Kgのものを乗せるとおおよそ針が1回転するように設計しています。

• 上皿に20kg乗せるとが1m下がるようにばね定数を設定

フックの法則より、

F = -k*x

このとき、Fは力[N]、kはばね定数[N/m]、xはバネの伸びた長さ[m]
重力を-9.8[m/s^2]としたとすると、20kg乗せたとき1m伸びるときのばね定数は、

F = -k*x　、 -9.8*20 = -k*1

より

k = 196 [N/m]

となります。

次にギアの半径を決めます。

• 上皿が1m下がったときに針が1回転

針の回転について、上皿が1m下がったときに針が1回転するように考えたとき、ギアの円周は1mとすれば良いことになります。
よって、

2π*r = 1
r = 1 / 2π
  = 約 0.159 [m]

ソースコード

↓ベタ書きでかなりみっともないですが。。。

#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;
static dJointGroupID gearjointgroup;
 
const dReal step_size = 0.01;
 
ST_Object* main_box = NULL;   // 本体
ST_Object* dialplate = NULL;  // 文字盤
ST_Object* pole = NULL;       // ポール
ST_Object* bowl = NULL;       // 皿
ST_Object* gear0 = NULL;      // ギア0
ST_Object* needle = NULL;     // 目盛針
 
ST_Object* ball = NULL;       // 錘
std::vector<ST_Object*> drawObjList;
 
dJointID dummy_contact0;  // ポールとギア0の接点位置（ダミー）
 
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 )
{
 
    dBodyID b1 = dGeomGetBody( o1 ); // 物体1
    dBodyID b2 = dGeomGetBody( o2 ); // 物体2
 
    if ( b1 && b2 && dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
        return; // 衝突対象でない物体の衝突ははずす
 
    unsigned long cat1 = dGeomGetCategoryBits(o1);
    unsigned long cat2 = dGeomGetCategoryBits(o2);
    unsigned long col1 = dGeomGetCollideBits(o1);
    unsigned long col2 = dGeomGetCollideBits(o2);
 
    if ((cat1 & col2) || (cat2 & col1)) {
 
        const int MAX_CONTACTS = 4; // 最大の衝突検出可能数
        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 );
            }
        }
    }
}
 
 
 
// start simulation - set viewpoint
static void start()
{
    static float xyz[3] = { 0.f, -5.0f, 2.f };
    static float hpr[3] = { 90.f, -15.f, 0.f };
 
    dsSetViewpoint( xyz, hpr );
}
 
 
void createHakari( void )
{
    dReal mx = 0.0;
    dReal my = 0.0;
    dReal mz = 0.0;
 
    dReal comm_width = 0.2;
 
    dReal gear0_radius = 0.159;
 
    // 本体
    {
        main_box = new ST_Object;
 
        dReal pos[3] = { mx+0.0, my+0.0, mz+0.5 };
        dReal size[3] = { 1.0, 1.0, 1.0 };
 
        // body setting
        main_box->body = dBodyCreate( world );
        dBodySetPosition( main_box->body, pos[0], pos[1], pos[2] );
        // geom setting
        main_box->geom = dCreateBox( space, size[0], size[1], size[2] );
        dGeomSetBody( main_box->geom, main_box->body );
        // mass setting
        dMass mass;
        dMassSetBoxTotal( &mass, 1.0, size[0], size[1], size[2] ); // 1kg
        dBodySetMass( main_box->body, &mass );
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 0.0*M_PI/180 );
        dBodySetRotation( main_box->body, R );
 
        main_box->setColor( 0.0, 1.0, 1.0, 0.5 );
 
    }
    // 文字盤
    {
        dialplate = new ST_Object;
 
        dReal radius = 0.5;
        dReal width = 0.1;
        dReal pos[3] = { mx+0.0, my-0.5-(width*0.5), mz+0.5 };
 
        // body setting
        dialplate->body = dBodyCreate( world );
        dBodySetPosition( dialplate->body, pos[0], pos[1], pos[2] );
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 90.0*M_PI/180 );
        dBodySetRotation( dialplate->body, R );
        // mass setting
        dMass mass;
        dMassSetCylinderTotal( &mass, 0.3, 1, radius, width ); // 0.3kg
        dMassRotate( &mass, R );
        dBodySetMass( main_box->body, &mass );
 
        // geom setting
        dialplate->geom = dCreateCylinder( space, radius, width );
        dGeomSetBody( dialplate->geom, dialplate->body );
 
        dialplate->setColor( 1.0, 1.0, 1.0, 0.5 );
 
        // 固定
        dJointID fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, dialplate->body, main_box->body );
        dJointSetFixed( fixed );
 
    }
    // ポール
    {
        pole = new ST_Object;
 
        dReal pole_width = 0.1;
        dReal pos[3] = { mx+(pole_width*0.5)+gear0_radius, my+0.0, mz+1.0 };
        dReal size[3] = { pole_width, comm_width, 1.0 };
 
        // body setting
        pole->body = dBodyCreate( world );
        dBodySetPosition( pole->body, pos[0], pos[1], pos[2] );
        // mass setting
        dMass mass;
        dMassSetBoxTotal( &mass, 0.001, size[0], size[1], size[2] ); // 1g
        dBodySetMass( pole->body, &mass );
 
        // geom setting
        pole->geom = dCreateBox( space, size[0], size[1], size[2] );
        dGeomSetBody( pole->geom, pole->body );
 
        dGeomSetCategoryBits( pole->geom, 0 );
        dGeomSetCollideBits( pole->geom, 0 );
 
        pole->setColor( 0.5, 0.5, 0.5, 1.0 );
 
        // 軸生成
        dJointID slider = dJointCreateSlider( world, 0 );
        dJointAttach( slider, pole->body, main_box->body );
        dJointSetSliderAxis( slider, 0.0, 0.0, 1.0 );
 
        //////////////////////////////////////////////////////////
        // バネ・ダンパー設定
        dReal h = step_size;
        dReal kp = 196;   // ばね定数 (spring constant)
        dReal kd = 50.0;  // 減衰定数 (damping constant)
        dReal erp = h*kp / (h*kp + kd );
        dReal cfm = 1.0 / (h*kp + kd);
 
        dJointSetSliderParam( slider, dParamLoStop, 0.0 ); // ばねの自然長位置
        dJointSetSliderParam( slider, dParamHiStop, 0.0 ); // ばねの自然長位置
 
        dJointSetSliderParam( slider, dParamStopERP, erp );
        dJointSetSliderParam( slider, dParamStopCFM, cfm );
        //////////////////////////////////////////////////////////
    }
    // 皿
    {
        bowl = new ST_Object;
 
        dReal pos[3] = { mx+0.0, my+0.0, mz+1.55 };
        dReal size[3] = { 1.0, 1.0, 0.1 };
 
        // body setting
        bowl->body = dBodyCreate( world );
        dBodySetPosition( bowl->body, pos[0], pos[1], pos[2] );
        // mass setting
        dMass mass;
        dMassSetBoxTotal( &mass, 0.001, size[0], size[1], size[2] ); // 1g
        dBodySetMass( bowl->body, &mass );
 
        // geom setting
        bowl->geom = dCreateBox( space, size[0], size[1], size[2] );
        dGeomSetBody( bowl->geom, bowl->body );
 
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 0.0*M_PI/180 );
        dBodySetRotation( bowl->body, R );
 
        bowl->setColor( 1.0, 1.0, 1.0, 1.0 );
 
        // 固定
        dJointID fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, bowl->body, pole->body );
        dJointSetFixed( fixed );
 
    }
 
    // ギア0
    {
        gear0 = new ST_Object;
 
        dReal pos[3] = { mx+0.0, my+0.0, mz+0.5 };
        dReal radius = 0.159;
        dReal width = 0.2;
 
        // body setting
        gear0->body = dBodyCreate( world );
        dBodySetPosition( gear0->body, pos[0], pos[1], pos[2] );
        // geom setting
        gear0->geom = dCreateCylinder( space, gear0_radius, comm_width );
        dGeomSetBody( gear0->geom, gear0->body );
 
        dGeomSetCategoryBits( gear0->geom, 0 );
        dGeomSetCollideBits( gear0->geom, 0 );
 
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 1.0, 0.0, 0.0, 90.0*M_PI/180 );
        dBodySetRotation( gear0->body, R );
 
        gear0->setColor( 1.0, 0.0, 0.0, 0.5 );
 
        // 軸生成
        dJointID hinge = dJointCreateHinge( world, 0 );
        dJointAttach( hinge, gear0->body, main_box->body );
        dJointSetHingeAnchor( hinge, pos[0], pos[1], pos[2] );
        dJointSetHingeAxis( hinge, 0.0, 1.0, 0.0 );
 
    }
 
    // ポールとギア0 間の接点位置 保持用 (ダミー)
    {
        dReal pos[3] = { mx+gear0_radius, my+0.0, mz+0.5 };
 
        // body setting (ダミー)
        dBodyID dummy_body = dBodyCreate( world );
        dBodySetPosition( dummy_body, pos[0], pos[1], pos[2] );
        // mass setting
        dMass mass;
        dMassSetSphereTotal( &mass, 0.0001, 0.001 ); // 0.1g
        dBodySetMass( dummy_body, &mass );
 
        // ダミーの接点
        dummy_contact0 = dJointCreateHinge( world, 0 );
        dJointAttach( dummy_contact0, dummy_body, main_box->body );
        dJointSetHingeAnchor( dummy_contact0, pos[0], pos[1], pos[2] ); // アンカーは接点の位置に使う
        dJointSetHingeAxis( dummy_contact0, -1.0, 0.0, 0.0 );            // 軸は接点のnormalで使う
    }
 
    // 目盛針
    {
        needle = new ST_Object;
 
        dReal pos[3] = { mx, my-0.65, mz+0.5 };
        dReal length = 0.4;
 
        // body setting
        needle->body = dBodyCreate( world );
        dBodySetPosition( needle->body, pos[0], pos[1], pos[2] );
 
        // rotation
        dMatrix3 R;
        dRFromAxisAndAngle( R, 0.0, 1.0, 0.0, 0.0*M_PI/180 );
        dBodySetRotation( needle->body, R );
 
        // geom setting
        needle->geom = dCreateRay( space, length );
        dGeomSetBody( needle->geom, needle->body );
 
        dGeomSetCategoryBits( needle->geom, 0 );
        dGeomSetCollideBits( needle->geom, 0 );
 
        needle->setColor( 1.0, 1.0, 1.0, 1.0 );
 
        // 固定
        dJointID fixed = dJointCreateFixed( world, 0 );
        dJointAttach( fixed, needle->body, gear0->body );
        dJointSetFixed( fixed );
    }
 
    drawObjList.push_back( needle );
    drawObjList.push_back( bowl );
    drawObjList.push_back( pole );
    drawObjList.push_back( gear0 );
    drawObjList.push_back( main_box );
    drawObjList.push_back( dialplate );
}
 
void updateGearContact()
{
    dJointGroupEmpty( gearjointgroup );
 
    // ポールとギアの接触点を更新
    if (dummy_contact0 && gear0 && pole) {
        dVector3 c0_pos;
        dVector3 c0_axis;
        dJointGetHingeAnchor( dummy_contact0, c0_pos );
        dJointGetHingeAxis( dummy_contact0, c0_axis );
 
        //printf( "pos    = %f, %f, %f\n", c0_pos[0], c0_pos[1], c0_pos[2] );
        //printf( "normal = %f, %f, %f\n", c0_axis[0], c0_axis[1], c0_axis[2] );
 
        dContact contact;
        contact.geom.depth = 0.0;
        contact.geom.pos[0] = c0_pos[0];
        contact.geom.pos[1] = c0_pos[1];
        contact.geom.pos[2] = c0_pos[2];
        contact.geom.normal[0] = c0_axis[0];
        contact.geom.normal[1] = c0_axis[1];
        contact.geom.normal[2] = c0_axis[2];
        contact.surface.mode = dContactBounce | dContactSoftERP | dContactSoftCFM;
        contact.surface.mu = dInfinity;
        contact.surface.bounce = 0.0; 
        contact.surface.soft_erp = 0.3; 
        contact.surface.soft_cfm = 0.001; 
        dJointID c = dJointCreateContact( world, gearjointgroup, &contact );
        dJointAttach( c, gear0->body, pole->body ); 
    }
}
 
 
 
// simulation loop
static void simLoop( int pause )
{
    // Ctl+p が押されたらifに入らない
    if (!pause)
    {
        dSpaceCollide( space, 0, &nearCallback ); // 衝突検出
 
        updateGearContact();
 
        dWorldStep( world, step_size );
 
        dJointGroupEmpty( contactgroup );
    }
 
    // debug
    if (!pause) {
        const dReal* pole_pos = dBodyGetPosition( pole->body );
        printf( " pole height = %f\n", -1.0+pole_pos[2] );
    }
 
    // 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 );
 
    gearjointgroup = dJointGroupCreate( 0 );
 
    dCreatePlane( space, 0, 0, 1, 0 );
 
 
    createHakari();
 
 
    // 錘
    {
        ball = new ST_Object;
 
        dReal radius = 0.5;
        dReal pos[3] = { 0.0, 0.0, 3.0 };
 
        // body setting
        ball->body = dBodyCreate( world );
        dBodySetPosition( ball->body, pos[0], pos[1], pos[2] );
        // mass setting
        dMass mass;
        dMassSetSphereTotal( &mass, 5.0, radius); // Kg
        dBodySetMass( ball->body, &mass );
 
        // geom setting
        ball->geom = dCreateSphere( space, radius );
        dGeomSetBody( ball->geom, ball->body );
 
        ball->setColor( 1.0, 1.0, 0.0 );
 
        drawObjList.push_back( ball );
    }
 
 
 
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    dWorldDestroy( world );
    dCloseODE();
    return 0;
}
 

メモ書き

• ばね定数の設定

ばね定数は196[N/m]にしたいため、支柱のスライダーの設定を次のようにしました。
このとき、減衰定数は適当に設定しています。

       //////////////////////////////////////////////////////////
       // バネ・ダンパー設定
       dReal h = step_size;
       dReal kp = 196;   // ばね定数 (spring constant)
       dReal kd = 50.0;  // 減衰定数 (damping constant)
       dReal erp = h*kp / (h*kp + kd );
       dReal cfm = 1.0 / (h*kp + kd);

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

       dJointSetSliderParam( slider, dParamStopERP, erp );
       dJointSetSliderParam( slider, dParamStopCFM, cfm );
       //////////////////////////////////////////////////////////

• ギアと支柱の接触点の位置と垂直ベクトルの求め方

接触点の位置と垂直ベクトル(normal)を求めるのに、ダミーのhingeを作成してそのアンカー位置と軸方向を取得して利用しました。
本来であれば、計算して求めるのが良いのですが、計算が苦手でだったのでこんな方法をとりました。

まとめ

今回、ERPとCFMの設定について、ODEのマニュアル通りにばね・ダンパーの計算を用いることでフックの法則が当てはまることが確認できました。

ギアと上皿を支える支柱との接点を無理矢理にContactJointを作っています。
とりあえず、はかりをそれ程動かさないようならば問題ないのですが、大きく傾けたりしてしまうとはかりがバラバラに分解してしまう不具合があります。
なにか解決方法があれば。。。