Backup of ロボット・ダイナミクス（ヒューマノイド）_v1.4.8(No. 7)

Up:Tutorial?　　　Previous：ロボット・ダイナミクス（単純モデル）　　　　Next: 車両のダイナミクス（ダンベルモデル）　　

ロボット・ダイナミクス（ヒューマノイド）

ロボット・ダイナミクス（ヒューマノイド） †

ここではヒューマノイドロボットの物理シミュレーションを行うサンプルを紹介します。（バージョン110418以降で正常に動作）

世界ファイルでヒューマノイドロボットや人間の"dynamics"の設定を"false"に設定した場合はエージェントの関節角度を直接指定することができます。しかし、 "dynamics"の設定を"true"にした場合、関節角度を直接指定することができなくなります。また、シミュレーション開始と同時に重力の影響で関節が曲がってしまい、エージェントが崩れ落ちてしまいます。ここではヒューマノイドロボットの関節に力を加え関節角度を制御するサンプルを紹介します。

↑

コントローラ作成 †

コントローラを作成します。

$ cd ~/sigverse-<version>/bin/NewWorld
$ emacs RobotContoroller.cpp

RobotController.cpp

#include "Controller.h"
#include "ControllerEvent.h"
#include "Logger.h"
#include <map> 

#define  PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 ) 

using namespace std; 

typedef map<string, double> JMap; 

class RobotController : public Controller {
public:
  void onInit(InitEvent &evt);
  double onAction(ActionEvent&);
  void onRecvMessage(RecvMessageEvent &evt); 

private:
  double kp;
  double fmax; 

  JMap jmap;
}; 
  
void RobotController::onInit(InitEvent &evt)
{
  kp     = 2.0;    //比例定数
  fmax   = 1000.0; //最大トルク 

  //ジョイント名と目標角度のマップを作成します.
  jmap.insert(JMap::value_type("WAIST_JOINT0",0.0));
  jmap.insert(JMap::value_type("WAIST_JOINT1",0.0));
  jmap.insert(JMap::value_type("WAIST_JOINT2",0.0));
  jmap.insert(JMap::value_type("CHEST",0.0));
  jmap.insert(JMap::value_type("HEAD_JOINT0",0.0));
  jmap.insert(JMap::value_type("HEAD_JOINT1",0.0)); 

  jmap.insert(JMap::value_type("RLEG_JOINT1",0.0));
  jmap.insert(JMap::value_type("RLEG_JOINT2",0.0));
  jmap.insert(JMap::value_type("RLEG_JOINT3",0.0));
  jmap.insert(JMap::value_type("RLEG_JOINT4",0.0));
  jmap.insert(JMap::value_type("RLEG_JOINT5",0.0));
  jmap.insert(JMap::value_type("LLEG_JOINT1",0.0));
  jmap.insert(JMap::value_type("LLEG_JOINT2",0.0));
  jmap.insert(JMap::value_type("LLEG_JOINT3",0.0));
  jmap.insert(JMap::value_type("LLEG_JOINT4",0.0));
  jmap.insert(JMap::value_type("LLEG_JOINT5",0.0)); 

  jmap.insert(JMap::value_type("LARM_JOINT0",0.0));
  jmap.insert(JMap::value_type("LARM_JOINT1",0.0));
  jmap.insert(JMap::value_type("LARM_JOINT2",0.0));
  jmap.insert(JMap::value_type("LARM_JOINT3",0.0));
  jmap.insert(JMap::value_type("LARM_JOINT4",0.0));
  jmap.insert(JMap::value_type("LARM_JOINT5",0.0));
  jmap.insert(JMap::value_type("LARM_JOINT6",0.0)); 

  jmap.insert(JMap::value_type("RARM_JOINT0",0.0));
  jmap.insert(JMap::value_type("RARM_JOINT1",0.0));
  jmap.insert(JMap::value_type("RARM_JOINT2",0.0));
  jmap.insert(JMap::value_type("RARM_JOINT3",0.0));
  jmap.insert(JMap::value_type("RARM_JOINT4",0.0));
  jmap.insert(JMap::value_type("RARM_JOINT5",0.0));
  jmap.insert(JMap::value_type("RARM_JOINT6",0.0));
}

