 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Grab and Release an Object Using pr2_gripper_sensor_action
Description: This tutorial will show you how to get the gripper to delicately grab an object, then after a certain amount of time automatically look for a contact event (hopefully the contact of the object and the surface you wanted to place it on), and release the object after contact.Tutorial Level: BEGINNER
Contents
Overview and prerequisites
This tutorial will show you to use the basic pr2_gripper_sensor_action commands of grab and release 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_gripper_sensor_msgs/PR2GripperGrabAction.h>
3 #include <pr2_gripper_sensor_msgs/PR2GripperReleaseAction.h>
4 #include <actionlib/client/simple_action_client.h>
5
6 // Our Action interface type, provided as a typedef for convenience
7 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperGrabAction> GrabClient;
8 // Our Action interface type, provided as a typedef for convenience
9 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperReleaseAction> ReleaseClient;
10
11
12 class Gripper{
13 private:
14 GrabClient* grab_client_;
15 ReleaseClient* release_client_;
16
17 public:
18 //Action client initialization
19 Gripper(){
20
21 //Initialize the client for the Action interface to the gripper controller
22 //and tell the action client that we want to spin a thread by default
23 grab_client_ = new GrabClient("r_gripper_sensor_controller/grab",true);
24 release_client_ = new ReleaseClient("r_gripper_sensor_controller/release",true);
25
26 while(!grab_client_->waitForServer(ros::Duration(5.0))){
27 ROS_INFO("Waiting for the r_gripper_sensor_controller/grab action server to come up");
28 }
29
30 while(!release_client_->waitForServer(ros::Duration(5.0))){
31 ROS_INFO("Waiting for the r_gripper_sensor_controller/release action server to come up");
32 }
33 }
34
35 ~Gripper(){
36 delete grab_client_;
37 delete release_client_;
38 }
39
40 //Open the gripper, find contact on both fingers, and go into slip-servo control mode
41 void grab(){
42 pr2_gripper_sensor_msgs::PR2GripperGrabGoal grip;
43 grip.command.hardness_gain = 0.03;
44
45 ROS_INFO("Sending grab goal");
46 grab_client_->sendGoal(grip);
47 grab_client_->waitForResult(ros::Duration(20.0));
48 if(grab_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
49 ROS_INFO("Successfully completed Grab");
50 else
51 ROS_INFO("Grab Failed");
52 }
53
54 // Look for side impact, finerpad slip, or contact acceleration signals and release the object once these occur
55 void release(){
56 pr2_gripper_sensor_msgs::PR2GripperReleaseGoal place;
57 // set the robot to release on a figner-side impact, fingerpad slip, or acceleration impact with hand/arm
58 place.command.event.trigger_conditions = place.command.event.FINGER_SIDE_IMPACT_OR_SLIP_OR_ACC;
59 // set the acceleration impact to trigger on to 5 m/s^2
60 place.command.event.acceleration_trigger_magnitude = 5.0;
61 // set our slip-gain to release on to .005
62 place.command.event.slip_trigger_magnitude = .01;
63
64
65 ROS_INFO("Waiting for object placement contact...");
66 release_client_->sendGoal(place);
67 release_client_->waitForResult();
68 if(release_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
69 ROS_INFO("Release Success");
70 else
71 ROS_INFO("Place Failure");
72
73 }
74
75
76 };
77
78 int main(int argc, char** argv){
79 ros::init(argc, argv, "simple_gripper");
80
81 Gripper gripper;
82
83 // wait a few seconds so we can put something in the robot's gripper
84 sleep(5.0);
85
86 // grab something
87 gripper.grab();
88
89 // move the robot arm all ove the place here!
90 sleep(5.0);
91
92 // now that we've decided we want to look for contact and let go
93 gripper.release();
94
95
96 return 0;
97 }
Building
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.
Running
- 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.
