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

The added line is THIS COLOR.
The deleted line is THIS COLOR.
[[ロボット・ダイナミクス（単純モデル）]]
Up:[[Tutorial]]　　 　Previous：[[ロボット・ダイナミクス（単純モデル）]]　　 　　
#contents
*ロボット・ダイナミクス（ヒューマノイド） [#c61f754b]
これまでのサンプルでは世界ファイルでヒューマノイドロボットや人間の"dynamics"の設定を"false"に設定していました。
"dynamics"の設定を"true"にした場合、エージェントの位置や関節角度を直接指定することができなくなります。また、シミュレーション開始と同時に重力の影響で関節が曲がってしまい、エージェントが崩れ落ちてしまいます。ここではダイナミクスシミレーションを行う時にヒューマノイドロボットの関節角度を制御するコントローラのサンプルを紹介します。

**コントローラ作成 [#e12abe50]
コントローラを作成します。
 $ 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°)が設定されています。また、メッセージを送信することによりそれぞれの関節の目標角度を設定することができます。

**コンパイル [#r342f1e4]
まず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
***世界ファイルの作成 [#j089b2b9]
次に世界ファイルを作成します。
 $ cd ..
 $ emacs xml/RobotWorld.xml

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