![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
Up:[[Tutorial]] Previous：[[エージェントの衝突]] Next:[[BVHファイルの読み込み]] Up:[[Tutorial]] Previous：[[Collision detection by agents]] Next:[[Control of humanoid agent by BVH file]] ---- #contents *物体を持つ動作 [#k9e46a26] * Grasping Function for Humanoid Agent [#j088d666] ※このチュートリアルはv2.1.1以降に対応しています。 This tutorial is valid for later than SIGVerse v2.1.1 ここではロボット型エージェントが物体を持つ動作をするサンプルを紹介します。 This tutorial explains how to let a humanoid agent grasp objects. **コントローラ作成 [#p75fcc04] 物体を持って移動するコントローラを作成します。 ** Controller [#m44faab5] Create a controller for a humanoid agent. $ cd ~/MyWorld $ emacs grasp.cpp grasp.cpp #highlight(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 onRecvMsg(RecvMsgEvent &evt); void onCollision(CollisionEvent &evt); private: //graspフラグ //flag for grasping string grasp_obj; //graspするパーツ //parts to be grasped const char* grasp_parts; //衝突フラグ //flag for collision bool Colli; }; void RobotController::onInit(InitEvent &evt) { Colli = false; //右手でgrasp //grasping by right hand grasp_parts = "RARM_LINK7"; } double RobotController::onAction(ActionEvent &evt) { return 10.0; } void RobotController::onRecvMsg(RecvMsgEvent &evt) { //取得したメッセージを表示します // Showing message string msg = evt.getMsg(); LOG_MSG(("msg : %s", msg.c_str())); SimObj *my = getObj(myname()); //" "(スペース)が含まれるメッセージを受信 // Receiving a message which includes " "(space) if(strstr(msg.c_str()," ")) { //メッセージをスペースの前と後(Joint名と角度)に分けます // Separate the message into joint name and angle value 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名と角度を表示します // Showing the received joint name and joint angle LOG_MSG(("Joint Name : %s", msg_j.c_str())); LOG_MSG(("Joint Angle : %s", angle_str.c_str())); //Jointを回転させます // Rotate Joint double angle = atof(angle_str.c_str()); my->setJointAngle(msg_j.c_str(), DEG2RAD(angle)); } //メッセージ"rotation"を受信するとエージェントの体全体が回転します // The agent will rotate when a message "rotation" is received. if (strcmp(msg.c_str(), "rotation") == 0){ //回転角を設定します // Set rotational angle int dy = 45; //y軸周りの自分の回転を得ます（クオータニオン） // Get rotation value (quaternion) around y-axis Rotation rot; my->getRotation(rot); //くオータニオンから回転角(ラジアン)を導出します // calculate radian value from quaternion double theta = 2*asin(rot.qy()); //体全体を回転させます // Rotate the body double y = theta + DEG2RAD(45); if( y >= PI) { y = y - 2 * PI; } my->setAxisAndAngle(0, 1.0, 0, y); } //メッセージ"move"を受信すると自分の向いている方向に進みます // The agent will go forward when a message "move" is come if (strcmp(msg.c_str(), "move") == 0){ //自分の位置を得ます // Get current position Vector3d pos; my->getPosition(pos); //y軸周りの自分の回転を得ます（クオータニオン） // Get rotation value (quaternion) around y-axis Rotation rot; my->getRotation(rot); //クオータニオンから回転角を導出します // calculate joint value from quaternion double theta = 2*asin(rot.qy()); //移動距離を初期化します // Initialize displacement in the movement double dx = 0.0; double dz = 0.0; //移動する速度を設定します // Set velocity double vel = 10; //移動する方向を決定します // Set orientation of the motion dx = sin(theta) * vel; dz = cos(theta) * vel; //移動します // Execute the motion my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz ); } //つかんでいる物体を離します // Release if (strcmp(msg.c_str(), "release") == 0){ //自分の手のパーツを得ます // Get part of hand CParts * parts = my->getParts(grasp_parts); // 掴んでいる物体を離します // Execution of release parts->releaseObj(); } } //衝突時に呼び出されます。 // Eventhandler for the collision void RobotController::onCollision(CollisionEvent &evt) { if (Colli == false){ typedef CollisionEvent::WithC C; //触れたエンティティの名前を得ます // Get the name of entity which is touched by the agent const std::vector<std::string> & with = evt.getWith(); // 衝突した自分のパーツを得ます // Get the name of link parts of the agent const std::vector<std::string> & mparts = evt.getMyParts(); // 衝突したエンティティでループします // Loop for every entity which is collided for(int i = 0; i < with.size(); i++){ //右手、右腕に衝突した場合 // If the right hand or right arm is touched if(mparts[i] == "RARM_LINK7" || mparts[i] == "RARM_LINK4"){ //自分を取得 SimObj *my = getObj(myname()); //自分の手のパーツを得ます CParts * parts = my->getParts(grasp_parts); // 掴みます // Execution of grasping parts->graspObj(with[i]); Colli = true; } } } } extern "C" Controller * createController () { return new RobotController; } }} これはエージェントがエンティティに触れる（衝突）するとその触れたエンティティを掴むというサンプルです。 ***エージェントの操作方法 [#u3025f1a] エージェントに"move"のメッセージを送信すると前に進み、"rotation"を送信すると体の向きを変えます。 また、"「関節名」 「角度」"というメッセージを送信するとエージェントは関節を指定した角度に曲げます。 例） 腰の関節をx軸（エージェント座標）を中心に45°曲げる場合は"WAIST_JOINT1 45"と送信します。JOINT名の定義は[[こちら>Joint定義一覧]]を参照ください。 Using the above controller, the agent will grasp an object if the agent touch on the object. ***掴む [#v27d193d] graspObjという関数を用いて物体をつかみます。引数に掴む対象となるエンティティの名前を指定します。一度掴むとgrasエージェントが移動しても手から離れなくなります。 ***離す [#y77ddd4d] エージェントに"release"というメッセージを送信するとエージェントは掴んでいる物体を離します。 *** Operation of the humanoid agent [#rd74fcf7] If you send a message "move", the agent will go forward. The agent change his orientation if a message "rotation" is sent. A message "[joint name] [angle]", the specified [joint] will be bent to the [angle] ex) "WAIST_JOINT1 45" The definition of each joint is explained at [[JointDefinition]] *** Grasping [#y81af1f1] API named graspObj will realize grasping behavior. An argument should be set a target object which should be grasped. The object will be moved along with the agent's hand *** Release [#sf53f43d] If a message "release" is sent to the agent, the agent will release the object. **コンパイル [#n4de9a65] ** Compilation [#le3205eb] $ ./sigmake.sh grasp.cpp **世界ファイル作成 [#ad481758] 次に世界ファイルを作成します。視覚に関する操作で使った世界ファイルとほぼ同じものを使います。 ** World file [#f7868e90] The following world file is almost the same as the one which is used in [[Vision sensor]] $ emacs grasp.xml grasp.xml #highlight(xml){{ <?xml version="1.0" encoding="utf8"?> <world name="myworld5"> <gravity x="0.0" y="-980.7" z="0.0"/> <!--エージェントRobot-niiの設定--> <!-- Set of agent name Robot-nii --> <instanciate class="Robot-nii.xml"> <!--エージェント名--> <!-- Agent name --> <set-attr-value name="name" value="robot_000"/> <!--C++言語の指定--> <!-- Set C++ as language --> <set-attr-value name="language" value="c++"/> <!--コントローラの指定--> <!-- Set of controller --> <set-attr-value name="implementation" value="./grasp.so"/> <!--動力学演算をfalseに設定--> <!-- Do not use dynamics calculation --> <set-attr-value name="dynamics" value="false"/> <!--エージェントの位置(x,y,z)--> <!-- Initial position of the agent (x,y,z) --> <set-attr-value name="x" value="0.0"/> <set-attr-value name="y" value="54.0"/> <set-attr-value name="z" value="-40.0"/> <!--カメラのID番号,リンク名、方向、位置の設定--> <!-- ID, link name, orientation and position of camera --> <camera id="1" link="HEAD_LINK" direction="0 -1 1" position="0.0 0.0 5.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <!--リビングルーム--> <!-- Living room --> <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="87.5"/> <set-attr-value name="z" value="-250.0"/> <set-attr-value name="visStateAttrName" value="switch"/> <set-attr-value name="switch" value="on"/> <set-attr-value name="collision" value="true"/> </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="9.9"/> <set-attr-value name="z" value="0.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="seToy_D.xml"> <set-attr-value name="name" value="penguin_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="50.0"/> <set-attr-value name="y" value="6.15"/> <set-attr-value name="z" value="-40.0"/> <set-attr-value name="collision" value="true"/> </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="31.85"/> <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"/> <set-attr-value name="collision" value="true"/> </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="25.1"/> <set-attr-value name="z" value="-250.0"/> <set-attr-value name="collision" value="true"/> </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="56.5"/> <set-attr-value name="z" value="-250.0"/> <set-attr-value name="collision" value="true"/> </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="16.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"/> <set-attr-value name="collision" value="true"/> </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="3.875"/> <set-attr-value name="z" value="30.0"/> <set-attr-value name="collision" value="true"/> </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="2.215"/> <set-attr-value name="z" value="-30.0"/> <set-attr-value name="collision" value="true"/> </instanciate> </world> }} **実行 [#rca7a9d2] それでは実行してみましょう。 ** Execution [#p21b57d2] $ sigserver.sh -w ./grasp.xml SIGViewerでシミュレーションサーバに接続してシミュレーションを開始します。 "move" "rotation" "関節名 角度"等のメッセージを送信するとエージェントは移動したり関節を曲げたりすることができます。これらを組み合わせて右手で何かエンティティに触れてみてください。 エージェントは物体を掴みます。 ※衝突判定は物理演算用の単純形状で行っているため、見た目は衝突しているのになかなかgraspできない場合があります。 You can control the avator wich messages such as "move", "rotation" and "[joint name] [angle]". Let's make the avator touch on something object by the right hand. If the agent touch on a certain object, the object will be grasped. ATTENTION: Since collision detection is executed by rough boundaly condition, the object might not be grasped even if the object is touched by the agent. #ref(物体を持つ動作(v2.0系)/grasp_1.PNG,40%) （※注意）SIGViewerで使われているGUIが日本語キーボードに対応していないため、アンダースコアを入力できないことがあります。その場合はwindowsのキーボードの設定を英語に設定しておくと入力できるようになります。 #ref(./grasp_1.PNG,40%) #ref(物体を持つ動作(v2.0系)/key_1.PNG) You can confirm that the target object is moved along with the hand when you bend or move the hand. そして一度掴んだら移動したり関節を曲げたりしても手を離さないことを確認してください。 #ref(./grasp_2.PNG,40%) #ref(物体を持つ動作(v2.0系)/grasp_2.PNG,40%) The a message "release", the robot agent then release the object. 最後に離したい場所でメッセージ"release"を送信して手を離します。 #ref(./grasp_3.PNG,40%) #ref(物体を持つ動作(v2.0系)/grasp_3.PNG,40%) You can confirm that the target object never move after the release. エージェントが移動したり、手を動かしたりしても掴んだオブジェクトは付いてこなくなります。 #highlight(end) *Old Version [#la93d438] -[[物体を持つ動作_v2.1.0]] -[[物体を持つ動作(v2.0系)]] -[[物体を持つ動作(v120330, v1.4.8)]] Up:[[Tutorial]] Previous：[[Collision detection by agents]] Next:[[Control of humanoid agent by BVH file]] ---- #counter Up:[[Tutorial]] Previous：[[エージェントの衝突]] Next:[[BVHファイルの読み込み]]
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.