- The added line is THIS COLOR.
- The deleted line is THIS COLOR.
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