Grasping Function

Up:Tutorial?　　　Previous：Collision detection by agents　　 Next:Control of humanoid agent by BVH file

Grasping Function for Humanoid Agent

Grasping Function for Humanoid Agent †

This tutorial is valid for later than SIGVerse v2.1.1

This tutorial explains how to let a humanoid agent grasp objects.

Controller †

Create a controller for a humanoid agent.

$ cd ~/MyWorld
$ emacs grasp.cpp

grasp.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){
    // 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 †

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 †

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 †

If a message "release" is sent to the agent, the agent will release the object. 　

↑

Compilation †

$ ./sigmake.sh grasp.cpp

↑

World file †

The following world file is almost the same as the one which is used in Vision sensor

$ emacs grasp.xml

grasp.xml

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

<instanciate class="Robot-nii.xml">

<set-attr-value name="name" value="robot_000"/>

<set-attr-value name="language" value="c++"/>

<set-attr-value name="implementation"
                    value="./grasp.so"/>

<set-attr-value name="dynamics" value="false"/>

<set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="54.0"/>
    <set-attr-value name="z" value="-40.0"/>

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

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

</world>

↑

Execution †

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

You can confirm that the target object is moved along with the hand when you bend or move the hand.

The a message "release", the robot agent then release the object.

You can confirm that the target object never move after the release.

Up:Tutorial?　　　Previous：Collision detection by agents　　 Next:Control of humanoid agent by BVH file

Counter: 4241, today: 2, yesterday: 0

recent(20)