LoginSignup
3
7

More than 5 years have passed since last update.

@ryo_grid(Ryo Kanbayashi)inオリィ研究所

コピペで倒立振子の物理シミュレーション with Open Dynamics Engineできた

Last updated at Posted at 2016-11-10

■環境
Windows10
Code::Blocks 16.01
gcc (MinGW)
ODE 0.13 (物理シミュレーションライブラリ)
Drawstuff (ODE付属の3Dグラフィック描画ライブラリ)

■コピペ元
http://2178.blog55.fc2.com/blog-category-4.html
こちらには実際に動作している動画も置いておられます。

■コード
#include<cmath> -> #include<math.h>
に変更。
インデントも見にくいので直しておきました。。。

※操作方法
[e] 前進
[d] 後退
[s] 左
[f] 右
[r] リセット
[space] ブロックを落とす

huriko.cpp
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <math.h>

#ifdef _MSC_VER
#pragma warning(disable:4244 4305)
#endif

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawCylinder dsDrawCylinderD
#endif

#define NUM 2

dWorldID world; // 動力学計算用ワールド
dSpaceID space; // 衝突検出用スペース
dGeomID ground; // 地面
dJointGroupID contactgroup;
dsFunctions fn;

typedef struct { // MyObject構造体
    dBodyID body; // ボディ(剛体)のID番号（動力学計算用）
    dGeomID geom; // ジオメトリのID番号(衝突検出計算用）
    double l,r,m; // 長さ[m], 半径[m]，質量[kg]
} MyObject;

MyObject wheel[NUM], leg[NUM], base, ball;
dJointID wheelJoint[NUM], legJoint[NUM];

dReal TIMESTEP=0.0;
dReal DT=0.01;

static const dReal BALL_R = 0.025;
static const dReal BALL_S[3] = {0.05,0.05,0.05};
static const dReal BALL_P[3] = {0.04, 0.0, 1.50};

dReal v_r = 0.0,v_l = 0.0, v_c = 0.0 ; // 右、左車輪、ロボットの中心の回転速度
dReal K1 = -800.0, K2 = -200.0, K3=20.0; //直立コントロールの係数
dReal GO_SPEED = 0.0, TURN_DIGREE= 0.5; // コントロールするときの速度

dReal BASE_THETA=0.0; // 本体の傾き
dReal BASE_M = 0.319; // 本体の質量
dReal BASE_S[3] = {0.04,0.07,0.11}; // 本体のサイズ
dReal LEG_M = 0.079; // 足の質量
dReal LEG_S[3] = {0.045,0.045,0.105}; // 足のサイズ
dReal BL_MAGIN = 0.04; // 本体と足ののりしろの長さ
dReal WH_R = 0.0415; // 車輪の半径
dReal WH_W = 0.015, WH_M = 0.030;
dReal START_X = 0, START_Y = 0, START_Z=0.15; // 初期位置

static void makeBall() {
    dMass m1;

    dReal ball_mass = 0.2;
    const dReal *rp;

    rp=dBodyGetPosition(base.body);

    ball.body = dBodyCreate(world);

    dMassSetZero(&m1);
    dMassSetBoxTotal(&m1,ball_mass,BALL_S[0], BALL_S[1], BALL_S[2]);
    dMassAdjust(&m1,ball_mass);
    dBodySetMass(ball.body,&m1);

    ball.geom = dCreateBox(space,BALL_S[0],BALL_S[1],BALL_S[2]);
    dGeomSetBody(ball.geom,ball.body);

    dBodySetPosition(ball.body,
    rp[0]+(rand()%10)/10.0 * 0.1-0.05,
    rp[1]+(rand()%10)/10.0 * 0.1-0.05,
    0.5);
}

static void destroyBall()
{
    dBodyDestroy(ball.body);
    dGeomDestroy(ball.geom);
}


static void destroyRobot()
{
    for (int i=0; i < NUM; i++){
        dJointDestroy(wheelJoint[i]);
        dJointDestroy(legJoint[i]);
        dBodyDestroy(wheel[i].body);
        dBodyDestroy(leg[i].body);
        dGeomDestroy(wheel[i].geom);
        dGeomDestroy(leg[i].geom);
    }
    dBodyDestroy(base.body);
    dGeomDestroy(base.geom);
}


