Backup of Collision detection by agents(No. 3)

List of Backups
View the diff.
View the diff current.
View the source.
Go to Collision detection by agents.
- 1 (2012-10-31 (Wed) 17:19:25)
- 2 (2012-10-31 (Wed) 18:41:03)
- 3 (2012-11-01 (Thu) 10:52:52)
- 4 (2014-01-13 (Mon) 03:28:14)

Up:Tutorial?　　　　Previous：エージェント視点の画像取得?　　　　　Next：物体を持つ動作?

エージェントの衝突
属性の追加
謝辞
Old Version

エージェントの衝突 †

ここではエージェントが衝突したときに衝突したエンティティを検出してUターンするサンプルを紹介します。

コントローラ作成 †

$ cd ~/sigverse-<version>/bin/NewWorld
$ emacs Colli.cpp

Colli.cpp

#include "Controller.h"
#include "ControllerEvent.h"
#include "Logger.h"
#include "math.h"

#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )

using namespace std;

class AgentController: public Controller {
public:
  double onAction(ActionEvent &evt);

void onInit(InitEvent &evt);

//衝突時に呼出される関数onCollisionの利用を宣言します。
  void onCollision(CollisionEvent &evt);

private:

//衝突フラグ
  bool Colli;
  int Colli_cnt;
  double vel;
};

void AgentController::onInit(InitEvent &evt)
{
  Colli = false;
  Colli_cnt = 0;

//"robot_000"の速度設定
  if(strstr(myname() , "000")!= NULL)
    {
      vel = 5;
    }

//"robot_001"の速度設定
  else  if(strstr(myname() , "001")!= NULL)
    {
      vel = 7;
    }
  else
    vel = 0;
}
//衝突時に呼び出されます。
void AgentController::onCollision(CollisionEvent &evt) {

if (Colli == false && Colli_cnt == 0){

const vector<string> & wname= evt.getWith();        // 衝突相手の名前を得る。
    const vector<string> & wparts = evt.getWithParts();  // 衝突した相手のパーツを得る
    const vector<string> & mparts = evt.getMyParts();    // 衝突した自分のパーツを得る

for(int i = 0; i < wname.size(); i++)
      {
      //衝突相手の名前表示します。
      LOG_MSG(("\"%s\"", wname[i].c_str()));
      LOG_MSG(("\"%s\"", wparts[i].c_str()));
      LOG_MSG(("\"%s\"", mparts[i].c_str()));

//LOG_MSG(("\"%s\"", myParts));
      SimObj *my = getObj(myname());

// 自分のy軸周りの回転を得ます(クオータニオン)
      double qy = my->qy();

//クオータニオンから回転角を導出します
      double theta = 2*asin(qy);

double dy = theta + DEG2RAD(-180);
      if (dy <= -PI) {
        dy = -1*dy - PI;
      }

my->setAxisAndAngle(0, 1.0, 0, dy);
      Colli = true;
      Colli_cnt = 3;
    }
  }
}

double AgentController::onAction(ActionEvent &evt) {

try {

Controller * con;
    //SimObj *my1 = con->getObj(myname());
    SimObj *my = getObj(myname());

//自分の位置を得ます。
    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;
    double dz = 0;

//移動する方向を決定します。
    dx = sin(theta) * vel;
    dz = cos(theta) * vel;

//移動します。
    my->setPosition( x + dx, y , z + dz );

if (Colli_cnt > 0)
      {
        if (--Colli_cnt <=0)
          Colli = false;
      }

} catch (SimObj::Exception &) {
  }

return 0.5;

}
extern "C" Controller * createController() {
  return new AgentController;
}

このサンプルコードは衝突を検出したら衝突した相手のエンティティ名を表示して、Uターンするというものです。衝突時に関数onCollisionが呼び出され、体を180°後ろに体を回転させます。onActionでは常に自分の向いている方向に進みます。また、このサンプルコードは2体のロボットが登場することを想定して、2体のロボットにそれぞれ違った速度を設定しています。

MakefileにColli.soを追加してコンパイルします。

↑

世界ファイルの作成 †

世界ファイルを作成します。

$ cd ..
$ emacs xml/ColliWorld.xml

ColliWorld.xml

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

<set-attr-value name="implementation"
                    value="./NewWorld/Colli.so"/>
    <set-attr-value name="dynamics" value="false"/>

<set-attr-value name="x" value="90.0"/>
    <set-attr-value name="y" value="60.0"/>
    <set-attr-value name="z" value="-90.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>

<set-attr-value name="implementation"
                    value="./NewWorld/Colli.so"/>
    <set-attr-value name="dynamics" value="false"/>

<set-attr-value name="x" value="30.0"/>
    <set-attr-value name="y" value="60.0"/>
    <set-attr-value name="z" value="-90.0"/>

<set-attr-value name="collision" value="true"/>
  </instanciate>

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

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

<set-attr-value name="x" value="-60.0"/>
    <set-attr-value name="y" value="60.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>
</world>

ここではロボットが2体登場します。それぞれ同じコントローラを割り当てます。

衝突判定を行いたいエンティティは以下のように属性"collision"を"true"に設定しておきます。デフォルトでは"false"となっているので注意が必要です。

↑

実行 †

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

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

サーバに接続するとロボットが2体いて、人間が通せんぼしているのがわかります。それではシミュレーションを開始してみましょう。

まず前にいるロボットが人間と衝突して前にいるロボットだけがUターンします。

次にロボット同士が衝突してそれぞれUターンします。

この後、先ほどと同様に前にいるロボットがまた人間と衝突してUターンして、後ろにいるロボットを追いかけますが、前にいるロボットのほうが速度を早く設定しているため、いずれ後ろのロボットに追い付きます。

追い付いたロボット2体は再びUターンをして人間の方向に進みます。

延々とこの動作を繰り返します。

↑

属性の追加 †

上のサンプルコントローラではロボットのエージェントごとに速度が設定されています。パラメータの値を変えるたびにコンパイルするのは面倒なので、ここではエージェントに属性として速度を追加して、世界ファイルでパラメータの値を設定する方法を紹介します。

↑

エンティティファイルの編集 †

世界ファイルが読み込むロボットの設定ファイルを編集します。

$ emacs -nw xml/Robot-nii.xml

以下の行を追加します。

<attr name="vel" type="double" group="velocity" value="0.0"/>

Robot-nii.xml

↑

世界ファイルの編集 †

次に世界ファイルでエージェントごとに速度を設定します。

$ emacs xml/ColliWorld.xml

"robot_000"の属性の値を設定しているところ

の下に以下の一行を加えます。

     <set-attr-value name="vel" value="5.0"/>

同様に"robot_001"の属性値を設定している箇所に以下を追加します。

    <set-attr-value name="vel" value="7.0"/>

↑

コントローラの編集 †

次にコントローラの編集を行います。

速度パラメータvelは世界ファイルで設定を行ったので、onInit関数の中のvelの設定箇所を以下のように変更します。

$ cd NewWorld
$ emacs Colli.cpp

Colli.cpp

　　　　　↓

これでコンパイルするとエージェントの速度は世界ファイルで設定可能となります。

$ make

正常に動作することを確認してみてください。

↑

謝辞 †

このサンプルは奈良先端技術大学院大学の柴田智広先生にアドバイスをいただきました。ありがとうございました。

↑

Old Version †

エージェントの衝突(v120330, v1.4.8)?