demura.net:RoboCup・ODE・夢考房

図：２リンク機構に初期角速度を与えるシミュレーション CohCohさんのご質問で初期の関節角速度の設定法がありましたので、久しぶりにODE(Open Dynamics Engine)講座を開講します。 ODEでは関節にモータが標準で付属していますが、関節角速度を設定しようとしても、目標角速度に到達するまで時間がかかりますし、その角速度を維持するためにはそれなりの制御が必要です。現実のモータを単純にモデル化したものです。 これでは、初期角速度を設定できません。この問題を解決する方法として、ODEには理想的なAMOTORがあります。マニュアルでは１ステップで目標角速度に到達するとありますが、関節に与える最大トルクdParamFmaxの値が0だと目標角速度に到達しません。また、小さな値の場合は、目標角速度に到達するまで数ステップかかることを確認しました。 そこで、十分大きな値を設定してください。以下のサンプルプログラムでは目標角速度dParamVelに無限大dInifinityを与えています。 また、初期角速度だけ与えたい場合は、角速度が目標値に到達したら、dJointDestroy()でamotorを破壊する必要があります。これを忘れると常に角速度は目標角速度のままです。amotorを破壊したくない場合は、dJointSetAMotorNumAxes(amotor, 0)で制御する軸数を０に設定し、無効化してもＯＫです。 次に、APIを紹介します。

dJointID dJointCreateAMotor(dWorldID, dJointGroupID); AMOTORの生成。 void dJointSetAMotorMode(dJointID, int mode); AMOTORのモード設定。modeはユーザが独自に設定するdAMotorUserまたは、自動的にオイラー角が計算されるdAMotorEulerの２つのモードがある。 void dJointSetAMotorNumAxes(dJointID, int num); AMOTORでコントロールされる軸数を設定する。numの値は0から3までで、0の場合はジョイントを無効化する。dAMotorEulerモードの場合は自動的に3が設定される。 void dJointSetAMotorAxis (dJointID, int anum, int rel, dReal x, dReal y, dReal z); AMOTORの回転軸を設定する。引数anumは変更する軸(0, 1, 2)、relは以下の相対回転モードを設定する。最後の(x,y,z)は回転軸ベクトル。

• 0: 軸は絶対座標系に固定
• 1: 1番目のボディに固定
• 2: 2番目のボディに固定

void dJointSetAMotorParam (dJointID, int parameter, dReal value); AMOTORのパラメータを設定する。 parameterについては他のジョイントと同じ。この例では角速度（スライダージョイントの場合は速度）dParamVel、最大発揮トルクdParamFMaxを使用した。

