Up:[[Tutorial]] Previous:[[エージェントの衝突]] Next:[[BVHファイルの読み込み]] ---- #contents *物体を持つ動作 [#k9e46a26] ※このチュートリアルはv2.1.1以降に対応しています。 ここではロボット型エージェントが物体を持つ動作をするサンプルを紹介します。 **コントローラ作成 [#p75fcc04] 物体を持って移動するコントローラを作成します。 $ 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フラグ string grasp_obj; //graspするパーツ const char* grasp_parts; //衝突フラグ bool Colli; }; void RobotController::onInit(InitEvent &evt) { Colli = false; //右手でgrasp grasp_parts = "RARM_LINK7"; } double RobotController::onAction(ActionEvent &evt) { return 10.0; } void RobotController::onRecvMsg(RecvMsgEvent &evt) { //取得したメッセージを表示します string msg = evt.getMsg(); 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軸周りの自分の回転を得ます(クオータニオン) Rotation rot; my->getRotation(rot); //くオータニオンから回転角(ラジアン)を導出します double theta = 2*asin(rot.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){ //自分の位置を得ます Vector3d pos; my->getPosition(pos); //y軸周りの自分の回転を得ます(クオータニオン) Rotation rot; my->getRotation(rot); //クオータニオンから回転角を導出します double theta = 2*asin(rot.qy()); //移動距離を初期化します double dx = 0.0; double dz = 0.0; //移動する速度を設定します double vel = 10; //移動する方向を決定します dx = sin(theta) * vel; dz = cos(theta) * vel; //移動します my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz ); } //つかんでいる物体を離します if (strcmp(msg.c_str(), "release") == 0){ //自分の手のパーツを得ます CParts * parts = my->getParts(grasp_parts); // 掴んでいる物体を離します parts->releaseObj(); } } //衝突時に呼び出されます。 void RobotController::onCollision(CollisionEvent &evt) { if (Colli == false){ typedef CollisionEvent::WithC C; //触れたエンティティの名前を得ます const std::vector<std::string> & with = evt.getWith(); // 衝突した自分のパーツを得ます const std::vector<std::string> & mparts = evt.getMyParts(); // 衝突したエンティティでループします for(int i = 0; i < with.size(); i++){ //右手、右腕に衝突した場合 if(mparts[i] == "RARM_LINK7" || mparts[i] == "RARM_LINK4"){ //自分を取得 SimObj *my = getObj(myname()); //自分の手のパーツを得ます CParts * parts = my->getParts(grasp_parts); // 掴みます parts->graspObj(with[i]); Colli = true; } } } } extern "C" Controller * createController () { return new RobotController; } }} これはエージェントがエンティティに触れる(衝突)するとその触れたエンティティを掴むというサンプルです。 ***エージェントの操作方法 [#u3025f1a] エージェントに"move"のメッセージを送信すると前に進み、"rotation"を送信すると体の向きを変えます。 また、"「関節名」 「角度」"というメッセージを送信するとエージェントは関節を指定した角度に曲げます。 例) 腰の関節をx軸(エージェント座標)を中心に45°曲げる場合は"WAIST_JOINT1 45"と送信します。JOINT名の定義は[[こちら>Joint定義一覧]]を参照ください。 ***掴む [#v27d193d] graspObjという関数を用いて物体をつかみます。引数に掴む対象となるエンティティの名前を指定します。一度掴むとgrasエージェントが移動しても手から離れなくなります。 ***離す [#y77ddd4d] エージェントに"release"というメッセージを送信するとエージェントは掴んでいる物体を離します。 **コンパイル [#n4de9a65] $ ./sigmake.sh grasp.cpp **世界ファイル作成 [#ad481758] 次に世界ファイルを作成します。視覚に関する操作で使った世界ファイルとほぼ同じものを使います。 $ 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の設定--> <instanciate class="Robot-nii.xml"> <!--エージェント名--> <set-attr-value name="name" value="robot_000"/> <!--C++言語の指定--> <set-attr-value name="language" value="c++"/> <!--コントローラの指定--> <set-attr-value name="implementation" value="./grasp.so"/> <!--動力学演算をfalseに設定--> <set-attr-value name="dynamics" value="false"/> <!--エージェントの位置(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番号,リンク名、方向、位置の設定--> <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> <!--リビングルーム--> <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] それでは実行してみましょう。 $ sigserver.sh -w ./grasp.xml SIGViewerでシミュレーションサーバに接続してシミュレーションを開始します。 "move" "rotation" "関節名 角度"等のメッセージを送信するとエージェントは移動したり関節を曲げたりすることができます。これらを組み合わせて右手で何かエンティティに触れてみてください。 エージェントは物体を掴みます。 ※衝突判定は物理演算用の単純形状で行っているため、見た目は衝突しているのになかなかgraspできない場合があります。 #ref(物体を持つ動作(v2.0系)/grasp_1.PNG,40%) (※注意)SIGViewerで使われているGUIが日本語キーボードに対応していないため、アンダースコアを入力できないことがあります。その場合はwindowsのキーボードの設定を英語に設定しておくと入力できるようになります。 #ref(物体を持つ動作(v2.0系)/key_1.PNG) そして一度掴んだら移動したり関節を曲げたりしても手を離さないことを確認してください。 #ref(物体を持つ動作(v2.0系)/grasp_2.PNG,40%) 最後に離したい場所でメッセージ"release"を送信して手を離します。 #ref(物体を持つ動作(v2.0系)/grasp_3.PNG,40%) エージェントが移動したり、手を動かしたりしても掴んだオブジェクトは付いてこなくなります。 #highlight(end) *Old Version [#la93d438] -[[物体を持つ動作_v2.1.0]] -[[物体を持つ動作(v2.0系)]] -[[物体を持つ動作(v120330, v1.4.8)]] ---- Up:[[Tutorial]] Previous:[[エージェントの衝突]] Next:[[BVHファイルの読み込み]]