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

■環境
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] ブロックを落とす

```cpp: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用のデモプログラムセットにあるプロジェクトのコードを１つ潰して、まるっと書き換えることでどうにかしました。

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