物体を持つ動作(v2.0系)

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

物体を持つ動作
Old Version

物体を持つ動作 †

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

コントローラ作成 †

物体を持って移動するコントローラを作成します。

$ 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 onRecvMsg(RecvMsgEvent &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.1;
}

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軸周りの回転を得ます(クオータニオン)
    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;
}

これはエージェントがエンティティに触れる（衝突）するとその触れたエンティティをつかむというサンプルです。

↑

エージェントの操作方法 †

エージェントに"move"のメッセージを送信すると前に進み、"rotation"を送信すると体の向きを変えます。また、"「関節名」「角度」"というメッセージを送信するとエージェントは関節を指定した角度に曲げます。

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

↑

掴む †

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

↑

離す †

エージェントに"release"というメッセージを送信するとエージェントは掴んでいる物体を離します。　

↑

コンパイル †

Makefileの変数OBJSにgrasp.soを追加してコンパイルします。

↑

世界ファイル作成 †

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

$ cd ..
$ emacs xml/grasp.xml

grasp.xml

<?xml version="1.0" encoding="utf8"?>
<world name="myworld5">

<instanciate class="Robot-nii.xml">

<set-attr-value name="name" value="robot_000"/>

<set-attr-value name="language" value="c++"/>

<set-attr-value name="implementation"
                    value="./NewWorld/grasp.so"/>

<set-attr-value name="dynamics" value="false"/>

<set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="54.0"/>
    <set-attr-value name="z" value="-40.0"/>

<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="62.0"/>
    <set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="visStateAttrName" value="switch"/>
    <set-attr-value name="switch" value="on"/>
  </instanciate>

</world>

↑

実行 †

それでは実行してみましょう。

$ ./sigserver.sh -p 9001 -w xml/grasp.xml

SIGViewerでシミュレーションサーバに接続してシミュレーションを開始します。 "move"　"rotation"　"関節名角度"等のメッセージを送信するとエージェントは移動したり関節を曲げたりすることができます。これらを組み合わせて右手で何かエンティティに触れてみてください。エージェントは物体を掴みます。

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