３関節マニピュレータのシミュレータ （ソースコードはたった100行)
５月２７，２８日と早稲田大学で開催されたRobomec06に参加しました．Robomecは日本機械学会ロボットメカトロニクス部門が主催する発表者約８００名というマンモスな講演会です．私もODE(Open Dynamics Engine)で開発したロボット教育教材について発表したところこのウェブサイトの読者の方とお会いできてハッピーな気分になりました．
さて，フリーの物理計算エンジンODE講座は２月５日ぶりの更新となりました．その間，工学設計Ⅲ（卒業研究に相当）の指導，本の素原稿を出版社に提出，そのフィードバック，RoboCup Japan Open，ODEを使用した講義の開講，RoboCup世界大会への準備などいろいろなことがあり更新をさぼっていました．
今回の講座ではRobomecの予稿に掲載した３関節マニピュレータロボットのソースコードを掲載します（コメントを補足しています）．これはODEを使うと「たった１００行でこのようなシミュレータができるんです．」というプログラムなので衝突計算部分は省略しています．運動学や逆運動学を習った方はこのロボットに実装してみてください．ロボットが動くと不思議にわかった気になるものです．
では，ソースコードを次に紹介しましょう．
ロボットはキーボードで操作可能です． ロボットの第１関節はj（増加）又はf（減少）キーで，第２関節はd又はkキーで，第３関節はs又はlキーで動かします．これはプログラムのcommand関数に記述しています．
過去のODE講座を理解されている方には簡単なので説明がほとんどいらないと思います．わからない箇所があればコメントに書き込んでください．
// arm.c 3 DOF manipulator copyright (c) 2006 Kosei Demura
#include <ode/ode.h> // ODE
#include <drawstuff/drawstuff.h> // ODEの描画ライブラリ
#define NUM 4 // リンク数(土台含む)
dWorldID world; // 動力学計算世界
dBodyID link[NUM]; // リンク link[0]は土台
dJointID joint[NUM]; // 関節
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:比例ゲイン
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の初期化
void start() {
float xyz[3] = { 3.04, 1.28, 0.76}; // 視点
float hpr[3] = { -160.0, 4.50, 0.00}; // 視線(heading, pitch, roll)
dsSetViewpoint(xyz,hpr); // 視点と視線の設定
}
// キー操作関数
void command(int cmd) { //
switch (cmd) {
case ‘j’: THETA[1] += 0.05; break; // jキー
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;
}
// 目標角度の制限
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;
}
// シミュレーションループ
void simLoop(int pause) {
control();
dWorldStep(world, 0.02);
// ロボットの描画
dsSetColor(1.0,1.0,1.0); // 色の設定
for (int i = 0; i < NUM; i++ )
dsDrawCappedCylinderD(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;
}
なお，プログラムはここからダウンロード可能です
動作させるためにはcygwinまたはlinuxが必要です．過去の講座を参考にしてください。以下の要領で展開，make，実行してください．
- arm.tarを/home/ユーザ名/src/ode-0.5/myprogの中にコピーする．一番最後のディレクトリ名myprogは別の名前でもかまいません．
- 以下のコマンドをcygwinのターミナルソフトやxtermなどで実行する．
- cd ~/src/ode-0.5/myprog
- tar xvf arm.tar
- cd arm
- make
- ./arm
おしまい．
コメント