STEP2: シミュレータを作ろう
ODE本 Step2 扉図のカラー画像
ODE本「簡単!実践!ロボットシミュレーション - Open Dynamics Engineによるロボットプログラミング
」のStep2です．
ここでは，ロボットを動かす上で必要になる関節を動かす方法，シミュレーションの再実行法，高速化，キーボードからの操作を学びホッピングロボットのシミュレータを作ります．
以下にStep2のシミュレータの動画を掲載します．ソースコードはこのページの注意事項を読み，ダウンロード，ビルドやコンパイルし実行してください．
- ソースコード
- EX2.1補足 (P33)
-
ヒントで「controlHinge関数をsimLoop関数の中に入れます。」という意味は、simLoop関数の中で呼び出すという意味です。関数の定義（本体）は、simLoop関数のすぐ上に挿入してください。プログラム2.5(P37)が参考になると思います。ここでは、controlSlider関数がsimLoop関数の中で呼び出されていますね。
- プログラム2.5： ホッピングロボット(P37)
- 説明：スライダ（直動式）ジョイントの動かし方を学び，一本脚ロボットをちょっとジャンプさせます．
- プログラム2.9： ホッピングロボット，再実行可能版 (P44)
- 説明：シミュレーションは普通は１回では終わらず，何度も繰り返し実行しなければなりませんよね．ここでは，ミュレーションの再実行法を学びます．
[cpp]
void controlHinge(dReal target)
{
static const dReal kp = 5.0; // 比例定数
static const dReal fmax = 200; // 最大トルク [Nm]
dReal tmp = dJointGetHingeAngle(joint); // 現在の角度を取得
dReal u = kp * (target - tmp); // 操作量
dJointSetHingeParam(joint,dParamVel,u); // 角速度の設定
dJointSetHingeParam(joint,dParamFMax,fmax); // 最大トルクの設定
}
[/cpp]
[cpp]
void controlHinge2(dReal target)
{
static const dReal kp = 5.0, kt = 2.0; // 比例定数
dReal tmp = dJointGetHingeAngle(joint); // 現在の角度を取得
dReal u = kp * (target - tmp); // 操作量
dReal omega = dJointGetHingeAngleRate(joint); // 角速度
dReal trq = kt * omega; // 摩擦トルク
dJointAddHingeTorque(joint, u - trq); // トルクを加える
}
[/cpp]
[cpp] s_joint = dJointCreateSlider(world, 0); // ジョイントの生成 dJointAttach(s_joint, leg[0].body,leg[1].body); // ジョイントの取付 dJointSetSliderAxis(s_joint, 0, 0, 1); // 軸ベクトルの設定 dJointSetSliderParam(s_joint, dParamLoStop, -0.5); // 最大収縮長[m] dJointSetSliderParam(s_joint, dParamHiStop, 0.5); // 最大伸展長[m] [/cpp]
[cpp]
static void controlSlider(dReal target) {
static dReal kp = 25.0; // 比例定数
static dReal fmax = 400; // 最大力[N]
dReal tmp = dJointGetSliderPosition(s_joint); // スライダの現在位置
dReal u = kp * (target - tmp); // 残差
dJointSetSliderParam(s_joint, dParamVel, u);
dJointSetSliderParam(s_joint, dParamFMax, fmax);
}
[/cpp]
[cpp]
void setDrawStuff()
{
fn.version = DS_VERSION; // drawStuffのバージョン
fn.start = &start; // シミュレーションループの前に呼び出される関数
fn.step = &simLoop; // ステップ毎に呼びだされる関数のアドレス
fn.command = &command; // キー入力により呼び出される関数のアドレス
fn.path_to_textures = "../../drawstuff/textures"; // テクスチャのパス
}
[/cpp]
[cpp]
void command(int cmd)
{
float xyz[3],hpr[3]; // 視点,視線
switch (cmd) {
case 'a':funcA(); break; // aキーを押すとfuncAを実行
case '1':func1(); break; // 1キーを押すとfunc1を実行
case 's': // sキーを押すと視点，視線を表示
dsGetViewpoint(xyz,hpr); // 視点，視線を取得
printf("xyz=%4.2f %4.2f %4.2f ",xyz[0],xyz[1],xyz[2]);
printf("hpr=%6.2f %6.2f %5.2f \n",hpr[0],hpr[1],hpr[2]);
break;
default:printf("Input a or 1\n");break;// 上記以外のキーを押すとき
}
}
[/cpp]
[cpp]
static void simLoop(int pause)
{
const dReal *pos1, *R1, *pos2, *R2;
int s = 200; // 跳躍する周期(ステップ)
if (!pause) { // 一時停止
STEPS++; // ステップ数
printf("STEPS:%4d\n",STEPS);
if ((0 <= (STEPS % s)) && ((STEPS % s) <= 10)) controlSlider(0.5);
else controlSlider(0.0);
dSpaceCollide(space,0,&nearCallback); // 衝突検出計算
dWorldStep(world,0.01); // 1ステップ進める
dJointGroupEmpty(contactgroup);
}
dsSetColor(1.0,0.0,0.0); // 赤色の設定
dsDrawSphere(dBodyGetPosition(torso.body), // 球の描画
dBodyGetRotation(torso.body), torso. r);
}
[/cpp]
（最終更新日 2008-7-27)
