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

エージェントの衝突 †

ここではエージェントが衝突したときに衝突したエンティティを検出して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 )


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){
   typedef CollisionEvent::WithC C;

   // 衝突相手の名前を得る。
   const C & with = evt.getWith();
   for (C::const_iterator i=with.begin(); i!=with.end(); i++) {

     //衝突相手の名前をsに代入します。
      std::string s = *i;

     //衝突相手の名前表示します。
     LOG_MSG(("\"%s\"", s.c_str()));
     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;
     }

     //体全体を180°後に回転します。
     my->setAxisAndAngle(0, 1.0, 0, dy);
     Colli = true;
     Colli_cnt = 3;
   }
 }
}

double AgentController::onAction(ActionEvent &evt) {

  try {

   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体のロボットにそれぞれ違った速度を設定しています。

それではコンパイルします。

$ emacs Makefile

オブジェクトファイルの指定を変更します。

#オブジェクトファイルの指定
OBJS     = Colli.so
$ make
↑

世界ファイルの作成 †

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

$ emacs xml/ColliWorld.xml
<?xml version="1.0" encoding="utf8"?>
<world name="VisTest2">

 <gravity x="0.0" y="-9.8" z="0.0"/>

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

<!--エージェント名-->
      <set-attr-value name="name" value="robot_000"/>

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

<!--オブジェクトファイルColli.soの指定-->
      <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"/>

<!--エージェントの向き(x,y,z)  -->
      <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="Robot-nii.xml">
<!--エージェント名-->
      <set-attr-value name="name" value="robot_001"/>

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

<!--オブジェクトファイルColli.soの指定-->
      <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"/>

<!--エージェントの向き(x,y,z)  -->
      <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>


<!--エージェントMan-niiの設定-->
 <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"/>

<!--エージェントの向き(x,y,z)  -->
      <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>
</world>

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

↑

実行 †

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

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

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

Colli_4.jpg

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

Colli_5.jpg

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

Colli_6.jpg

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

Colli_7.jpg

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

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

SIGViewerの下のMessagesタブを見てみると衝突時に検出したエージェントの名前と衝突相手の名前が表示されています。

Colli_8.jpg

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

↑

属性の追加 †

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

↑

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

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

$ emacs -nw xml/Robot-nii.xml

以下の行の追加により速度パラメータ"vel"を追加します。

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

Robot-nii.xml

<?xml version="1.0" encoding="utf8"?>
<define-class name="Robot" inherit="Agent.xml">

  <set-attr-value name="scalex" value="0.7"/>
  <set-attr-value name="scaley" value="0.7"/>
  <set-attr-value name="scalez" value="0.7"/>

  <!-- 属性"vel"を追加　-->
  <attr name="vel" type="double" group="velocity" value="0.0"/>

  <x3d>
      <filename>nii_robot.x3d</filename>
  </x3d>

</define-class>
↑

世界ファイルの編集 †

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

$ emacs xml/ColliWorld.xml

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

<!--エージェントの向き(x,y,z)  -->
      <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="vel" value="5.0"/>

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

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

コントローラの編集 †

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

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

$ cd NewWorld
$ emacs Colli.cpp
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::onInit(InitEvent &evt)
{
  Colli = false;
  Colli_cnt = 0;
 
 //自分の速度を得ます。
 SimObj *my = getObj(myname());
 vel = my->getAttr("vel").value().getDouble();
}

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

$ make

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

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

Front page   New List of pages Search Recent changes   Help   RSS of recent changes