ODE講座24：AMOTORはエー(良い)モータ

図：２リンク機構に初期角速度を与えるシミュレーション
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]

終わり。