Diff of Grasping Function

The added line is THIS COLOR.
The deleted line is THIS COLOR.
Go to Grasping Function.
Deleting diff of Grasping Function
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