ODEで学ぶC言語２ [Step6: まとめ]

STEP6: 玉突きロボット

ODEで学ぶC言語２のStep6です．今回で最終回となります．ODEのジョイントの生成法と簡単な制御のサンプルプログラムを学びましょう．

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

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

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

• ジョイントの使い方

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

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

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

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

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

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

/* step6　*/

#define START_X 0.0
#define START_Y 0.0
#define START_Z 0.1

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

// ロボットの生成
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 = 100.0; // 胴体の質量
    double r_torso = 0.25;  // 胴体の半径
    double l_torso = 0.2;   // 胴体の高さ
    double r_ball = 0.11;   // ボールの半径
    double m_ball = 0.5;    // ボールの質量
    double p_ball[3] = {START_X, START_Y+2*l_leg+0.1, START_Z}; // ボールの位置
    double p_torso[3] = {START_X, START_Y, START_Z};  // 位置
    double p_leg[2][3] = {{START_X, START_Y+0.5*l_leg, START_Z},
        {START_X, START_Y+l_leg, START_Z}
    };
    double p_anchor[3] = {START_X, START_Y, START_Z};
    double R[12];   // 回転行列

    static double  red[3] = {1.0, 0.0, 0.0}; // 色
    static double  color_leg[2][3] = {{0.0, 0.0, 1.0},{1.3, 1.3, 1.3}};

    dRSetIdentity(R);  //回転行列を単位行列で初期化
    dmCreateSphere(&ball, p_ball,R,m_ball, r_ball, red); // ボールの生成
    dmCreateCylinder(&torso,p_torso,R,m_torso,r_torso,l_torso,red); // 胴体の生成

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

    // ヒンジジョイント
    leg[0].joint = dmJointCreateHinge();
    dJointAttach(leg[0].joint, torso.body,leg[0].body);
    dJointSetHingeAnchor(leg[0].joint, p_anchor[0],p_anchor[1],p_anchor[2]);
    dJointSetHingeAxis(leg[0].joint, 0, 0, 1);

    // スライダージョイント
    leg[1].joint = dmJointCreateSlider();
    dJointAttach(leg[1].joint, leg[0].body,leg[1].body);
    dJointSetSliderAxis(leg[1].joint, 0, 1, 0);
    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);
}

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

    if (STEPS != 0) dmClose(); // シミュレーションの終了

    dmInit();  // シミュレーションの初期化
    createMonoBot();
}

// キー操作
void command(int cmd)
{
    switch (cmd)
    {
    case 'j':
        S_LENGTH =   0.25;
        break;
    case 'l':
        S_LENGTH = - 0.25;
        break;
    case 'i':
        H_ANGLE +=   0.25;
        if (H_ANGLE  >   M_PI) H_ANGLE  =  -M_PI;
        break;
    case 'm':
        H_ANGLE -=   0.25;
        if (H_ANGLE  <  -M_PI) H_ANGLE  =   M_PI;
        break;
    case 'r':
        resetSim();
        break;
    case 'a':
        dBodyAddForce(torso.body, 0, -500, 0);
        break;
    case 'w':
        dBodyAddForce(torso.body, -500, 0, 0);
        break;
    case 'd':
        dBodyAddForce(torso.body, 0,  500, 0);
        break;
    case 'x':
        dBodyAddForce(torso.body,  500, 0, 0);
        break;
    case 's':
        dBodySetLinearVel(torso.body,  0, 0, 0);
        dBodySetAngularVel(torso.body, 0, 0, 0);
        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);   // ヒンジジョイントの制御

        dmSimStep(); // シミュレーションを１ステップ進める
    }

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

int main()         /*** main関数 ***/
{
    dmInit(); // ODEの初期化
    resetSim();   // ロボットの生成
    dmLoop(800, 600, simLoop, command);  // シミュレーションループ ウインドウの幅，高
    dmClose();          // ODEの終了
    return 0;
}

ホームワーク

• step6-100726.zipをダウンロードして実行してみましょう．
• サンプルプログラムを改良してビリヤードをプレイするロボットを作りましょう．