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

List of Backups
View the diff.
View the diff current.
View the source.
Go to ロボット・ダイナミクス（ヒューマノイド）.
- 1 (2013-02-25 (Mon) 14:09:53)
- 2 (2013-02-25 (Mon) 14:14:30)
- 3 (2013-04-19 (Fri) 14:35:20)

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

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

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

※このサンプルはv121029, v2.0.1以降には対応しておりません。

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

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

↑

コントローラ作成 †

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

$ cd ~/MyWorld
$ 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°)が設定されています。また、メッセージを送信することによりそれぞれの関節の目標角度を設定することができます。

↑

コンパイル †

コンパイルします。

$ ./sigmake.sh RobotController.cpp

↑

世界ファイルの作成 †

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

$ emacs RobotWorld.xml

RobotWorld.xml

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

↑

実行 †

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

$ sigserver.sh -w ./RobotWorld.xml

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

#ref(): File not found: "dyna_1.jpg" at page "ロボット・ダイナミクス（ヒューマノイド）"

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

#ref(): File not found: "dyna_2.jpg" at page "ロボット・ダイナミクス（ヒューマノイド）"

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

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

#ref(): File not found: "dyna_3.jpg" at page "ロボット・ダイナミクス（ヒューマノイド）"

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