![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
Up:[[Tutorial]] Previous：[[ロボット・ダイナミクス（単純モデル）]] Next: [[車輪移動ロボット]] #contents *ロボット・ダイナミクス（ヒューマノイド） [#c61f754b] ※このサンプルはv121029, v2.0.1以降には対応しておりません。 ※このサンプルはv2.2.0以降で正常に動作します。 ここでは物理シミュレーションモードでヒューマノイドロボットの関節を制御するサンプルを紹介します。 ここではヒューマノイドロボットの物理シミュレーションを行うサンプルを紹介します。（バージョン110418以降で正常に動作） 世界ファイルでヒューマノイドロボットや人間の"dynamics"の設定を"false"に設定した場合はエージェントの関節角度を直接指定することができます。しかし、 世界ファイルでヒューマノイドロボットや人間の"dynamics"の設定を"false"に設定した場合はエージェントの関節角度を直接指定することができました。しかし、 "dynamics"の設定を"true"にした場合、関節角度を直接指定することができなくなります。また、シミュレーション開始と同時に重力の影響で関節が曲がってしまい、エージェントが崩れ落ちてしまいます。ここではヒューマノイドロボットの関節に力を加え関節角度を制御するサンプルを紹介します。 **コントローラ作成 [#e12abe50] コントローラを作成します。 $ cd ~/MyWorld $ emacs RobotContoroller.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> #include "Controller.h" #include "ControllerEvent.h" #include "Logger.h" #include <map> #define PI 3.141592 #define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 ) #define PI 3.141592 #define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 ) using namespace std; using namespace std; typedef map<string, double> JMap; typedef map<string, double> JMap; class RobotController : public Controller { public: void onInit(InitEvent &evt); double onAction(ActionEvent&); void onRecvMsg(RecvMsgEvent &evt); class RobotController : public Controller { public: void onInit(InitEvent &evt); double onAction(ActionEvent&); void onRecvMessage(RecvMessageEvent &evt); private: double kp; double fmax; private: double kp; double fmax; JMap jmap; }; JMap jmap; }; void RobotController::onInit(InitEvent &evt) { kp = 2.0; //比例定数 fmax = 1000.0; //最大トルク 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("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("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_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("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("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)); jmap.insert(JMap::value_type("RARM_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)); } 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 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 * ((*it).second - angle); double vel = kp * (targetAngle - nowAngle); //角速度を設定します。 my->setJointVelocity((*it).first.c_str(), vel, fmax); it++; my->setJointVelocity(jointName.c_str(), vel, fmax); } return 0.7; } now_it++; } return 0.01; } //メッセージ受信時に呼び出される関数。ここで関節の目標角度を設定します。 void RobotController::onRecvMessage(RecvMessageEvent &evt) { SimObj *my = getObj(myname()); //メッセージ受信時に呼び出される関数。ここで関節の目標角度を設定します。 void RobotController::onRecvMsg(RecvMsgEvent &evt) { SimObj *my = getObj(myname()); //メッセージのサイズを取得します。 int size = evt.getSize(); if (size>0) { //メッセージを取得します。 string msg = evt.getMsg(); //メッセージを取得します。 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); //メッセージに=が含まれる場合 if(strstr(msg.c_str(),"=")) { //受け取ったJoint名と角度を表示します。 LOG_MSG(("Joint Name : %s", msg_j.c_str())); LOG_MSG(("Joint Angle : %s", angle_str.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; } //目標角度を設定します。 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°)が設定されています。また、メッセージを送信することによりそれぞれの関節の目標角度を設定することができます。 これは関節の角度が目標角度に達するように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="-9.8" 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="60.0"/> <set-attr-value name="z" value="0.0"/> <set-attr-value name="language" value="c++"/> <set-attr-value name="implementation" value="./RobotController.so"/> </instanciate> <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(dyna_1.jpg) #ref(./RobotDynamics.PNG,60%) 次にロボットにメッセージを送信して関節の目標角度を設定します。 #ref(dyna_2.jpg) #ref(./RobotDynamics2.PNG,60%) 右腕の関節角度を-20°に設定しました。 実行ボタンを押すと右肩の関節の目標角度が設定されて、右肩の関節がゆっくりと動き始めます。 左肩の関節角度を-90°に設定しました。 実行ボタンを押すと左肩の関節の目標角度が設定されて、左肩の関節が動きます。 ※関節が動き始めるまで時間がかかることがあります。 #ref(./RobotDynamics3.PNG,60%) #ref(dyna_3.jpg) 目標角度を大きくしすぎるとバランスが取れなくなってロボットは転倒してしまいます。 #ref(dyna_4.jpg) #highlight(end) Up:[[Tutorial]] Previous：[[ロボット・ダイナミクス（単純モデル）]] Next: [[車輪移動ロボット]]
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.