| 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 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. |
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 packageTutorial Level: ADVANCED
Contents
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>
7
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
16
17
18
19 class Gripper{
20 private:
21 GripperClient* gripper_client_;
22 ContactClient* contact_client_;
23 SlipClient* slip_client_;
24 EventDetectorClient* event_detector_client_;
25
26 public:
27 //Action client initialization
28 Gripper(){
29
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);
36
37
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 }
42
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 }
46
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 }
50
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 }
55
56 ~Gripper(){
57 delete gripper_client_;
58 delete contact_client_;
59 delete slip_client_;
60 delete event_detector_client_;
61
62 }
63
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
69
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 }
78
79
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
85
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 }
99
100
101 //Slip servo the robot
102 void slipServo(){
103 pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal slip_goal;
104
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 }
113
114
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;
121
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 };
134
135 int main(int argc, char** argv){
136 ros::init(argc, argv, "simple_gripper");
137
138 Gripper gripper;
139
140 // open the hand to prepare for a grasp
141 gripper.open();
142
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);
147
148 // close the gripper until we have a contact on each finger and move into force control mode
149 gripper.findTwoContacts();
150
151 // now hold the object but don't drop it
152 gripper.slipServo();
153
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);
157
158 // now we decided we are ready to put the object down, so tell the gripper that
159 gripper.place();
160
161 // open the gripper once placement has been detected
162 gripper.open();
163
164 return 0;
165 }
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.