double RobotController::onAction(ActionEvent &evt) {
  SimObj *my = getObj(myname());

 //ジョイント名と角度のマップでループします.
 JMap::iterator it = jmap.begin();
 while(it != jmap.end())
   {
     //現在の角度を取得します。
     double angle = my->getJointAngle((*it).first.c_str());

     //現在の角度と目標角度の差から角速度を決定します。
     double vel   = kp * ((*it).second - angle);

     //角速度を設定します。
     my->setJointVelocity((*it).first.c_str(), vel, fmax);
     it++;
   }
 return 0.7;
}
 
//メッセージ受信時に呼び出される関数。ここで関節の目標角度を設定します。
void RobotController::onRecvMessage(RecvMessageEvent &evt)
{
  SimObj *my = getObj(myname());
  
  //メッセージのサイズを取得します。
  int size = evt.getSize();
  if (size>0)
    {

      //メッセージを取得します。
      string msg = evt.getString(0);
      SimObj *my = getObj(myname()); 

      //メッセージに=が含まれる場合
      if(strstr(msg.c_str(),"="))
        { 

          //メッセージをJoint名と角度に分割します。
          string msg_j;
         string angle_str;
         int n = 0;
         n = msg.find("=");
         msg_j = msg.substr(0,n);
         angle_str = msg.substr(n+1);

         //受け取ったJoint名と角度を表示します。
          LOG_MSG(("Joint Name : %s", msg_j.c_str()));
         LOG_MSG(("Joint Angle : %s", angle_str.c_str()));

         //目標角度を設定します。
          double angle = atof(angle_str.c_str());
         JMap::iterator it = jmap.find(msg_j.c_str());
         (*it).second = DEG2RAD(angle);
        }
    }
}
extern "C" Controller * createController() {
  return new RobotController;
}

これは関節の角度が目標角度に達するようにonActionで各関節の角速度を設定するサンプルです。onInitでヒューマノイドロボット（nii_robot.x3d)が持つすべてのヒンジ関節の関節名と目標角度の初期値(0°)が設定されています。また、メッセージを送信することによりそれぞれの関節の目標角度を設定することができます。

↑

コンパイル †

まずMakefileを修正します。

$ emacs Makefile

#オブジェクトファイルの指定でobotController.soを指定

#オブジェクトファイルの指定
OBJS     = RobotController.so

Makefile

#SIGVerseソースの場所指定
SIG_SRC  = /home/<username>/sigverse-<version>/include/sigverse

#オブジェクトファイルの指定
OBJS     = RobotController.so

all: $(OBJS) 

#コンパイルを行います。
./%.so: ./%.cpp
        g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_SRC) - I$(SIG_SRC)/comm/controller  -fPIC -shared -o $@   $<

コンパイルします。

$ make

↑

世界ファイルの作成 †

次に世界ファイルを作成します。

$ cd ..
$ emacs xml/RobotWorld.xml

RobotWorld.xml

<?xml version="1.0" encoding="utf8"?>
<world name="myworld"> 

  <gravity x="0.0" y="-9.8" z="0.0"/> 

  <instanciate class="Robot-x3d.xml">
        <set-attr-value name="name" value="Robot1"/> 

        <set-attr-value name="dynamics" value="true"/>

        <set-attr-value name="x" value="0.0"/>
        <set-attr-value name="y" value="86.0"/>
        <set-attr-value name="z" value="0.0"/>

        <set-attr-value name="language" value="c++"/>
        <set-attr-value name="implementation"  value="./NewWorld/RobotController.so"/>

  </instanciate>
</world>

"dynamics"を"true"に設定しました。

↑

実行 †

それでは実行してみましょう。

$ ./sigserver.sh -p 9001 -w RobotWorld.xml

SIGViewerでサーバに接続してシミュレーションを開始します。するとヒューマノイドロボットがバランスを取りながら立っているのが見えます。

次にロボットにメッセージを送信して関節の目標角度を設定します。

右腕の関節角度を-20°に設定しました。実行ボタンを押すと右肩の関節の目標角度が設定されて、右肩の関節がゆっくりと動き始めます。

※関節が動き始めるまで時間がかかることがあります。

目標角度を大きくしすぎるとバランスが取れなくなってロボットは転倒してしまいます。