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

Up:[[Tutorial]]　　 　Previous：[[ロボット・ダイナミクス（単純モデル）]]　　 　　Next: [[車輪移動ロボット]]　　

----
#contents

*ロボット・ダイナミクス（ヒューマノイド） [#c61f754b]

※このサンプルはv2.2.0以降で正常に動作します。

ここでは物理シミュレーションモードでヒューマノイドロボットの関節を制御するサンプルを紹介します。

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

**コントローラ作成 [#e12abe50]
コントローラを作成します。
 $ cd ~/MyWorld
 $ emacs RobotContoroller.cpp
 $ emacs RobotController.cpp

RobotController.cpp

#highlight(cpp){{
#include "Controller.h"
#include "ControllerEvent.h"
#include "Logger.h"
#include <map> 

#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 onRecvMsg(RecvMsgEvent &evt); 
 
private:
  double kp;
  double fmax; 
 
  JMap jmap;
}; 
   
void RobotController::onInit(InitEvent &evt)
{
  kp     = 5.0;    //比例定数
  fmax   = 5000000.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("ROOT_JOINT0",0.0));
  jmap.insert(JMap::value_type("ROOT_JOINT1",0.0));
  jmap.insert(JMap::value_type("ROOT_JOINT2",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_JOINT2",-0.34));
  jmap.insert(JMap::value_type("RLEG_JOINT4",0.45));
  jmap.insert(JMap::value_type("RLEG_JOINT6",-0.16));
  jmap.insert(JMap::value_type("LLEG_JOINT2",-0.34));
  jmap.insert(JMap::value_type("LLEG_JOINT4",0.45));
  jmap.insert(JMap::value_type("LLEG_JOINT6",-0.16)); 

  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("LARM_JOINT7",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));
  jmap.insert(JMap::value_type("RARM_JOINT7",0.0));

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

  // 現在の全ジョイント角度取得
  JMap allJoints = my->getAllJointAngles();
  JMap::iterator now_it = allJoints.begin();

  while(now_it != allJoints.end()){
    std::string jointName = (*now_it).first;
    double nowAngle = (*now_it).second;
    
    // 目標角度取得
    JMap::iterator tar_it = jmap.find(jointName);
    if(tar_it != jmap.end()){
      double targetAngle = (*tar_it).second;
      
      //現在の角度と目標角度の差から角速度を決定します。
      double vel   = kp * (targetAngle - nowAngle);      

      //角速度を設定します。
      my->setJointVelocity(jointName.c_str(), vel, fmax);
    }
    now_it++;
  }
  return 0.01;
}
  
//メッセージ受信時に呼び出される関数。ここで関節の目標角度を設定します。
void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
  SimObj *my = getObj(myname());
   
  //メッセージを取得します。
  string msg = evt.getMsg();
 
  //メッセージにスペースが含まれる場合
  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)が持つすべてのヒンジ関節の関節名と目標角度の初期値が設定されています。また、メッセージを送信することによりそれぞれの関節の目標角度を設定することができます。

**コンパイル [#r342f1e4]
コンパイルします。

 $ ./sigmake.sh RobotController.cpp


**世界ファイルの作成 [#j089b2b9]
次に世界ファイルを作成します。

 $ emacs RobotWorld.xml

RobotWorld.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<world name="myworld">
  <gravity x="0.0" y="-980.7" z="0.0"/>
  <instanciate class="Robot-nii.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="55.0"/>
    <set-attr-value name="z" value="0.0"/>

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

    <set-attr-value name="collision" value="true"/>
    <camera id="1"
            link="HEAD_LINK"
            position="0 0 5"/>
  </instanciate>
</world>
}}
"dynamics"を"true"に設定しました。
**実行 [#h444c314]
それでは実行してみましょう。
 $ sigserver.sh -w ./RobotWorld.xml

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

次にロボットにメッセージを送信して関節の目標角度を設定します。
#ref(./RobotDynamics2.PNG,60%)

左肩の関節角度を-90°に設定しました。
実行ボタンを押すと左肩の関節の目標角度が設定されて、左肩の関節が動きます。

#ref(./RobotDynamics3.PNG,60%)


#highlight(end)

----
Up:[[Tutorial]]　　 　Previous：[[ロボット・ダイナミクス（単純モデル）]]　　 　　Next: [[車輪移動ロボット]]

#counter