Collision detection by agents
[
Front page
] [
New
|
List of pages
|
Search
|
Recent changes
|
Help
]
Start:
Up:[[Tutorial]] Previous:[[Image capture from ag...
-----
#contents
* Collision detection [#kb473d90]
A sample code in this tutorial make the agent detect coll...
** 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(); ...
const vector<string> & wparts = evt.getWithParts(); ...
const vector<string> & mparts = evt.getMyParts(); ...
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 i...
The callback function onCollision is called in the collis...
In this world file, two agents appear and they have diffe...
** 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 ...
<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 ...
<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 ...
<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" ...
Please be careful that the initial state of this flag is ...
#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. ...
#ref(./Colli_2.PNG,40%)
First, a robot agent which stands in front of the human a...
#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 ...
It is a little bit troublesome to change the velocity bec...
So, the following section introduces how to change the ve...
** Modification of entity file [#t32fe4bf]
First, copy the original file to your working space.
$ cp ~/sigverse-<version>/share/sigverse/data/xml/Robot-...
Edit the configuration of the robot agent.
$ emacs ./Robot-nii.xml
Add the following line.
<attr name="vel" type="double" group="velocity" value="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=...
<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_0...
#highlight(xml:firstline[26]){{
<!-- Set the flag of collision detection function to ...
<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 "rob...
<set-attr-value name="vel" value="7.0"/>
** Modification of controller [#hea86854]
Since the velocity is already set by the world file, remo...
$ 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 w...
$ ./sigmake.sh Collision.cpp
Please test the behavior of the controller file and the w...
#highlight(end)
Up:[[Tutorial]] Previous:[[Image capture from ag...
#counter
End:
Up:[[Tutorial]] Previous:[[Image capture from ag...
-----
#contents
* Collision detection [#kb473d90]
A sample code in this tutorial make the agent detect coll...
** 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(); ...
const vector<string> & wparts = evt.getWithParts(); ...
const vector<string> & mparts = evt.getMyParts(); ...
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 i...
The callback function onCollision is called in the collis...
In this world file, two agents appear and they have diffe...
** 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 ...
<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 ...
<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 ...
<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" ...
Please be careful that the initial state of this flag is ...
#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. ...
#ref(./Colli_2.PNG,40%)
First, a robot agent which stands in front of the human a...
#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 ...
It is a little bit troublesome to change the velocity bec...
So, the following section introduces how to change the ve...
** Modification of entity file [#t32fe4bf]
First, copy the original file to your working space.
$ cp ~/sigverse-<version>/share/sigverse/data/xml/Robot-...
Edit the configuration of the robot agent.
$ emacs ./Robot-nii.xml
Add the following line.
<attr name="vel" type="double" group="velocity" value="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=...
<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_0...
#highlight(xml:firstline[26]){{
<!-- Set the flag of collision detection function to ...
<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 "rob...
<set-attr-value name="vel" value="7.0"/>
** Modification of controller [#hea86854]
Since the velocity is already set by the world file, remo...
$ 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 w...
$ ./sigmake.sh Collision.cpp
Please test the behavior of the controller file and the w...
#highlight(end)
Up:[[Tutorial]] Previous:[[Image capture from ag...
#counter
Page: