Up:[[Tutorial]]　　　 　Previous：[[Image capture from agent's perspective]]　　　　　Next：[[Grasping Function]]
-----

#contents

* Collision detection [#kb473d90]

A sample code in this tutorial make the agent detect collision event and turn around after the collision detection. 


** Creation of controller [#r1eb9ebf]

 $ cd ~/MyWorld
 $ emacs Collision.cpp

Collision.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);

  // Callback function when the collision event happens
  void onCollision(CollisionEvent &evt);

private:

  bool   Colli;
  int    Colli_cnt;
  double vel;
};

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

  // Velocity of the "robot_000"
  if(strstr(myname() , "000")!= NULL)
    {
      vel = 5;
    }

  // Velocity of the "robot_001"
  else  if(strstr(myname() , "001")!= NULL)
    {
      vel = 7;
    }
  else
    vel = 0;
}

// Called when the collision is detected
void AgentController::onCollision(CollisionEvent &evt) {

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

    const vector<string> & wname  = evt.getWith();       // Get a name of the other
    const vector<string> & wparts = evt.getWithParts();  // Get a parts name of the other's collision point
    const vector<string> & mparts = evt.getMyParts();    // Get a parts of collision point of myself

    for(int i = 0; i < wname.size(); i++)
      {
      // Print the name of the other
      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());

      // Refer rotation (quaternion) around y-axis
      double qy = my->qy();

      // Calculate joint angle from the quaternion
      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());

    // Position of myself
    double x = my->x();
    double y = my->y();
    double z = my->z();

    // Refer rotation (quaternion) around y-axis

    double qy = my->qy();

    // Calculate joint angle from the quaternion
    double theta = 2*asin(qy);

    double dx = 0;
    double dz = 0;

    // Determine the moving direction
    dx = sin(theta) * vel;
    dz = cos(theta) * vel;

    // Execute the movement
    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;
}



}}

This controller displays the name of other's agent name if a collision event is detected. After the collision detection, the agent will change the moving direction (making U-turn).
The callback function onCollision is called in the collision event. The function onAction keeps going forward to the current direction.
In this world file, two agents appear and they have different velocity.

** World file [#mfe1a036]

Create a world file.

 $ emacs ./CollisionWorld.xml

CollisionWorld.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<world name="myworld8">

  <gravity x="0.0" y="-980.7" z="0.0"/>
  <instanciate class="Robot-nii.xml">
    <!-- Name of the first agent -->
    <set-attr-value name="name" value="robot_000"/>
    <set-attr-value name="language" value="c++"/>

    <!-- Controller file-->
    <set-attr-value name="implementation"
                    value="./Collision.so"/>
    <set-attr-value name="dynamics" value="false"/>

    <!-- Position of the first agent -->
    <set-attr-value name="x" value="90.0"/>
    <set-attr-value name="y" value="60.0"/>
    <set-attr-value name="z" value="-90.0"/>

    <!-- Direction of the first agent (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 the flag of collision detection function to true -->
    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <instanciate class="Robot-nii.xml">
    <!-- Name of the second agent -->
    <set-attr-value name="name" value="robot_001"/>
    <set-attr-value name="language" value="c++"/>

    <!-- Controller file-->
    <set-attr-value name="implementation"
                    value="./Collision.so"/>
    <set-attr-value name="dynamics" value="false"/>

    <!-- Position of the second agent -->
    <set-attr-value name="x" value="30.0"/>
    <set-attr-value name="y" value="60.0"/>
    <set-attr-value name="z" value="-90.0"/>

    <!-- Orientation of the second agent (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 the flag of collision detection function to true -->
    <set-attr-value name="collision" value="true"/>
  </instanciate>


  <!-- Configuration of a human agent Man-nii -->
  <instanciate class="Man-nii.xml">

    <!-- Name of the avatar agent -->
    <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"/>

    <!-- Orientation the avatar agent (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 the flag of collision detection function to true -->
    <set-attr-value name="collision" value="true"/>
  </instanciate>
</world>
}}

The two robot agents use the same controller "Collision.so"
The flag for collision detection should be set as "true" if you want to use the collision detection function.
Please be careful that the initial state of this flag is "false".

#highlight(xml:firstline[27]){{
    <set-attr-value name="collision" value="true"/>
}}

** Execution [#h3ae0c35]

 $ ./sigserver.sh -w CollisionWorld.xml

#ref(./Colli_1.PNG,40%)

Connect to the server with SIGViewer.

You will see two robots and a human avatar in the world. Let's push the start button to start the simulation.

#ref(./Colli_2.PNG,40%)

First, a robot agent which stands in front of the human avatar will make U-turn after collides with the human avatar.

#ref(./Colli_3.PNG,40%)

Next, the robots collide each other, then make U-turn.

After that, the two robots will repeat the U-turn behavior.


* Addtion of attribute [#g0d29843]

In the above controller samples, two velocity values are set to two robot agents respectively.
It is a little bit troublesome to change the velocity because the compilation is required.
So, the following section introduces how to change the velocity using attribution in the worldfile.

** Modification of entity file [#t32fe4bf]

First, copy the original file to your working space.

 $ cp ~/sigverse-<version>/share/sigverse/data/xml/Robot-nii.xml .

Edit the configuration of the robot agent.

 $ emacs ./Robot-nii.xml

Add the following line.

 <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"/>
 
   <!-- Add an attribution "vel"　-->
   <attr name="vel" type="double" group="velocity" value="0.0"/>
 
   <x3d>
       <filename>nii_robot.x3d</filename>
   </x3d>
 
</define-class>
}}

** Modification of the world file [#wb4c4814]

Next, set velocity values in the world file.

 $ emacs CollisionWorld.xml


Add the line

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

after the line which sets the the attribution of "robot_000"

#highlight(xml:firstline[26]){{
    <!-- Set the flag of collision detection function to true -->
    <set-attr-value name="collision" value="true"/>
    <set-attr-value name="vel" value="5.0"/>
}}


Add the following one line at set of attribution for "robot_001" as well as the above way.

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

** Modification of controller [#hea86854]

Since the velocity is already set by the world file, remove the setting of velocity as follows:

 $ emacs Collision.cpp

Collision.cpp

Old:

#highlight(cpp:firstline[27]){{
 void AgentController::onInit(InitEvent &evt)
 {
   Colli = false;
   Colli_cnt = 0; 
  
  // Velocity of the "robot_000"
  if(strstr(myname() , "000")!= NULL)
    {
      vel = 5;
    }
 
  // Velocity of the "robot_001"
  else  if(strstr(myname() , "001")!= NULL)
    { 
    vel = 7;
    }
  else
    vel = 0;
 }
}}

New:

#highlight(cpp:firstline[27]){{
void AgentController::onInit(InitEvent &evt)
{
   Colli = false;
   Colli_cnt = 0;
  
  // Refer the velocity
  SimObj *my = getObj(myname());
  vel = my->getAttr("vel").value().getDouble();
}
}}

After the compilation, the velocity could be set by the worldfile.

 $ ./sigmake.sh Collision.cpp

Please test the behavior of the controller file and the world file.


#highlight(end)

Up:[[Tutorial]]　　　 　Previous：[[Image capture from agent's perspective]]　　　　　Next：[[Grasping Function]]

----
Up:[[Tutorial]]　　 　　Previous：[[Image capture from agent's perspective]]　　　　　Next：[[Grasping Function]]

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