Up:[[Tutorial]] Previous:[[Collision detection by agents]] Next:[[Control of humanoid agent by BVH file]] ---- #contents * Grasping Function for Humanoid Agent [#j088d666] This tutorial is valid for later than SIGVerse v2.1.1 This tutorial explains how to let a humanoid agent grasp objects. ** Controller [#m44faab5] Create a controller for a humanoid agent. $ cd ~/MyWorld $ emacs grasp.cpp grasp.cpp #highlight(cpp){{ #include <Controller.h> #include <ControllerEvent.h> #include <Logger.h> #include <ViewImage.h> #include <math.h> #define PI 3.141592 #define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 ) using namespace std; class RobotController : public Controller { public: void onInit(InitEvent &evt); double onAction(ActionEvent &evt); void onRecvMsg(RecvMsgEvent &evt); void onCollision(CollisionEvent &evt); private: //flag for grasping string grasp_obj; //parts to be grasped const char* grasp_parts; //flag for collision bool Colli; }; void RobotController::onInit(InitEvent &evt) { Colli = false; //grasping by right hand grasp_parts = "RARM_LINK7"; } double RobotController::onAction(ActionEvent &evt) { return 10.0; } void RobotController::onRecvMsg(RecvMsgEvent &evt) { // Showing message string msg = evt.getMsg(); LOG_MSG(("msg : %s", msg.c_str())); SimObj *my = getObj(myname()); // Receiving a message which includes " "(space) if(strstr(msg.c_str()," ")) { // Separate the message into joint name and angle value string msg_j; string angle_str; int n = 0; n = msg.find(" "); msg_j = msg.substr(0,n); angle_str = msg.substr(n+1); // Showing the received joint name and joint angle LOG_MSG(("Joint Name : %s", msg_j.c_str())); LOG_MSG(("Joint Angle : %s", angle_str.c_str())); // Rotate Joint double angle = atof(angle_str.c_str()); my->setJointAngle(msg_j.c_str(), DEG2RAD(angle)); } // The agent will rotate when a message "rotation" is received. if (strcmp(msg.c_str(), "rotation") == 0){ // Set rotational angle int dy = 45; // Get rotation value (quaternion) around y-axis Rotation rot; my->getRotation(rot); // calculate radian value from quaternion double theta = 2*asin(rot.qy()); // Rotate the body double y = theta + DEG2RAD(45); if( y >= PI) { y = y - 2 * PI; } my->setAxisAndAngle(0, 1.0, 0, y); } // The agent will go forward when a message "move" is come if (strcmp(msg.c_str(), "move") == 0){ // Get current position Vector3d pos; my->getPosition(pos); // Get rotation value (quaternion) around y-axis Rotation rot; my->getRotation(rot); // calculate joint value from quaternion double theta = 2*asin(rot.qy()); // Initialize displacement in the movement double dx = 0.0; double dz = 0.0; // Set velocity double vel = 10; // Set orientation of the motion dx = sin(theta) * vel; dz = cos(theta) * vel; // Execute the motion my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz ); } // Release if (strcmp(msg.c_str(), "release") == 0){ // Get part of hand CParts * parts = my->getParts(grasp_parts); // Execution of release parts->releaseObj(); } } // Eventhandler for the collision void RobotController::onCollision(CollisionEvent &evt) { if (Colli == false){ typedef CollisionEvent::WithC C; // Get the name of entity which is touched by the agent const std::vector<std::string> & with = evt.getWith(); // Get the name of link parts of the agent const std::vector<std::string> & mparts = evt.getMyParts(); // Loop for every entity which is collided for(int i = 0; i < with.size(); i++){ // If the right hand or right arm is touched if(mparts[i] == "RARM_LINK7" || mparts[i] == "RARM_LINK4"){ SimObj *my = getObj(myname()); CParts * parts = my->getParts(grasp_parts); // Execution of grasping parts->graspObj(with[i]); Colli = true; } } } } extern "C" Controller * createController () { return new RobotController; } }} Using the above controller, the agent will grasp an object if the agent touch on the object. *** Operation of the humanoid agent [#rd74fcf7] If you send a message "move", the agent will go forward. The agent change his orientation if a message "rotation" is sent. A message "[joint name] [angle]", the specified [joint] will be bent to the [angle] ex) "WAIST_JOINT1 45" The definition of each joint is explained at [[JointDefinition]] *** Grasping [#y81af1f1] API named graspObj will realize grasping behavior. An argument should be set a target object which should be grasped. The object will be moved along with the agent's hand *** Release [#sf53f43d] If a message "release" is sent to the agent, the agent will release the object. ** Compilation [#le3205eb] $ ./sigmake.sh grasp.cpp ** World file [#f7868e90] The following world file is almost the same as the one which is used in [[Vision sensor]] $ emacs grasp.xml grasp.xml #highlight(xml){{ <?xml version="1.0" encoding="utf8"?> <world name="myworld5"> <gravity x="0.0" y="-980.7" z="0.0"/> <!-- Set of agent name Robot-nii --> <instanciate class="Robot-nii.xml"> <!-- Agent name --> <set-attr-value name="name" value="robot_000"/> <!-- Set C++ as language --> <set-attr-value name="language" value="c++"/> <!-- Set of controller --> <set-attr-value name="implementation" value="./grasp.so"/> <!-- Do not use dynamics calculation --> <set-attr-value name="dynamics" value="false"/> <!-- Initial position of the agent (x,y,z) --> <set-attr-value name="x" value="0.0"/> <set-attr-value name="y" value="54.0"/> <set-attr-value name="z" value="-40.0"/> <!-- ID, link name, orientation and position of camera --> <camera id="1" link="HEAD_LINK" direction="0 -1 1" position="0.0 0.0 5.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <!-- Living room --> <instanciate class="seTV.xml"> <set-attr-value name="name" value="TV_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-20.0"/> <set-attr-value name="y" value="87.5"/> <set-attr-value name="z" value="-250.0"/> <set-attr-value name="visStateAttrName" value="switch"/> <set-attr-value name="switch" value="on"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="seDoll_Bear.xml"> <set-attr-value name="name" value="kuma_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="0.0"/> <set-attr-value name="y" value="9.9"/> <set-attr-value name="z" value="0.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="seToy_D.xml"> <set-attr-value name="name" value="penguin_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="50.0"/> <set-attr-value name="y" value="6.15"/> <set-attr-value name="z" value="-40.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="seSofa_2seater.xml"> <set-attr-value name="name" value="sofa_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-200.0"/> <set-attr-value name="y" value="31.85"/> <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> <instanciate class="seTVbass_B.xml"> <set-attr-value name="name" value="TVdai_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-20.0"/> <set-attr-value name="y" value="25.1"/> <set-attr-value name="z" value="-250.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="sePlant_B.xml"> <set-attr-value name="name" value="ki_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="100.0"/> <set-attr-value name="y" value="56.5"/> <set-attr-value name="z" value="-250.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="seSidetable_B.xml"> <set-attr-value name="name" value="sidetable_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-100.0"/> <set-attr-value name="y" value="16.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> <instanciate class="seApple.xml"> <set-attr-value name="name" value="apple_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="-50.0"/> <set-attr-value name="y" value="3.875"/> <set-attr-value name="z" value="30.0"/> <set-attr-value name="collision" value="true"/> </instanciate> <instanciate class="seOrange.xml"> <set-attr-value name="name" value="orange_0"/> <set-attr-value name="dynamics" value="false"/> <set-attr-value name="x" value="70.0"/> <set-attr-value name="y" value="2.215"/> <set-attr-value name="z" value="-30.0"/> <set-attr-value name="collision" value="true"/> </instanciate> </world> }} ** Execution [#p21b57d2] $ sigserver.sh -w ./grasp.xml You can control the avator wich messages such as "move", "rotation" and "[joint name] [angle]". Let's make the avator touch on something object by the right hand. If the agent touch on a certain object, the object will be grasped. ATTENTION: Since collision detection is executed by rough boundaly condition, the object might not be grasped even if the object is touched by the agent. #ref(./grasp_1.PNG,40%) You can confirm that the target object is moved along with the hand when you bend or move the hand. #ref(./grasp_2.PNG,40%) The a message "release", the robot agent then release the object. #ref(./grasp_3.PNG,40%) You can confirm that the target object never move after the release. #highlight(end) Up:[[Tutorial]] Previous:[[Collision detection by agents]] Next:[[Control of humanoid agent by BVH file]] #counter