Note: This tutorial assumes that you have completed the previous tutorials: pr2_gripper_sensor_action/Tutorials/Grab and Release an Object Using pr2_gripper_sensor_action.
(!) Please ask about problems and questions regarding this tutorial on Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Object Manipulation using find_contact, slip_servo, event_detector, and gripper_action

Description: Advanced tutorial on how to use the low-level actions in the pr2_gripper_sensor_action package

Tutorial Level: ADVANCED

Overview and prerequisites

This tutorial will show you to use the basic pr2_gripper_sensor_action commands of find_contact,slip_servo,event_detector, and gripper_action to delicately grasp and hold an object, and eventually release it based on contact between the object and environment.

This is only a toy example and does not contain any code to actually move the arm. To test this code you could integrate it with code to move the arm, or run it by itself while putting the PR2 into mannequin mode and moving the arm around manually.

Using any of the pr2_gripper_sensor_action commands requires three components:

  • a controller that directly sends commands to the joint motors;
  • an action node that translates your desired actions to low-level commands for the controller;
  • the higher level program that sends your commands to the action node.

The first two of these components are available in ROS as part of the pr2_gripper_sensor_action, pr2_gripper_sensor_controller, and pr2_gripper_sensor_msgs packages. In this tutorial, we will show you how to use them by writing the third component, the higher level program.

Building pr2_gripper_sensor_action

You'll need to build pr2_gripper_sensor_action, if you haven't already.

rosdep install pr2_gripper_sensor_action
rosmake pr2_gripper_sensor_action

After that you can bring up a robot on the hardware.

Package setup

In order to create a ROS node that sends goals to the gripper_sensor_action, the first thing we'll need to do is create a package. To do this we'll use the handy roscreate-pkg command where we want to create the package directory:

roscreate-pkg my_gripper_project roscpp actionlib pr2_gripper_sensor_action

After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace.

roscd my_gripper_project

Launching the controller and action nodes

Currently the pr2_gripper_sensor_controller and pr2_gripper_sensor_action nodes are not brought up by default when you launch the robot. To write a program that uses these resources you will first need to launch the controller and action nodes for both the left and right gripper by running the following launch script:

roslaunch pr2_gripper_sensor_action pr2_gripper_sensor_actions.launch

Creating the Action Client

Now we will make an action client node that grabs and releases an object.