static void makeRobot()
{
    dMatrix3 R;
    dMass mass;

    // 台車
    base.body = dBodyCreate(world);
    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass,BASE_M,BASE_S[0],BASE_S[1],BASE_S[2]);
    dBodySetMass(base.body,&mass);

    base.geom = dCreateBox(space,BASE_S[0],BASE_S[1],BASE_S[2]);
    dGeomSetBody(base.geom,base.body);
    dBodySetPosition(base.body,START_X,START_Y,START_Z);

    // 足
    for (int i = 0; i < NUM; i++){
        leg[i].body=dBodyCreate(world);
        dMassSetZero(&mass);
        dMassSetBoxTotal(&mass,LEG_M,LEG_S[0],LEG_S[1],LEG_S[2]);
        dBodySetMass(leg[i].body,&mass);
        leg[i].geom = dCreateBox(space,LEG_S[0],LEG_S[1],LEG_S[2]);
        dGeomSetBody(leg[i].geom, leg[i].body);
    }
    dBodySetPosition(leg[0].body,START_X,START_Y+BASE_S[1]/2.0+LEG_S[1]/2.0,
    START_Z-BASE_S[2]/2.0-LEG_S[2]/2.0+BL_MAGIN);
    dBodySetPosition(leg[1].body,START_X,START_Y-BASE_S[1]/2.0-LEG_S[1]/2.0,
    START_Z-BASE_S[2]/2.0-LEG_S[2]/2.0+BL_MAGIN);


    // 車輪
    dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
    for (int i = 0; i < NUM; i++) {
        wheel[i].body = dBodyCreate(world);

        dMassSetZero(&mass);
        dMassSetCylinderTotal(&mass,WH_M, 2, WH_R, WH_W);
        dBodySetMass(wheel[i].body,&mass);
        wheel[i].geom = dCreateCylinder(space, WH_R, WH_W);
        dGeomSetBody(wheel[i].geom, wheel[i].body);
        dBodySetRotation(wheel[i].body,R);
    }

    dBodySetPosition(wheel[0].body,START_X,START_Y+BASE_S[1]/2.0-WH_W/2.0,
    START_Z-BASE_S[2]/2.0-LEG_S[2]+BL_MAGIN);
    dBodySetPosition(wheel[1].body,START_X,START_Y-BASE_S[1]/2.0+WH_W/2.0,
    START_Z-BASE_S[2]/2.0-LEG_S[2]+BL_MAGIN);

    // ジョイント
    for (int i = 0; i < NUM; i++) {
        legJoint[i] = dJointCreateHinge(world,0);
        dJointAttach(legJoint[i], base.body, leg[i].body);
        wheelJoint[i] = dJointCreateHinge(world,0);
        dJointAttach(wheelJoint[i], leg[i].body, wheel[i].body);
        dJointSetHingeAxis(wheelJoint[i],0, 1, 0);
        dJointSetHingeAxis(legJoint[i],0, 1, 0);
        dJointSetHingeParam(legJoint[i], dParamLoStop, 0);
        dJointSetHingeParam(legJoint[i], dParamHiStop, 0);
    }

    dJointSetHingeAnchor(wheelJoint[0],
    START_X,
    START_Y+BASE_S[1]/2.0-WH_W/2.0,
    START_Z-BASE_S[2]/2.0-LEG_S[2]+BL_MAGIN);
    dJointSetHingeAnchor(wheelJoint[1],
    START_X,
    START_Y-BASE_S[1]/2.0+WH_W/2.0,
    START_Z-BASE_S[2]/2.0-LEG_S[2]+BL_MAGIN);

    dJointSetHingeAnchor(legJoint[0],
    START_X,
    START_Y+BASE_S[1]/2.0,
    START_Z-BASE_S[2]/2.0+BL_MAGIN/2.0);

    dJointSetHingeAnchor(legJoint[1],
    START_X,
    START_Y-BASE_S[1]/2.0,
    START_Z-BASE_S[2]/2.0+BL_MAGIN/2.0);
}

static void disturbance()
{
    destroyBall();
    makeBall();
}


static void restart()
{
    TIMESTEP=0.0;
    BASE_THETA=0.0;
    v_c=0.0;
    GO_SPEED=0.0;
    TURN_DIGREE=0.5;
    destroyRobot();
    dJointGroupDestroy(contactgroup);
    contactgroup = dJointGroupCreate(0);
    makeRobot();
}



static void command(int cmd)
{
    switch (cmd) {
        case 'e': GO_SPEED += 1.0; break;
        case 'd': GO_SPEED -= 1.0; break;
        case 's': TURN_DIGREE -= 0.025; break;
        case 'f': TURN_DIGREE += 0.025; break;
        case 'w': TURN_DIGREE = 0.5; GO_SPEED=0.0; break;
        case ' ': disturbance(); break;
        case 'r':restart();break; // リセット
        default: printf("\nInput e,d,s,f,r,w,[SPC] \n");break;
    }
}

