ロボット・ダイナミクス(ヒューマノイド)
[
Front page
] [
New
|
List of pages
|
Search
|
Recent changes
|
Help
]
Start:
Up:[[Tutorial]] Previous:[[ロボット・ダイナミクス...
#contents
*ロボット・ダイナミクス(ヒューマノイド) [#c61f754b]
※このサンプルはv2.2.0以降で正常に動作します。
ここでは物理シミュレーションモードでヒューマノイドロボッ...
世界ファイルでヒューマノイドロボットや人間の"dynamics"の...
"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>
#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で各関節...
**コンパイル [#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="./Robot...
<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:[[ロボット・ダイナミクス...
End:
Up:[[Tutorial]] Previous:[[ロボット・ダイナミクス...
#contents
*ロボット・ダイナミクス(ヒューマノイド) [#c61f754b]
※このサンプルはv2.2.0以降で正常に動作します。
ここでは物理シミュレーションモードでヒューマノイドロボッ...
世界ファイルでヒューマノイドロボットや人間の"dynamics"の...
"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>
#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で各関節...
**コンパイル [#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="./Robot...
<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:[[ロボット・ダイナミクス...
Page: