![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
Tutorial?
ここではロボット型エージェントが物体を持つ動作をするサンプルを紹介します。
$ cd sigverse-<version>/bin/NewWorld $ emacs grasp.cpp
grasp.cpp
#include <Controller.h>
#include <ControllerEvent.h>
#include <Logger.h>
#include <ViewImage.h>
#include <math.h>
#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )
using namespace std;
class RobotController : public Controller
{
public:
void onInit(InitEvent &evt);
double onAction(ActionEvent &evt);
void onRecvMessage(RecvMessageEvent &evt);
void onCollision(CollisionEvent &evt);
private:
//物体をつかむ関数Graspの利用を宣言します。
void Grasp(string obj, char *arm);
//graspフラグ
bool grasp;
string grasp_obj;
//衝突フラグ
bool Colli;
};
void RobotController::onInit(InitEvent &evt)
{
Colli = false;
grasp = false;
}
double RobotController::onAction(ActionEvent &evt)
{
//graspがtrueのときgraspし続けます。
if( grasp == true)
{
Grasp( grasp_obj,"RARM_LINK7");
}
return 0.1;
}
void RobotController::onRecvMessage(RecvMessageEvent &evt)
{
int size = evt.getSize();
if (size>0)
{
//取得したメッセージを表示します。
string msg = evt.getString(0);
LOG_MSG(("msg : %s", msg.c_str()));
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()));
//Jointを回転させます。
double angle = atof(angle_str.c_str());
my->setJointAngle(msg_j.c_str(), DEG2RAD(angle));
}
//メッセージ"rotation"を受信するとエージェントの体全体が回転します。
if (strcmp(msg.c_str(), "rotation") == 0){
//回転角を設定します。
int dy = 45;
// 自分のy軸周りの回転を得ます(クオータニオン)
double qy = my->qy();
//くオータニオンから回転角(ラジアン)を導出します
double theta = 2*asin(qy);
//体全体を回転させます。
double y = theta + DEG2RAD(45);
if( y >= PI) {
y = y - 2 * PI;
}
my->setAxisAndAngle(0, 1.0, 0, y);
}
//メッセージ"move"を受信すると自分の向いている方向に進みます。
if (strcmp(msg.c_str(), "move") == 0){
//自分の位置を得ます。
double x = my->x();
double y = my->y();
double z = my->z();
//y軸周りの自分の回転を得ます。（クオータニオン）
double qy = my->qy();
//クオータニオンから回転角を導出します。
double theta = 2*asin(qy);
//移動距離を初期化します。
double dx = 0.0;
double dz = 0.0;
//移動する速度を設定します。
double vel = 10;
//移動する方向を決定します。
dx = sin(theta) * vel;
dz = cos(theta) * vel;
//移動します。
my->setPosition( x + dx, y , z + dz );
}
//つかんでいる物体を離します。
if (strcmp(msg.c_str(), "release") == 0){
grasp = false;
grasp_obj = "";
}
}
}
//衝突時に呼び出されます。
void RobotController::onCollision(CollisionEvent &evt) {
if (Colli == false){
typedef CollisionEvent::WithC C;
//触れたエンティティの名前を得ます。
const C & with = evt.getWith();
for (C::const_iterator i=with.begin(); i!=with.end(); i++) {
//触れたエンティティの名前を表示します。
std::string s = *i;
//LOG_MSG(("\"%s\"", s.c_str()));
//触ったエンティティをつかみます。
Grasp(s,"RARM_LINK7");
Colli = true;
}
}
}
//物体をつかむ関数Grasp
void RobotController::Grasp( string obj, char* hand)
{
//自分とつかむ物体を得ます。
SimObj *my = getObj(myname());
SimObj *g_obj = getObj(obj.c_str());
if(!g_obj->dynamics()){
//自分の手のパーツを得ます。
CParts * parts = my->getParts(hand);
if (parts != NULL){
//自分の手の位置とつかむ物体の位置を得ます。
Vector3d hand_pos;
Vector3d obj_pos;
parts->getPosition(hand_pos);
g_obj->getPosition(obj_pos);
//物体をつかみます。
if (hand_pos.x() != obj_pos.x() || hand_pos.y() != obj_pos.y() || hand_pos.z() != obj_pos.z()){
g_obj->setPosition(hand_pos.x(), hand_pos.y(), hand_pos.z());
LOG_MSG(("grasp %d \n", obj.c_str()));
//graspフラグをtrueにします。
grasp = true;
grasp_obj = obj;
}
}
}
}
extern "C" Controller * createController ()
{
return new RobotController;
}
これはエージェントがエンティティに触れる（衝突）するとその触れたエンティティをつかむというサンプルです。
エージェントに"move"のメッセージを送信すると前に進み、"rotation"を送信すると体の向きを変えます。 また、"「関節名」＝「角度」"というメッセージを送信するとエージェントは関節を指定した角度に曲げます。
例） 腰の関節をx軸（エージェント座標）を中心に45°曲げる場合は"WAIST_JOINT1=45"と送信します。JOINT名の定義はこちら?を参照ください。
このサンプルではGraspという関数を新しく作成しました。この関数の1番目の引数に物体をつかむパーツの名前（右手の場合はRARM_LINK7、左手の場合はLARM_LINK7）、2番目の引数に掴む対象となるエンティティの名前を指定します。一度掴むとgraspフラグがtrueとなり、エージェントが移動しても手から離れません。
エージェントに"release"というメッセージを送信するとエージェントは掴んでいる物体を離します。
コンパイルします。
$ emacs Makefile
オブジェクトファイルgrasp.soを指定
#オブジェクトファイルの指定 OBJS = grasp.so
$ make
次に世界ファイルを作成します。視覚に関する操作で使った世界ファイルとほぼ同じものを使います。
$ cd .. $ emacs xml/grasp.xml
grasp.xml
<?xml version="1.0" encoding="utf8"?> <world name="VisTest2"> <gravity x="0.0" y="-9.8" z="0.0"/> <instanciate class="Robot-nii.xml"> <set-attr-value name="name" value="robot_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="0.0"/> <set-attr-value name="y" value="60.0"/> <set-attr-value name="z" value="-40.0"/> <set-attr-value name="vpx" value="0.0"/> <set-attr-value name="vpy" value="70.0"/> <set-attr-value name="vpz" value="5.0"/> <set-attr-value name="vvx" value="0.0"/> <set-attr-value name="vvy" value="0.0"/> <set-attr-value name="vvz" value="1.0"/> <set-attr-value name="language" value="c++"/> <set-attr-value name="implementation" value="./NewWorld/grasp.so"/> </instanciate> <instanciate class="seTV.xml"> <set-attr-value name="name" value="TV_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-20.0"/> <set-attr-value name="y" value="82.0"/> <set-attr-value name="z" value="-250.0"/> <set-attr-value name="visStateAttrName" value="switch"/> <set-attr-value name="switch" value="on"/> </instanciate> <instanciate class="seDoll_Bear.xml"> <set-attr-value name="name" value="kuma_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="0.0"/> <set-attr-value name="y" value="5.0"/> <set-attr-value name="z" value="0.0"/> </instanciate> <instanciate class="seSofa_2seater.xml"> <set-attr-value name="name" value="sofa_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-200.0"/> <set-attr-value name="y" value="20.0"/> <set-attr-value name="z" value="-100.0"/> <set-attr-value name="qw" value="0.707"/> <set-attr-value name="qx" value="0.0"/> <set-attr-value name="qy" value="0.707"/> <set-attr-value name="qz" value="0.0"/> </instanciate> <instanciate class="seTVbass_B.xml"> <set-attr-value name="name" value="TVdai_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-20.0"/> <set-attr-value name="y" value="13.0"/> <set-attr-value name="z" value="-250.0"/> </instanciate> <instanciate class="sePlant_B.xml"> <set-attr-value name="name" value="ki_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="100.0"/> <set-attr-value name="y" value="35.0"/> <set-attr-value name="z" value="-250.0"/> </instanciate> <instanciate class="seSidetable_B.xml"> <set-attr-value name="name" value="sidetable_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-100.0"/> <set-attr-value name="y" value="23.0"/> <set-attr-value name="z" value="-100.0"/> <set-attr-value name="qw" value="0.707"/> <set-attr-value name="qx" value="0.0"/> <set-attr-value name="qy" value="0.707"/> <set-attr-value name="qz" value="0.0"/> </instanciate> <instanciate class="seApple.xml"> <set-attr-value name="name" value="apple_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-50.0"/> <set-attr-value name="y" value="10.0"/> <set-attr-value name="z" value="30.0"/> </instanciate> <instanciate class="seOrange.xml"> <set-attr-value name="name" value="orange_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="70.0"/> <set-attr-value name="y" value="10.0"/> <set-attr-value name="z" value="-30.0"/> </instanciate> <instanciate class="seOrange.xml"> <set-attr-value name="name" value="orange_1"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="80.0"/> <set-attr-value name="y" value="10.0"/> <set-attr-value name="z" value="-10.0"/> </instanciate> </world>
それでは実行してみましょう。
$ ./sigserver.sh -p 9001 -w xml/grasp.xml
SIGViewerでシミュレーションサーバに接続してシミュレーションを開始します。 "move" "rotation" "関節名=角度"等のメッセージを送信するとエージェントは移動したり関節を曲げたりすることができます。これらを組み合わせて何かエンティティに触れてみてください。 エージェントは物体を掴みます。（脚が触れるだけでも掴みます。）
そして一度掴んだら移動したり関節を曲げたりしても手を離さないことを確認してください。
次に掴んだエンティティを移動したい場所に運びます。
最後に離したい場所でメッセージ"release"を送信して手を離します。
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.