物体を持つ動作

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

物体を持つ動作
Old Version

物体を持つ動作 †

※このチュートリアルはv2.1.1以降に対応しています。

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

コントローラ作成 †

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

$ cd ~/MyWorld
$ 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フラグ
  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;
}

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

↑

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

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

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

↑

掴む †

graspObjという関数を用いて物体をつかみます。引数に掴む対象となるエンティティの名前を指定します。一度掴むとgrasエージェントが移動しても手から離れなくなります。

↑

離す †

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

↑

コンパイル †

$ ./sigmake.sh grasp.cpp

↑

世界ファイル作成 †

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

$ emacs 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="./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="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>

</world>

↑

実行 †

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

$ sigserver.sh  -w ./grasp.xml

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

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