ODE講座16：ロボットアーム・プチシミュレータ

３関節マニピュレータのシミュレータ　（ソースコードはたった100行)

ゲーム開発やロボットの研究者にも使われているオープンソースの物理計算エンジンODE(Open Dynamics  Engine、オープン ダイナミクスエンジン）を学ぶODE講座の第１６回目です。

今回は今までの知識を整理するために、簡単な３関節マニピュレータのプチシミュレータを作ってみましょう。これはODEを使うと「たった１００行程度でこのようなシミュレータができるんです．」というプログラムなので衝突計算部分は省略しています．運動学や逆運動学を習った方はこのロボットに実装してみてください．ロボットが動くと不思議にわかった気になるものです．

では，ソースコードを次に紹介しましょう．

ロボットはキーボードで操作可能です． ロボットの第１関節はj（増加）又はf（減少）キーで，第２関節はd又はkキーで，第３関節はs又はlキーで動かします．これはプログラムのcommand関数に記述しています．

過去のODE講座を理解されている方には簡単なので説明がほとんどいらないと思います．わからない箇所があればコメントに書き込んでください．

[code]
// arm.cpp 3 DOF manipulator by Kosei Demura 2006
#include // ODE
#include // ODEの描画ライブラリ
#ifdef dDouble
#define dsDrawCapsule dsDrawCapsuleD
#endif
#define NUM 4 // リンク数(土台含む)
dWorldID world; // 動力学計算世界
dBodyID link[NUM]; // リンク　link[0]は土台
dJointID joint[NUM]; // 関節 joint[0]は土台と地面を固定
static double THETA[NUM] = { 0.0, 0.0, 0.0, 0.0}; // 関節目標角[rad]
static double l[NUM] = { 0.10, 0.90, 1.00, 1.00}; // リンク長[m]
static double r[NUM] = { 0.20, 0.04, 0.04, 0.04}; // リンク半径[m]

// P制御
void control() {
static int step = 0; // シミュレーションのステップ数
double k1 = 10.0, fMax = 100.0; // k1:比例ゲイン, fMax：最大トルク[Nm]
printf("\r%6d:",step++);
for (int j = 1; j < NUM; j++) {
double tmpAngle = dJointGetHingeAngle(joint[j]); // 現在の関節角[rad]
double z = THETA[j] - tmpAngle; // z: 残差=目標関節角－現在関節角
dJointSetHingeParam(joint[j], dParamVel, k1*z); // 角速度の設定
dJointSetHingeParam(joint[j], dParamFMax, fMax); // 最大トルクの設定
}
}

// 描画APIの初期化
// ODEのAPIは全て関節の単位は[rad]ですが、drawstuffのAPIであるdsSetViewpointの引数3次元配列hpr[3]の各要素の単位は[°]となっています。これだけ違うので気をつけましょう！
void start() {
float xyz[3] = { 3.04, 1.28, 0.76}; // 視点x, y, z　[m]
float hpr[3] = { -160.0, 4.50, 0.00}; // 視線(heading, pitch, roll)　[°]
dsSetViewpoint(xyz,hpr); // 視点と視線の設定
}

// キー操作関数
//　キーの入力があるたびにcmdにはそのキーの値が代入されます。
void command(int cmd) { //
switch (cmd) {
case ‘j’: THETA[1] += 0.05; break; // jキーが押されるとTHETA[1]の角度が0.05[rad]増加する
case ‘f’: THETA[1] -= 0.05; break;
case ‘k’: THETA[2] += 0.05; break;
case ‘d’: THETA[2] -= 0.05; break;
case ‘l’: THETA[3] += 0.05; break;
case ’s’: THETA[3] -= 0.05; break;
}
// 目標角度が関節可動域を越えないように制限する
// 関節可動域はdJointSetHingeParam(dJointID, int parameter, dReal value)で、parameterを関節下限の場合dParamLoStop、関節上限の場合はdParamHiStopに設定する。valueはその値を入れる。単位は[rad]。
if (THETA[1] < - M_PI) THETA[1] = - M_PI; // M_PI：円周率
if (THETA[1] > M_PI) THETA[1] = M_PI;
if (THETA[2] < -2*M_PI/3) THETA[2] = - 2*M_PI/3;
if (THETA[2] > 2*M_PI/3) THETA[2] = 2*M_PI/3;
if (THETA[3] < -2*M_PI/3) THETA[3] = - 2*M_PI/3;
if (THETA[3] > 2*M_PI/3) THETA[3] = 2*M_PI/3;
}

// シミュレーションループ
// シミュレーション実行時に毎回呼び出されます。ソースコードではわかりませんが、この部分はwhileループに入っています。また、ここでは衝突検出に関するコードは省略されています。
void simLoop(int pause) {
control();
dWorldStep(world, 0.02);
// ロボットの描画
dsSetColor(1.0,1.0,1.0); // 色の設定(r, g, b) 各値は0～1、ここでは白に設定
for (int i = 0; i < NUM; i++ )　//　ロボットのリンクをカプセルで描画
dsDrawCapsule(dBodyGetPosition(link[i]), dBodyGetRotation(link[i]), l[i], r[i]);
}

int main(int argc, char *argv[]) {
dsFunctions fn; // 描画関数
dMass mass; // 質量パラメータ
double x[NUM] = {0.00}, y[NUM] = {0.00}; // 重心位置
double z[NUM] = { 0.05, 0.50, 1.50, 2.55};
double m[NUM] = {10.00, 2.00, 2.00, 2.00}; // 質量
double anchor_x[NUM] = {0.00}, anchor_y[NUM] = {0.00};
double anchor_z[NUM] = { 0.00, 0.10, 1.00, 2.00} ;//回転中心
double axis_x[NUM] = { 0.00, 0.00, 0.00, 0.00}; //回転軸
double axis_y[NUM] = { 0.00, 0.00, 1.00, 1.00};
double axis_z[NUM] = { 1.00, 1.00, 0.00, 0.00};
fn.version = DS_VERSION; fn.start = &start;
fn.step = &simLoop; fn.command = &command;
fn.path_to_textures = "../../drawstuff/textures";

world = dWorldCreate(); // 動力学計算世界の生成
dWorldSetGravity(world, 0, 0, -9.8); // 重力設定

for (int i = 0; i < NUM; i++) { // リンクの生成と設定
link[i] = dBodyCreate(world); // リンクの生成
dBodySetPosition(link[i], x[i], y[i], z[i]); // 位置の設定
dMassSetZero(&mass); // 質量パラメータの初期化
dMassSetCappedCylinderTotal(&mass,m[i],3,r[i],l[i]); // リンクの質量計算
dBodySetMass(link[i], &mass); // 質量の設定
}
// 関節の生成と設定
joint[0] = dJointCreateFixed(world, 0); // 固定関節（土台と地面の固定）
dJointAttach(joint[0], link[0], 0); // 固定関節の取付け
dJointSetFixed(joint[0]); // 固定関節の設定
for (int j = 1; j < NUM; j++) {
joint[j] = dJointCreateHinge(world, 0); // ヒンジ関節生成
dJointAttach(joint[j], link[j-1], link[j]); // 関節の取付け
dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]); //関節中心の設定
dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]); // 関節回転軸の設定
}
dsSimulationLoop(argc, argv, 640, 570, &fn);　//　シミュレーションループ
return 0;
}

[/code]

なお，プログラムarm070919.tgzはここからダウンロード可能です
動作させるためにはmingwまたはLinuxが必要です．過去の講座を参考にしてください。以下の要領で展開，make，実行してください．

1. arm070919.tgzを/home/ユーザ名/src/ode-0.8/myprogの中にコピーする．一番最後のディレクトリ名myprogは別の名前でもかまいません．
2. 以下のコマンドをcygwinのターミナルソフトやxtermなどで実行する．

• cd  ~/src/ode-0.8/myprog
• tar   xvzf   arm070919.tgz
• cd   arm
• make
• ./arm

おしまい．

• 2008-1-13: コードの整形
• 2007-9-19: makefileの更新。arm070919.tgzにバージョンアップ。