ODEで学ぶＣ言語　[Step7: まとめ]

Step7: 一本足ロボット

ODEで学ぶC言語のStep7です．Step6までで一通りC言語の項目が終わったので今回はまとめの演習です．プラス，ODEのジョイントの生成法と簡単な制御のサンプルプログラムを紹介します．

ジョイントは我々の周りでは、折畳み携帯のヒンジやドアの蝶番に相当します。小難しくいうと、２つのボディの位置や姿勢をある一定の関係に保つ拘束がジョイントとなのです。ODEではジョイントと拘束を同じ意味で使っています。

ODEのジョイント（関節）は２つのボディ(剛体，body)をつなげるものです。１つのジョイントで３個以上のボディをつなげることはできませんし，１個のボディだけをつなげることもできません．必ず２個のボディを１つのジョイントでつなげなければなりません．

また，ODEのジョイントは摩擦がありませんし，可動域を設定しないと+dInfinity(+無限大)から-dInfinity（-無限大）まで回転または移動します．角度は[rad]です．

• ジョイントの使い方

1. ***ジョイントの生成　　　　　　dJointCreate***()
2. ***ジョイントとボディの結合　 dJointAttach(dJointID, dBodyID, dBodyID)
3. ***ジョイントの中心点を設定　dJointSet***Anchor()
4. ***ジョイントの回転軸を設定　dJointSet***Axis()

