13. 簡単な制御

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

ODE (Open Dynamics Engine) 初級講座の第１３回目です。

今回は今までの知識を整理するために、簡単な３関節マニピュレータのプチシミュレータを作ってみましょう。これはODEを使うと「たった１００行程度でこのようなシミュレータができるんです．」というプログラムなので衝突計算部分は省略しています．このロボットはキーボードで操作可能です．ロボットの第１関節はj（増加）又はf（減少）キーで，第２関節はd又はkキーで，第３関節はs又はlキーで動かすことができます。

速度＆比例制御

関節を制御する方法を説明します。ODEの関節にはモータが内蔵されています．このモータのトルクをコントロールするトルク制御も可能ですが，シミュレータの安定性を考えるとモータの回転速度をコントロールする速度制御がお勧めです．ここでは速度制御の実現法について説明します．

速度制御の中でも色々ありますが，ここでは最も簡単なP制御（比例制御）を説明します．これは目標値と現在値の差に比例して関節の角速度を変更する方法です。関節角度の現在値が目標値と離れている場合は大きな角速度を与え、目標値と同じ場合は角速度が０となり関節が停止します。また、現在値が目標値をとおり越した場合は、符号がマイナスになるため回転が反転し、現在値が目標値に近くなります。

これを実現している関数がcontrolです。関節のパラメータdParamVelは目標角速度で、dParamFMaxはその角速度を実現するためにジョイントが発揮できる最大トルクです。dParamFMaxを０に設定すると目標角速度を設定してもトルクが０なので関節は動いてくれません。

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

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

//  sample13.cpp  3 DOF manipulator by   Kosei Demura  2006-2008
#include "ode/ode.h"     // ODE
#include "drawstuff/drawsutt.h"  // ODEの描画ライブラリ
#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]

void control() {  /***  P制御  ****/
  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); // 最大トルクの設定
  }
}

void start() { /*** 描画APIの初期化 ***/
  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);               // 視点と視線の設定
}

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 ‘j’:  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); // 色の設定(r, g, b) 各値は0～1、ここでは白に設定
  for (int i = 0; i < NUM; i++ )　//　ロボットのリンクをカプセルで描画
    dsDrawCapsuleD(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";

  dInitODE();  // ODEの初期化
  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);　//　シミュレーションループ
  dCloseODE();
  return 0;
}

なお、ソースコードは次からダウンロードできます。

サンプルプログラム１３

おしまい．