| Note: 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 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. |
Force Control with the PR2 Gripper
Description: This tutorial teaches the user how to use the pr2_gripper_sensor_action package to do force control with the PR2 gripper.Tutorial Level: INTERMEDIATE
Contents
Overview and prerequisites
This tutorial will show you to use the basic pr2_gripper_sensor_action commands of find_contact force_servo and gripper_action to open the gripper, then quickly find contact with an object, and move into a force control mode to hold the object with a user-specified amount of force.
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/PR2GripperForceServoAction.h>
5 #include <actionlib/client/simple_action_client.h>
6
7 // Our Action interface type, provided as a typedef for convenience
8 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperFindContactAction> ContactClient;
9 // Our Action interface type, provided as a typedef for convenience
10 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;
11 // Our Action interface type, provided as a typedef for convenience
12 typedef actionlib::SimpleActionClient<pr2_gripper_sensor_msgs::PR2GripperForceServoAction> ForceClient;
13
14
15 class Gripper{
16 private:
17 GripperClient* gripper_client_;
18 ContactClient* contact_client_;
19 ForceClient* force_client_;
20
21 public:
22 //Action client initialization
23 Gripper(){
24
25 //Initialize the client for the Action interface to the gripper controller
26 //and tell the action client that we want to spin a thread by default
27 gripper_client_ = new GripperClient("r_gripper_sensor_controller/gripper_action", true);
28 contact_client_ = new ContactClient("r_gripper_sensor_controller/find_contact",true);
29 force_client_ = new ForceClient("r_gripper_sensor_controller/force_servo",true);
30
31 //wait for the gripper action server to come up
32 while(!gripper_client_->waitForServer(ros::Duration(5.0))){
33 ROS_INFO("Waiting for the r_gripper_sensor_controller/gripper_action action server to come up");
34 }
35
36 while(!contact_client_->waitForServer(ros::Duration(5.0))){
37 ROS_INFO("Waiting for the r_gripper_sensor_controller/find_contact action server to come up");
38 }
39
40 while(!force_client_->waitForServer(ros::Duration(5.0))){
41 ROS_INFO("Waiting for the r_gripper_sensor_controller/force_servo action server to come up");
42 }
43 }
44
45 ~Gripper(){
46 delete gripper_client_;
47 delete contact_client_;
48 delete force_client_;
49 }
50
51 //Open the gripper
52 void open(){
53 pr2_controllers_msgs::Pr2GripperCommandGoal open;
54 open.command.position = 0.08;
55 open.command.max_effort = -1.0;
56
57 ROS_INFO("Sending open goal");
58 gripper_client_->sendGoal(open);
59 gripper_client_->waitForResult();
60 if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
61 ROS_INFO("The gripper opened!");
62 else
63 ROS_INFO("The gripper failed to open.");
64 }
65
66
67 //Hold somethign with a constant force in the gripper
68 void hold( double holdForce){
69 pr2_gripper_sensor_msgs::PR2GripperForceServoGoal squeeze;
70 squeeze.command.fingertip_force = holdForce; // hold with X N of force
71
72 ROS_INFO("Sending hold goal");
73 force_client_->sendGoal(squeeze);
74 force_client_->waitForResult();
75 if(force_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
76 ROS_INFO("Stable force was achieved");
77 else
78 ROS_INFO("Stable force was NOT achieved");
79 }
80
81
82 //Find two contacts and go into force control mode
83 void findTwoContacts(){
84 pr2_gripper_sensor_msgs::PR2GripperFindContactGoal findTwo;
85 findTwo.command.contact_conditions = findTwo.command.BOTH; // close until both fingers contact
86 findTwo.command.zero_fingertip_sensors = true; // zero fingertip sensor values before moving
87
88
89 ROS_INFO("Sending find 2 contact goal");
90 contact_client_->sendGoal(findTwo);
91 contact_client_->waitForResult(ros::Duration(5.0));
92 if(contact_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
93 {
94 ROS_INFO("Contact found. Left: %d, Right: %d", contact_client_->getResult()->data.left_fingertip_pad_contact,
95 contact_client_->getResult()->data.right_fingertip_pad_contact);
96 ROS_INFO("Contact force. Left: %f, Right: %f", contact_client_->getResult()->data.left_fingertip_pad_force,
97 contact_client_->getResult()->data.right_fingertip_pad_force);
98 }
99 else
100 ROS_INFO("The gripper did not find a contact or could not maintain contact force.");
101 }
102 };
103
104 int main(int argc, char** argv){
105 ros::init(argc, argv, "simple_gripper");
106
107 Gripper gripper;
108
109 gripper.open();
110 sleep(2.0);
111
112 gripper.findTwoContacts();
113 gripper.hold(10.0); // hold with 10 N of force
114
115 return 0;
116 }
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.