AMOTORはAngular Motorの略ですが、エー（良い）モータですね。ソースコードを以下に示しますので試してください。 なお、簡単にするために衝突検出機能は実装されていません。ここからダウンロード可能です(amotor2-080115.tgz)。 [code] // 2008-1-15 // amotor2.c: 2 Links with an amotor by Kosei Demura #include #include #include #include #define NUM 2 #ifdef dDOUBLE #define dsDrawCylinder dsDrawCylinderD #endif dWorldID world; dSpaceID space; dGeomID ground; dJointID joint[NUM], amotor; dsFunctions fn; typedef struct { dBodyID body; dGeomID geom; } MyLink; MyLink rlink[NUM]; void createArm() { dReal init_avel = 1.0; // 初期角速度[rad/s] dMass mass; dReal x[] = { 0.00, 0.00}, y[] = { 0.00, 0.00}, z[]= { 0.75 ,0.25}; // 重心 dReal length[] = { 0.50, 0.50}; // リンク長 dReal weight[] = { 1.00, 1.00}; // 質量 dReal r[] = { 0.02, 0.02}; // 半径 dReal anchor_x[] = { 0.00, 0.00}; // 回転軸中心 dReal anchor_y[] = { 0.00, 0.00}; dReal anchor_z[] = { 1.00, 0.50}; dReal axis_x[] = { 1.00, 1.00}; // 回転軸ベクトル dReal axis_y[] = { 0.00, 0.00}; dReal axis_z[] = { 0.00, 0.00}; // リンクの生成 for (int i = 0; i < NUM; i++) { rlink[i].body = dBodyCreate(world); dBodySetPosition(rlink[i].body, x[i], y[i], z[i]); dMassSetZero(&mass); dMassSetCylinderTotal(&mass,weight[i],3,r[i],length[i]); dBodySetMass(rlink[i].body, &mass); rlink[i].geom = dCreateCylinder(space,r[i],length[i]); dGeomSetBody(rlink[i].geom,rlink[i].body); } //ジョイントの生成と結合 for (int j = 0; j < NUM; j++) { joint[j] = dJointCreateHinge(world, 0); // リンク１はjoint[0]により静的環境0と結合 if (j == 0) dJointAttach(joint[0], rlink[0].body, 0); else dJointAttach(joint[j], rlink[j].body, rlink[j-1].body); dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j], anchor_z[j]); dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]); } // リンク1とリンク2を結合しているジョイントを可動域設定 dJointSetHingeParam(joint[1], dParamLoStop, -0.0 * M_PI); // 可動域の下限 dJointSetHingeParam(joint[1], dParamHiStop, 0.0 * M_PI); // 可動域の上限 // amotorの生成 amotor = dJointCreateAMotor(world, 0); // AMotorの生成 dJointAttach(amotor, rlink[0].body, 0); // AMotorとリンクとの結合 dJointSetAMotorMode(amotor, dAMotorUser); // ユーザモードに設定、デフォルト、省略可 dJointSetAMotorNumAxes(amotor, 1); // AMotorでコントロールする軸数 // 軸の設定：回転軸0を絶対座標系に固定、回転軸ベクトルは(1, 0, 0) dJointSetAMotorAxis(amotor, 0 /*軸0*/, 0 /*絶対座標系に固定*/, 1 /*x成分*/, 0 /*y成分*/, 0 /*z成分*/); // amotorの設定, 初期角速度をinit_avelに設定 dReal fmax = dInfinity;　// 無限大 dJointSetAMotorParam(amotor, dParamVel, init_avel); dJointSetAMotorParam(amotor, dParamFMax, fmax); } void drawLink() { dReal r,length; for (int i = 0; i < NUM; i++ ) { if (i == 1) dsSetColor(1.3, 1.3, 0.0); else dsSetColor(0.0, 0.0, 1.3); dGeomCylinderGetParams(rlink[i].geom, &r,&length); dsDrawCylinder(dBodyGetPosition(rlink[i].body), dBodyGetRotation(rlink[i].body),length,r); } } void start() { float xyz[3] = { 2, 0, 1}; float hpr[3] = {-180, 0, 0.0}; dsSetViewpoint(xyz,hpr); } // 簡単にするため衝突検出は未実装 void simLoop(int pause) { static int step = 0; drawLink(); const dReal ang_rate = dJointGetHingeAngleRate(joint[0]); printf(”step=%4d: angle rate=%8.6f \n”, step, ang_rate); dWorldStep(world, 0.001); dReal k1 = 0.0; // 摩擦トルク用比例係数 負値を設定 dJointAddHingeTorque(joint[0], k1 * ang_rate); // 摩擦トルクを付加 if (step == 0) dJointDestroy(amotor); // amotorの破壊 // amotorを破壊したくない場合は以下でも良い // if (step == 0) dJointSetAMotorNumAxes(amotor, 0); step++; } void setDrawStuff() { fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.path_to_textures = “../../drawstuff/textures”; } int main(int argc, char *argv[]) { setDrawStuff(); world = dWorldCreate(); space = dHashSpaceCreate(0); // dWorldSetGravity (world, 0, 0, -9.8); // 重力加速度 createArm(); dsSimulationLoop(argc,argv,640, 480,&fn); dSpaceDestroy(space); dWorldDestroy(world); return 0; } [/code] 終わり。