Put the following into src/my_gripper_project.cpp:

   1 #include <ros/ros.h>
   2 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
   3 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactAction.h>
   4 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoAction.h>
   5 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorAction.h>
   6 #include <actionlib/client/simple_action_client.h>
   8 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperSlipServoAction> SlipClient;
   9 // Our Action interface type, provided as a typedef for convenience
  10 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> ContactClient;
  11 // Our Action interface type, provided as a typedef for convenience                   
  12 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperEventDetectorAction> EventDetectorClient;
  13 // Our Action interface type, provided as a typedef for convenience                   
  14 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
  15 // Our Action interface type, provided as a typedef for convenience                   
  19 class Gripper{
  20 private:
  21   GripperClient* gripper_client_;  
  22   ContactClient* contact_client_;
  23   SlipClient* slip_client_;
  24   EventDetectorClient* event_detector_client_;
  26 public:
  27   //Action client initialization
  28   Gripper(){
  30     //Initialize the client for the Action interface to the gripper controller
  31     //and tell the action client that we want to spin a thread by default
  32     gripper_client_ = new GripperClient("r_gripper_sensor_controller/gripper_action", true);
  33     contact_client_  = new ContactClient("r_gripper_sensor_controller/find_contact",true);
  34     slip_client_  = new SlipClient("r_gripper_sensor_controller/slip_servo",true);
  35     event_detector_client_  = new EventDetectorClient("r_gripper_sensor_controller/event_detector",true);
  38     //wait for the gripper action server to come up 
  39     while(!gripper_client_->waitForServer(ros::Duration(5.0))){
  40       ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
  41     }
  43     while(!contact_client_->waitForServer(ros::Duration(5.0))){
  44       ROS_INFO("Waiting for the r_gripper_sensor_controller/find_contact action server to come up");
  45     }
  47     while(!slip_client_->waitForServer(ros::Duration(5.0))){
  48       ROS_INFO("Waiting for the r_gripper_sensor_controller/slip_servo action server to come up");
  49     }    
  51     while(!event_detector_client_->waitForServer(ros::Duration(5.0))){
  52       ROS_INFO("Waiting for the r_gripper_sensor_controller/event_detector action server to come up");
  53     }    
  54   }
  56   ~Gripper(){
  57     delete gripper_client_;
  58     delete contact_client_;
  59     delete slip_client_;
  60     delete event_detector_client_;
  62   }
  64   //Open the gripper
  65   void open(){
  66     pr2_controllers_msgs::Pr2GripperCommandGoal open;
  67     open.command.position = 0.09;    // position open (9 cm)
  68     open.command.max_effort = -1.0;  // unlimited motor effort
  70     ROS_INFO("Sending open goal");
  71     gripper_client_->sendGoal(open);
  72     gripper_client_->waitForResult();
  73     if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
  74       ROS_INFO("The gripper opened!");
  75     else
  76       ROS_INFO("The gripper failed to open.");
  77   }
  80   //Find two contacts on the robot gripper
  81   void findTwoContacts(){
  82     pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
  83     findTwo.command.contact_conditions = findTwo.command.BOTH;  // close until both fingers contact
  84     findTwo.command.zero_fingertip_sensors = true;   // zero fingertip sensor values before moving
  86     ROS_INFO("Sending find 2 contact goal");
  87     contact_client_->sendGoal(findTwo);
  88     contact_client_->waitForResult(ros::Duration(5.0));
  89     if(contact_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
  90     {
  91       ROS_INFO("Contact found. Left: %d, Right: %d", contact_client_->getResult()->data.left_fingertip_pad_contact, 
  92                contact_client_->getResult()->data.right_fingertip_pad_contact);
  93       ROS_INFO("Contact force. Left: %f, Right: %f", contact_client_->getResult()->data.left_fingertip_pad_force, 
  94            contact_client_->getResult()->data.right_fingertip_pad_force);
  95     }
  96     else
  97       ROS_INFO("The gripper did not find a contact.");
  98   }
 101   //Slip servo the robot
 102   void slipServo(){
 103     pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal slip_goal;
 105     ROS_INFO("Slip Servoing");
 106     slip_client_->sendGoal(slip_goal);
 107     //slip_client_->waitForResult();  // thre is no reason to wait since this action never returns a result
 108     if(slip_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
 109       ROS_INFO("You Should Never See This Message!");
 110     else
 111       ROS_INFO("SlipServo Action returned without success.");
 112   }  
 115   //move into event_detector mode to detect object contact
 116   void place(){
 117     pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal place_goal;
 118     place_goal.command.trigger_conditions = place_goal.command.FINGER_SIDE_IMPACT_OR_SLIP_OR_ACC;  
 119     place_goal.command.acceleration_trigger_magnitude = 4.0;  // set the contact acceleration to n m/s^2
 120     place_goal.command.slip_trigger_magnitude = .005;
 122     ROS_INFO("Waiting for object placement contact...");
 123     event_detector_client_->sendGoal(place_goal);
 124     event_detector_client_->waitForResult();
 125     if(event_detector_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
 126     {
 127       ROS_INFO("Place Success");
 128       ROS_INFO("cond met: %d, acc_met: %d, slip_met: %d", event_detector_client_->getResult()->data.trigger_conditions_met, event_detector_client_->getResult()->data.acceleration_event, event_detector_client_->getResult()->data.slip_event);
 129     }
 130     else
 131       ROS_INFO("Place Failure");
 132   }  
 133 };
 135 int main(int argc, char** argv){
 136   ros::init(argc, argv, "simple_gripper");
 138   Gripper gripper;
 140     // open the hand to prepare for a grasp
 143     // wait (you don't have to do this in your code)
 144     // in this demo here is a good time to put an object inside the gripper
 145     // in your code the robot would move its arm around an object
 146     sleep(7.0);
 148     // close the gripper until we have a contact on each finger and move into force control mode
 149     gripper.findTwoContacts();
 151     // now hold the object but don't drop it
 152     gripper.slipServo();
 154     // now we wait some amount of time until we want to put it down
 155     // in your code you probably wouldn't do this but you would move the arm around instead
 156     sleep(8.0);
 158     // now we decided we are ready to put the  object down, so tell the gripper that
 161     // open the gripper once placement has been detected
 164   return 0;
 165 }


Add the following line to the CMakeLists.txt:

rosbuild_add_executable(my_gripper_project src/my_gripper_project.cpp)

and make the binary by typing 'make' in the my_gripper_project directory.


  • Bring up a robot.
  • Make sure the pr2_gripper_sensor controller and action nodes are launched (see above)
  • Start the ROS console (rxconsole) to view any errors that occur.

  • Run your new node that you created:

rosrun my_gripper_project my_gripper_project

The robot should now wait a few seconds, grab an object, wait a few more seconds, and release upon object contact with the environment.

Wiki: pr2_gripper_sensor_action/Tutorials/Object Manipulation using find_contact, slip_servo, event_detector, and gripper_action (last edited 2010-09-01 02:24:12 by JoeRomano)