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]] #counter