Backup of Collision detection by agents(No. 4)

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：Image capture from agent's perspective　　　　　Next：Grasping Function

Collision detection
Addtion of attribute

Collision detection †

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

Creation of controller †

$ cd ~/MyWorld
$ emacs Collision.cpp

Collision.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 †

Create a world file.

$ emacs ./CollisionWorld.xml

CollisionWorld.xml

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

<set-attr-value name="implementation"
                    value="./Collision.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="./Collision.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="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>

<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>

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".

↑

Execution †

$ ./sigserver.sh -w CollisionWorld.xml

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.

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

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

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

↑

Addtion of attribute †

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 †

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

↑

Modification of the world file †

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"

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 †

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

$ emacs Collision.cpp

Collision.cpp

Old:

New:

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.