Diff of 物体を持つ動作(v120330, v1.4.8)

The added line is THIS COLOR.
The deleted line is THIS COLOR.
Go to 物体を持つ動作(v120330, v1.4.8).
Deleting diff of 物体を持つ動作(v120330, v1.4.8)
Up:[[Tutorial]]　　 　Previous：[[エージェントの衝突]]　　 Next:[[BVHファイルの読み込み]]　　　
#contents

*物体を持つ動作 [#k9e46a26]
ここではロボット型エージェントが物体を持つ動作をするサンプルを紹介します。

**コントローラ作成 [#p75fcc04]
物体を持って移動するコントローラを作成します。
 $ cd sigverse-<version>/bin/NewWorld
 $ 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 onRecvMessage(RecvMessageEvent &evt); 
  void onCollision(CollisionEvent &evt); 
 
private:
 
  //物体をつかむ関数Graspの利用を宣言します。
  void Grasp(string obj, const char *arm); 
 
  //graspフラグ
  bool grasp;
  string grasp_obj;

  //graspするパーツ
  const char* grasp_parts;

  //衝突フラグ
  bool Colli;
};
 
void RobotController::onInit(InitEvent &evt)
{
  Colli = false;
  grasp = false;

  //右手でgrasp
  grasp_parts = "RARM_LINK7";
} 
 
double RobotController::onAction(ActionEvent &evt)
{
  //graspがtrueのときgraspし続けます。
  if( grasp == true){
    Grasp(grasp_obj, grasp_parts);
  }
  return 0.5;
} 

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 std::vector<std::string> & with = evt.getWith();

    // 衝突した自分のパーツを得ます.
    const std::vector<std::string> & mparts = evt.getMyParts();    

    //　衝突したエンティティでループします.
    for(int i = 0; i < with.size(); i++){

      //右手、右腕に衝突した場合
      if(strcmp(mparts[i].c_str(),"RARM_LINK7") == 0 || strcmp(mparts[i].c_str(),"RARM_LINK4") == 0){

        //触ったエンティティをつかみます。
        Grasp(with[i],grasp_parts);
        Colli = true;
      }
    }
  }
}
 
//物体をつかむ関数Grasp
void RobotController::Grasp( string obj,const 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 %s \n", obj.c_str()));
        
        //graspフラグをtrueにします。
        grasp = true;
        grasp_obj = obj;
        
      }     
    }
  }
} 
extern "C" Controller * createController ()
{
  return new RobotController;
}



}}
これはエージェントがエンティティに触れる（衝突）するとその触れたエンティティをつかむというサンプルです。
***エージェントの操作方法 [#u3025f1a]
エージェントに"move"のメッセージを送信すると前に進み、"rotation"を送信すると体の向きを変えます。
また、"「関節名」＝「角度」"というメッセージを送信するとエージェントは関節を指定した角度に曲げます。

例）　腰の関節をx軸（エージェント座標）を中心に45°曲げる場合は"WAIST_JOINT1=45"と送信します。JOINT名の定義は[[こちら>Joint定義一覧]]を参照ください。

***掴む [#v27d193d]
このサンプルではGraspという関数を新しく作成しました。この関数の1番目の引数に物体をつかむパーツの名前（右手の場合はRARM_LINK7、左手の場合はLARM_LINK7）、2番目の引数に掴む対象となるエンティティの名前を指定します。一度掴むとgraspフラグがtrueとなり、エージェントが移動しても手から離れません。

***離す [#y77ddd4d]
エージェントに"release"というメッセージを送信するとエージェントは掴んでいる物体を離します。
　
**コンパイル [#n4de9a65]
コンパイルします。

 $ emacs Makefile

オブジェクトファイルgrasp.soを指定
 #オブジェクトファイルの指定
 OBJS     = grasp.so

 $ make

**世界ファイル作成 [#ad481758]
次に世界ファイルを作成します。視覚に関する操作で使った世界ファイルとほぼ同じものを使います。

 $ cd ..
 $ emacs xml/grasp.xml

grasp.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
 <world name="myworld9">
 
  <gravity x="0.0" y="-9.8" z="0.0"/>
 
  <instanciate class="Robot-nii-v2.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="10.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>
}}
**実行 [#rca7a9d2]
それでは実行してみましょう。
 $ ./sigserver.sh -p 9001 -w xml/grasp.xml
SIGViewerでシミュレーションサーバに接続してシミュレーションを開始します。
"move"　"rotation"　"関節名=角度"等のメッセージを送信するとエージェントは移動したり関節を曲げたりすることができます。これらを組み合わせて右手で何かエンティティに触れてみてください。
エージェントは物体を掴みます。

衝突判定は物理演算用の単純形状で行っているため、見た目は衝突しているのになかなかgraspできない場合があります。
※衝突判定は物理演算用の単純形状で行っているため、見た目は衝突しているのになかなかgraspできない場合があります。

#ref(./grasp_1.PNG,50%)

そして一度掴んだら移動したり関節を曲げたりしても手を離さないことを確認してください。

#ref(grasp_2.jpg)

次に掴んだエンティティを移動したい場所に運びます。
#ref(grasp_3.jpg)

最後に離したい場所でメッセージ"release"を送信して手を離します。

#ref(grasp_4.jpg)

エージェントが移動したり、手を動かしたりしても掴んだクマのぬいぐるみはついてこなくなります。

#highlight(end)

Up:[[Tutorial]]　　 　Previous：[[エージェントの衝突]]　　　　Next:[[BVHファイルの読み込み]]