static void control() {
    double fMax = 100; // 最大発揮トルク[Nm]

    dJointSetHingeParam(wheelJoint[0], dParamVel, v_l );
    dJointSetHingeParam(wheelJoint[0], dParamFMax, fMax);
    dJointSetHingeParam(wheelJoint[1], dParamVel, v_r );
    dJointSetHingeParam(wheelJoint[1], dParamFMax, fMax);
}

static void start()
{
    float xyz[3] = { 0.5f, -0.5f, 0.5f};
    float hpr[3] = { -230.0f, -30.0f, 0.0f};

    dsSetViewpoint(xyz,hpr);
    dsSetSphereQuality(2);
}

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
    int i,n;

    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);
    if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return;

    static const int N = 10;
    dContact contact[N];
    n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
    if (n > 0) {
        for (i=0; i < n; i++) {
            contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
            dContactSoftERP | dContactSoftCFM | dContactApprox1;
            contact[i].surface.mu = dInfinity;
            contact[i].surface.slip1 = 0.1;
            contact[i].surface.slip2 = 0.1;
            contact[i].surface.soft_erp = 0.8;
            contact[i].surface.soft_cfm = 1e-5;
            dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
            dJointAttach(c,b1,b2);
        }
    }
}

static void drawBall()
{
    dsSetColor(1.0,0.0,0.0);
    dsDrawBox(dGeomGetPosition(ball.geom),
    dGeomGetRotation(ball.geom),BALL_S);
}

static void drawRobot()
{
    dsSetColor(0.5,0.5,1.0);
    dsDrawBox(dGeomGetPosition(base.geom),
    dGeomGetRotation(base.geom),BASE_S);

    dsSetColor(0.0,0.0,1.0);
    for (int i=0; i < NUM; i++) {
        dsDrawBox(dBodyGetPosition(leg[i].body),
        dBodyGetRotation(leg[i].body),LEG_S);
    }

    dsSetColor(1.1,1.1,1.1);
    for (int i=0; i < NUM; i++) {
        dsDrawCylinder(dBodyGetPosition(wheel[i].body),
        dBodyGetRotation(wheel[i].body),WH_W,WH_R);
    }
}

static void jyroSensor(dReal *xv, dReal *yv, dReal *zv)
{
    const dReal *av;
    dReal result[3];
    av=dBodyGetAngularVel(base.body);
    dBodyVectorFromWorld(base.body, av[0], av[1], av[2], result);
    *xv=result[0];
    *yv=result[1];
    *zv=result[2];
}

static void controlErect(dReal s, dReal ds)
{
    v_c=v_c+( K1*s+ K2*ds +K3* (v_c-GO_SPEED) )*DT;
    if (v_c < -100.0 | 100 < v_c){
        v_c=0;
    }

    v_r=2*TURN_DIGREE*v_c;
    v_l=2*(1-TURN_DIGREE)*v_c;

    if (s > M_PI/6.0){
        v_r=0;
        v_l=0;
    }
    if (s < -M_PI/6.0){
        v_r=0;
        v_l=0;
    }
}

static void simLoop(int pause)
{
    dReal vx, vy, vz;
    control();
    dSpaceCollide(space,0,&nearCallback);
    dWorldStep(world, DT);
    TIMESTEP=TIMESTEP+DT;
    dJointGroupEmpty(contactgroup);
    drawBall();
    drawRobot();

    jyroSensor(&vx, &vy, &vz);
    BASE_THETA=BASE_THETA+vy*DT;

    controlErect(BASE_THETA, vy);
}

static void setDrawStuff() {
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.path_to_textures = "../../drawstuff/textures";
}

int main(int argc, char *argv[])
{
    dInitODE();
    setDrawStuff();

    world = dWorldCreate();
    space = dHashSpaceCreate(0);
    contactgroup = dJointGroupCreate(0);
    ground = dCreatePlane(space,0,0,1,0);

    dWorldSetGravity(world, 0, 0, -9.8);

    makeRobot();
    makeBall();

    dsSimulationLoop(argc,argv,640,480,&fn);

    dSpaceDestroy(space);
    dWorldDestroy(world);
    dCloseODE();

    return 0;
}

ビルド設定はよくわからないので、ODEのCodeBlocks用のデモプログラムセットにあるプロジェクトのコードを１つ潰して、まるっと書き換えることでどうにかしました。

これからコード読む。。。

3
7
2

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
3
7