Backup diff of Collision detection by agents (No. 1)

[ Front page ]   [ New | List of pages | Search | Recent changes | Help ]

• List of Backups
• View the diff current.
• View the source.
• View the backup.
• 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)

---

• The added line is THIS COLOR.
• The deleted line is THIS COLOR.

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

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

**コントローラ作成 [#b16b5ba2]
 $ cd ~/sigverse-<version>/bin/NewWorld
 $ emacs Colli.cpp

Colli.cpp

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

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

 $ emacs Makefile

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

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

 $ make

**世界ファイルの作成 [#v96d9bc1]
世界ファイルを作成します。
 $ cd ..
 $ emacs xml/ColliWorld.xml

ColliWorld.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
 <world name="myworld8">
 
  <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体登場します。それぞれ同じコントローラを割り当てます。

**実行 [#ge5926ae]
それでは実行してみましょう。

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

#ref(Colli_3.jpg)

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

#ref(Colli_4.jpg)
まず前にいるロボットが人間と衝突して前にいるロボットだけがUターンします。

#ref(Colli_5.jpg)
次にロボット同士が衝突してそれぞれUターンします。

#ref(Colli_6.jpg)

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

#ref(Colli_7.jpg)

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

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


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

#ref(Colli_8.jpg)


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

**エンティティファイルの編集 [#xcda9213]
世界ファイルが読み込むロボットの設定ファイルを編集します。

 $ emacs -nw xml/Robot-nii.xml

以下の行の追加により速度パラメータ"vel"を追加します。
 <attr name="vel" type="double" group="velocity" value="0.0"/>

Robot-nii.xml
#highlight(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>
}}

**世界ファイルの編集 [#l72a2513]

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

 $ emacs xml/ColliWorld.xml

"robot_000"の属性の値を設定しているところ
#highlight(xml:firstline[24]){{
 <!--エージェントの向き(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"/>

**コントローラの編集 [#v237b73a]
次にコントローラの編集を行います。

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

 $ cd NewWorld
 $ emacs Colli.cpp

Colli.cpp

#highlight(cpp:firstline[27]){{
 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;
 }
}}
　　　　　↓

#highlight(cpp:firstline[27]){{
  void AgentController::onInit(InitEvent &evt)
 {
   Colli = false;
   Colli_cnt = 0;
  
  //自分の速度を得ます。
  SimObj *my = getObj(myname());
  vel = my->getAttr("vel").value().getDouble();
 }
}}
これでコンパイルするとエージェントの速度は世界ファイルで設定可能となります。

 $ make

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

**衝突判定フラグ [#tc922a90]
衝突判定を行うかどうかのフラグを設定することができます。
世界ファイルの衝突判定を行わないエージェントの箇所に以下の行を一行追加します。


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

属性"collision"をfalseに設定した場合そのエンティティは衝突判定を行わなくなります。

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

#highlight(end)

*更新履歴 [#cf12fda7]
-[[エージェントの衝突(v120330, v1.4.8)]]

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

     

Site admin: inamura

PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.
Based on "PukiWiki" 1.3 by yu-ji. Powered by PHP 5.5.9-1ubuntu4.29. HTML convert time: 0.006 sec.