上で***にはジョイントのタイプが入ります。タイプにはHinge(ヒンジ）、Ball(ボール)、Slider(スライダー)、Universal(ユニバーサル)等があります。サンプルプログラムではヒンジジョイントと直動式のスライダージョイントを使います．

• ジョイントのパラメータ設定

1. 可動域の設定
   dJointSetHingeParam(dJointID, dParamLoStop, 可動域の下限）;
   dJointSetHingeParam(dJointID, dParamHiStop, 可動域の上限）;
2. 目標角速度とそれを実現するための最大トルクの設定
   dJointSetHingeParam(dJointID, dParamVel, 目標角速度）;
   dJointSetHingeParam(dJointID, dParamFMax, 最大トルク）;

パラメータとしては、関節可動域の下限を示すdParamLoStop、上限を示すdParamHiStop、角速度（ヒンジジョイント）または速度(直動式関節）を示すdParamVel、最大トルクを示すdParaFMaxなどがあります。なお、ODEでは関節にモータが標準で組み込まれているので、dParamVelやdParaFMaxを指定すると関節が動きます。

ソースコード
少し長くなりますが，ソースコードを掲載します．このコードは今まで習った，キー操作関数，シミュレーションの再スタート，ボディへ力を加えるなどのほかに，ジョイントの生成と制御法など簡単なゲームやシミュレータを作るために必要なことは一通り入っています．

/* step7　*/
#include "dm7.h"

#define START_X 0.0　// 初期位置のｘ座標
#define START_Y 0.0
#define START_Z 1.5

dWorldID world;  // 動力学の世界
dSpaceID space;  // 衝突検出用スペース
dGeomID  ground; // 地面
dJointGroupID contactgroup; // コンタクトグループ

typedef struct
{
    dBodyID body; // ボディのID
    dGeomID geom; // ジオメトリのID
    dJointID joint; // ジョイントのID
    double pos[3]; // x, y, z　[m]
    double *R;   // 回転行列 要素数4x3
    double sides[3]; // ボックスのサイズ
    double l, r, m; // 直径[m], 半径 [m], 質量 [kg]
    float color[3]; // 色 r,g,b
} MyObject;

MyObject torso, leg[2];　// 胴体，脚
static int STEPS = 0;      // シミュレーションのステップ数
double S_LENGTH = 0.0;     // スライダー長
double H_ANGLE  = 0;       //ヒンジの角度

// コールバック関数
void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
    static const int N = 10; // 接触点数の最大値
    dContact contact[N];     // 接触点

    // 接触している物体のどちらかが地面ならisGroundに非0をセット
    int isGround = ((ground == o1) || (ground == o2));

    // 2つのボディがジョイントで結合されていたら衝突検出しない
    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);
    if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return;

    // 衝突情報の生成 nは衝突点数
    int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
    if (isGround)
    {
        for (int i = 0; i < n; i++)
        {
            contact[i].surface.mode = dContactBounce; // 接触面の反発性を設定
            contact[i].surface.mu   = dInfinity;      // 摩擦係数
            contact[i].surface.bounce = 0.5;          // 反発係数(0.0から1.0)
            contact[i].surface.bounce_vel = 0.0;      // 反発に必要な最低速度

            // 接触ジョイントの生成
            dJointID c = dJointCreateContact(world,contactgroup,
                                             &contact[i]);
            // 接触している２つの剛体を接触ジョイントにより拘束
            dJointAttach(c,dGeomGetBody(contact[i].geom.g1),
                         dGeomGetBody(contact[i].geom.g2));
        }
    }
}

void dmDraw(MyObject obj) /*** 物体の描画 ***/
{
    const double *pos, *R;

    pos = dGeomGetPosition(obj.geom); // 位置を取得
    R   = dGeomGetRotation(obj.geom); // 姿勢を取得

    dsSetColor(obj.color[0],obj.color[1],obj.color[2]);  // 色の設定(r,g,b)

    int type = dGeomGetClass(obj.geom);

    if (type == dBoxClass)
    {
        dsDrawBox(pos,R,obj.sides);
    }
    else if (type == dSphereClass)
    {
        dsDrawSphere(pos,R,obj.r);
    }
    else if (type == dCapsuleClass)
    {
        dsDrawCapsule(pos,R,obj.l,obj.r);
    }
    else if (type == dCylinderClass)
    {
        dsDrawCylinder(pos,R,obj.l,obj.r);
    }
}

void dmSphereCreate(MyObject *obj,double p[3], double R[12],
                    double m, double r, float color[3])
{
    int i;

    obj->m = m;
    obj->r = r;
    obj->R = R;

    for (i = 0; i < 3; i++)
    {
        obj->pos[i] = p[i];
        obj->color[i] = color[i];
    }

    dRSetIdentity(obj->R);
    obj->body = dBodyCreate(world);           // ボールの生成
    dMass mass;                               // 構造体massの宣言
    dMassSetZero(&mass);                      // 構造体massの初期化
    dMassSetSphereTotal(&mass,obj->m,obj->r); // 構造体massに質量を設定
    dBodySetMass(obj->body,&mass);            // ボールにmassを設定
    dBodySetPosition(obj->body,obj->pos[0],obj->pos[1],obj->pos[2]); // ボールの位置(x,y,z)を設定

    obj->geom = dCreateSphere(space,obj->r); // 球ジオメトリの生成
    dGeomSetBody(obj->geom, obj->body);      // ボディとジオメトリの関連付け
}

void dmCapsuleCreate(MyObject *obj,double p[3], double R[12],
                     double m, double r, double l, float color[3])
{
    int i;

    obj->m = m;
    obj->l = l;
    obj->r = r;
    obj->R = R;

     for (i = 0; i < 3; i++)
    {
        obj->pos[i]   = p[i];
        obj->color[i] = color[i];
    }

    dRSetIdentity(obj->R);
    obj->body = dBodyCreate(world);           // 剛体の生成
    dMass mass;                               // 構造体massの宣言
    dMassSetZero(&mass);                      // 構造体massの初期化
    dMassSetCapsuleTotal(&mass,obj->m,
                         3, obj->r, obj->l); // 構造体massに質量を設定
    dBodySetMass(obj->body,&mass);            // 剛体にmassを設定
    dBodySetPosition(obj->body,obj->pos[0],obj->pos[1],obj->pos[2]); // 剛体の位置(x,y,z)を設定

    obj->geom = dCreateCylinder(space,obj->r, obj->l); // ボックスジオメトリの生成
    dGeomSetBody(obj->geom, obj->body);      // ボディとジオメトリの関連付け
}

// ロボットの生成
void createMonoBot()
{
    int i;
    double l_leg = 0.75; // 長さ
		double m_leg = 1.0;  // 質量
		double r_leg[2] = {0.05, 0.03}; // 半径
    double m_torso = 10.0;
    double r_torso = 0.25;
		double p_torso[3] = {START_X, START_Y, START_Z};  // 位置
    double p_leg[2][3] = {{START_X, START_Y, START_Z-0.5*l_leg},
			{START_X, START_Y, START_Z-0.5*l_leg-0.5}};
    double p_anchor[3] = {START_X, START_Y, START_Z};
    double R[12];   // 回転行列

    float  color_torso[3] = {1.0, 0.0, 0.0}; // 色
    float  color_leg[2][3] = {{0.0, 0.0, 1.0},{1.3, 1.3, 1.3}};

    dRSetIdentity(R);  //回転行列を単位行列で初期化

    dmSphereCreate(&torso,p_torso,R,m_torso,r_torso,color_torso); // 胴体の生成

    for (i = 0; i < 2; i++)
    {
        dmCapsuleCreate(&leg[i],p_leg[i],R,m_leg,r_leg[i],l_leg,color_leg[i]); // 脚の生成
    }

    // ヒンジジョイント
    leg[0].joint = dJointCreateHinge(world, 0);　// ヒンジジョイントの生成
    dJointAttach(leg[0].joint, torso.body,leg[0].body); // 2つのボディとの結合
    dJointSetHingeAnchor(leg[0].joint, p_anchor[0],p_anchor[1],p_anchor[2]);　// ジョイント中心の設定
    dJointSetHingeAxis(leg[0].joint, 1, 0, 0); // 回転軸ベクトルの設定

    // スライダージョイント
    leg[1].joint = dJointCreateSlider(world, 0); // スライダジョイントの生成
    dJointAttach(leg[1].joint, leg[0].body,leg[1].body); // 2つのボディとの結合
    dJointSetSliderAxis(leg[1].joint, 0, 0, 1);　// スライダの軸ベクトルの設定
    dJointSetSliderParam(leg[1].joint, dParamLoStop, -0.25); // スライダの可動域　下限
    dJointSetSliderParam(leg[1].joint, dParamHiStop,  0.25); // スライダの可動域　上限
}

// ヒンジジョイントの制御
static void controlHinge(dReal target)
{
    static dReal kp = 10.0, fmax = 1000;

    dReal tmp   = dJointGetHingeAngle(leg[0].joint);
    dReal diff  = target - tmp;
    if (diff >=   M_PI) diff -= 2.0 * M_PI; // diffが2πより小さく
    if (diff <= - M_PI) diff += 2.0 * M_PI; // diffが-2πより大きく
    dReal u     = kp * diff;

    dJointSetHingeParam(leg[0].joint, dParamVel,  u);
    dJointSetHingeParam(leg[0].joint, dParamFMax, fmax);
}

// スライダの制御 プログラム2.4
static void controlSlider(dReal target)
{
    static dReal kp   = 25.0;                       // 比例定数
    static dReal fmax = 1000;                        // 最大力[N]
    double tmp, u;

    tmp  = dJointGetSliderPosition(leg[1].joint);  // スライダの現在位置
    u    = kp * (target - tmp);                    // 残差
    dJointSetSliderParam(leg[1].joint, dParamVel,  u);
    dJointSetSliderParam(leg[1].joint, dParamFMax, fmax);
}

// ロボットの破壊
void destroyMonoBot()
{
    int i;

    dJointDestroy(leg[0].joint);   // ヒンジ
    dJointDestroy(leg[1].joint);   // スライダー
    dBodyDestroy(torso.body); // 胴体のボディを破壊
    dGeomDestroy(torso.geom); // 胴体のジオメトリを破壊

    for (i = 0; i < 2; i++)
    {
        dBodyDestroy(leg[i].body);  // 脚のボディを破壊
        dGeomDestroy(leg[i].geom);  // 脚のジオメトリを破壊
    }
}

// シミュレーションの再スタート
static void restart()
{
    STEPS    = 0;      // ステップ数の初期化
    H_ANGLE  = 0.0;    // ヒンジ角度の初期化

    destroyMonoBot();  // ロボットの破壊
    dJointGroupDestroy(contactgroup);     // ジョイントグループの破壊
    contactgroup = dJointGroupCreate(0);  // ジョイントグループの生成
    createMonoBot();                      // ロボットの生成
}

// キー操作
void command(int cmd)
{
    switch (cmd)
    {
    case 'j':
        S_LENGTH =   0.25;        break;
    case 'f':
        S_LENGTH = - 0.25;        break;
    case 'k':
        H_ANGLE +=   0.25;
        if (H_ANGLE  >   M_PI) H_ANGLE  =  -M_PI;        break;
    case 'd':
        H_ANGLE -=   0.25;
        if (H_ANGLE  <  -M_PI) H_ANGLE  =   M_PI;        break;
    case 'u':
        dBodyAddForce(torso.body, 0, 0, 500);        break;
    case 'r':
        restart();        break;
    default :
        printf("key missed \n");        break;
    }
}

void simLoop(int pause)           /***  シミュレーションループ　***/
{
    int i;
    int s = 1000;                   // 跳躍する周期(ステップ)

    if (!pause)
    {
				printf("STEPS=%d \n",STEPS++); //  ステップ数

        // スライダーの伸縮
        if ((0 <= (STEPS%s)) && ((STEPS%s) <= 10)) S_LENGTH = 0.6;
        else if ((11 <= (STEPS%s)) && ((STEPS%s) <= 15)) S_LENGTH = 0.0;

        controlSlider(S_LENGTH); // スライダージョイントの制御
        controlHinge(H_ANGLE);   // ヒンジジョイントの制御

        dSpaceCollide(space,0,&nearCallback); // 衝突検出関数
        dWorldStep(world,0.01); // シミュレーションを１ステップ進める
        dJointGroupEmpty(contactgroup); // ジョイントグループを空にする
    }

    // ロボットの描画
    dmDraw(torso);   // 胴体の描画
    for (i = 0; i < 2; i++)dmDraw(leg[i]); //脚の描画
}

int main()         /*** main関数 ***/
{
    dInitODE(); // ODEの初期化
    world = dWorldCreate();               // 動力学用世界の創造
    space = dHashSpaceCreate(0);          // 衝突用空間の創造
    contactgroup = dJointGroupCreate(0);  // ジョイントグループの生成
    ground = dCreatePlane(space,0,0,1,0); // 地面の生成
    dWorldSetGravity(world,0,0,-9.8);     // 重力設定

    createMonoBot();   // ロボットの生成

    dmLoop(800, 600);  // シミュレーションループ ウインドウの幅，高

    dSpaceDestroy(space); // 衝突用空間の破壊
    dWorldDestroy(world); // 動力学用世界の破壊
    dCloseODE();          // ODEの終了
    return 0;
}

ホームワーク

• step7-090720.zipをダウンロードして実行してみましょう．
• サンプルプログラムのロボットを10台出現するようにプログラムを変更しましょう．