 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. |
Using the PR2 gripper's reactive actions and services
Description: This tutorial teaches you to call the actions and services contained in pr2_gripper_reactive_approach: reactive grasp, reactive approach, reactive lift, reactive place, compliant close, and grasp adjustment.Keywords: reactive grasp lift place adjustment compliant close pr2 gripper
Tutorial Level: INTERMEDIATE
Robot Setup
Bring up the robot and position it at the edge of a table, facing the table. When manipulating objects on tables, the workspace of the arms is generally increased by raising the robot torso all the way up. Place an object on the table, within reach of the robot. Start the left arm well out of the way, and start the right arm well above the table and somewhat off to the side.
Starting the Manipulation Pipeline
Start the manipulation pipeline (see the relevant tutorial here)
The Code
In the following code, the table height is hard-coded. Measure the height of the table above the ground, and modify the table_height line with the relevant height.
This code is also available in pr2_gripper_reactive_approach/test/test_pr2_gripper_reactive_approach_server.py:
1 #!/usr/bin/python
2 """test client for pr2_gripper_reactive_approach_server
3 requires pr2_gripper_reactive_approach_server.py r (right arm) to be running
4 """
5
6 from __future__ import division
7 import roslib
8 roslib.load_manifest('pr2_gripper_reactive_approach')
9 import rospy
10 import math
11 from geometry_msgs.msg import PoseStamped, PointStamped, \
12 QuaternionStamped, Pose, Point, Quaternion
13 from object_manipulation_msgs.msg import ReactiveGraspAction, \
14 ReactiveGraspGoal, ReactiveLiftAction, ReactiveLiftGoal, \
15 GripperTranslation, ReactivePlaceAction, ReactivePlaceGoal
16 import actionlib
17 from trajectory_msgs.msg import JointTrajectory
18 from pr2_gripper_reactive_approach import controller_manager
19 from std_srvs.srv import Empty
20 from object_manipulator.convert_functions import *
21
22
23 #call reactive_grasp to do a reactive grasp using the fingertip sensors
24 #(backs up and move to the side if the tip contacts on the way to the grasp,
25 #does a compliant grasp if it gets there, tries the approach and grasp
26 #several times if the gripper opening isn't within bounds)
27 def call_reactive_grasp(ac, grasp_pose, trajectory):
28
29 if trajectory == None:
30 trajectory = JointTrajectory()
31
32 goal = ReactiveGraspGoal()
33 goal.final_grasp_pose = grasp_pose
34 goal.trajectory = trajectory
35 goal.collision_support_surface_name = "table"
36
37 ac.send_goal(goal)
38 ac.wait_for_result()
39 result = ac.get_result()
40 print "reactive grasp result:", result
41
42 return result
43
44
45 #call reactive_grasp to do a reactive approach using the fingertip sensors
46 #(backs up and move to the side if the tip contacts on the way to the grasp)
47 def call_reactive_approach(ac, grasp_pose, trajectory):
48
49 if trajectory == None:
50 trajectory = JointTrajectory()
51
52 goal = ReactiveGraspGoal()
53 goal.final_grasp_pose = grasp_pose
54 goal.trajectory = trajectory
55 goal.collision_support_surface_name = "table"
56
57 ac.send_goal(goal)
58 ac.wait_for_result()
59 result = ac.get_result()
60 print "reactive approach result:", result
61
62 return result
63
64
65 #call reactive lift (starts up slip servoing if using the slip controllers,
66 #otherwise just a Cartesian move)
67 def call_reactive_lift(ac, lift):
68
69 goal = ReactiveLiftGoal()
70 goal.lift = lift
71
72 ac.send_goal(goal)
73 ac.wait_for_result()
74 result = ac.get_result()
75 print "reactive lift result:", result
76
77 return result
78
79
80 #call reactive place (uses the slip controllers to detect when the object
81 #hits the table, and opens the gripper)
82 def call_reactive_place(ac, place_pose):
83
84 goal = ReactivePlaceGoal()
85 goal.final_place_pose = place_pose
86
87 ac.send_goal(goal)
88 ac.wait_for_result()
89 result = ac.get_result()
90 print "reactive place result:", result
91
92 return result
93
94
95 #pause for input
96 def keypause():
97 print "press enter to continue"
98 input = raw_input()
99 return input
100
101
102 #move to a Cartesian pose goal
103 def move_cartesian_step(cm, pose, timeout = 10.0,
104 settling_time = 3.0, blocking = 0):
105 if type(pose) == list:
106 pose = create_pose_stamped(pose, 'base_link')
107 cm.move_cartesian(pose, blocking = blocking, \
108 pos_thres = .0025, rot_thres = .05, \
109 timeout = rospy.Duration(timeout), \
110 settling_time = rospy.Duration(settling_time))
111
112
113 #used to call compliant close or grasp adjustment
114 def call_empty_service(service_name, serv):
115 try:
116 serv()
117 except rospy.ServiceException, e:
118 rospy.logerr("error when calling %s,%s"%(service_name,e))
119 return 0
120 return 1
121
122
123 #return the current pos and rot of the wrist for the right arm
124 #as a 7-list (pos, quaternion rot)
125 def return_current_pose_as_list(cm):
126 (pos, rot) = cm.return_cartesian_pose()
127 return pos+rot
128
129
130 #run the test
131 if __name__ == "__main__":
132
133 rospy.init_node('test_reactive_grasp_server', anonymous = True)
134
135 use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_controller')
136 rospy.loginfo("use_slip_detection:"+str(use_slip_detection))
137
138 rospy.loginfo("waiting for compliant_close and grasp_adjustment services")
139 rospy.wait_for_service("r_reactive_grasp/compliant_close")
140 rospy.wait_for_service("r_reactive_grasp/grasp_adjustment")
141 rospy.loginfo("service found")
142
143 rg_ac = actionlib.SimpleActionClient("reactive_grasp/right", \
144 ReactiveGraspAction)
145 ra_ac = actionlib.SimpleActionClient("reactive_approach/right", \
146 ReactiveGraspAction)
147 rl_ac = actionlib.SimpleActionClient("reactive_lift/right", \
148 ReactiveLiftAction)
149 rp_ac = actionlib.SimpleActionClient("reactive_place/right", \
150 ReactivePlaceAction)
151 cc_srv = rospy.ServiceProxy("r_reactive_grasp/compliant_close", Empty)
152 ga_srv = rospy.ServiceProxy("r_reactive_grasp/grasp_adjustment", Empty)
153
154 rospy.loginfo("waiting for reactive grasp action")
155 rg_ac.wait_for_server()
156 rospy.loginfo("reactive grasp action found")
157
158 rospy.loginfo("waiting for reactive approach action")
159 ra_ac.wait_for_server()
160 rospy.loginfo("reactive approach action found")
161
162 rospy.loginfo("waiting for reactive lift action")
163 rl_ac.wait_for_server()
164 rospy.loginfo("reactive lift action found")
165
166 rospy.loginfo("waiting for reactive place action")
167 rp_ac.wait_for_server()
168 rospy.loginfo("reactive place action found")
169
170 cm = controller_manager.ControllerManager('r', \
171 using_slip_controller = use_slip_detection)
172
173 #joint names for the right arm
174 joint_names = ["r_shoulder_pan_joint",
175 "r_shoulder_lift_joint",
176 "r_upper_arm_roll_joint",
177 "r_elbow_flex_joint",
178 "r_forearm_roll_joint",
179 "r_wrist_flex_joint",
180 "r_wrist_roll_joint"]
181
182 #reactive approach from above
183 #table_height = .55 #simulated table
184 table_height = .7239 #round table
185 tip_dist_to_table = .165
186 wrist_height = table_height + tip_dist_to_table + .02
187
188 approachpos = [.52, -.05, wrist_height+.1]
189 approachquat = [-0.5, 0.5, 0.5, 0.5] #from the top
190
191 current_goal = approachpos[:] + approachquat[:]
192 small_step = .02
193
194 while(not rospy.is_shutdown()):
195 print "enter:"
196 print "appi to go to the approach position with interpolated IK"
197 print "appc to go to the approach position with Cartesian controllers"
198 print "appm to go to the approach position with move_arm"
199 print "rg to grasp reactively, ra to just approach reactively"
200 print "cc to do a compliant close"
201 print "rl to do a reactive lift"
202 print "rp to do a reactive place"
203 print "ga to do a grasp adjustment"
204 print "c to close, o to open"
205 print "u to go up, d to go down, r to go right, l to go left"
206 print "a to go away from the robot, t to go toward the robot"
207 print "s to stop"
208 c = keypause()
209 pregrasp_pose = create_pose_stamped(current_goal)
210 if c == 'appi':
211 #Go to the approach position using the interpolated
212 #IK controllers (collision-aware)
213 cm.command_interpolated_ik(pregrasp_pose)
214 elif c == 'appc':
215 #Go to the approach position using the Cartesian controllers
216 cm.command_cartesian(pregrasp_pose)
217 elif c == 'appm':
218 #Go to the approach position using move_arm
219 cm.move_arm_pose(pregrasp_pose, blocking = 1)
220 elif c == 'rg' or c == 'ra':
221 grasp_goal = current_goal[:]
222 grasp_goal[2] -= .1
223 grasp_pose = create_pose_stamped(grasp_goal)
224 start_angles = [0.]*7
225 trajectory = None
226 if c == 'rg':
227 print "calling reactive grasp"
228 result = call_reactive_grasp(rg_ac, grasp_pose, trajectory)
229 else:
230 print "calling reactive approach"
231 result = call_reactive_approach(ra_ac, grasp_pose, trajectory)
232 print "result:", result
233
234 #update current_goal
235 current_goal = return_current_pose_as_list(cm)
236
237 elif c == 'rl':
238 print "calling reactive lift"
239 lift = GripperTranslation()
240 lift.direction = create_vector3_stamped([0,0,1])
241 lift.desired_distance = .1
242 lift.min_distance = 0.
243 result = call_reactive_lift(rl_ac, lift)
244
245 #update current_goal
246 current_goal = return_current_pose_as_list(cm)
247
248 elif c == 'rp':
249 print "calling reactive place"
250 place_goal = current_goal[:]
251 place_goal[2] -= .1
252 place_pose = create_pose_stamped(place_goal)
253 result = call_reactive_place(rp_ac, place_pose)
254
255 #update current_goal
256 current_goal = return_current_pose_as_list(cm)
257
258 elif c == 'cc':
259 print "calling compliant close"
260 call_empty_service("compliant close", cc_srv)
261 elif c == 'ga':
262 print "calling grasp adjustment"
263 call_empty_service("grasp adjustment", ga_srv)
264 elif c == 'c':
265 print "closing gripper"
266 cm.command_gripper(0, 100.0)
267 elif c == 'o':
268 print "opening gripper"
269 cm.command_gripper(.087, -1)
270 elif c == 'u':
271 print "going up"
272 current_goal[2] += .1
273 move_cartesian_step(cm, current_goal, blocking = 1)
274 elif c == 'us':
275 print "going up a small amount"
276 current_goal[2] += .02
277 move_cartesian_step(cm, current_goal, blocking = 1)
278 elif c == 'd':
279 print "going down"
280 current_goal[2] -= .05
281 move_cartesian_step(cm, current_goal, blocking = 1)
282 elif c == 'ds':
283 print "going down a small amount"
284 current_goal[2] -= .02
285 move_cartesian_step(cm, current_goal, blocking = 1)
286 elif c == 'r':
287 print "moving right"
288 current_goal[1] -= small_step
289 move_cartesian_step(cm, current_goal, blocking = 1)
290 elif c == 'l':
291 print "moving left"
292 current_goal[1] += small_step
293 move_cartesian_step(cm, current_goal, blocking = 1)
294 elif c == 'a':
295 print "moving away from robot"
296 current_goal[0] += small_step
297 move_cartesian_step(cm, current_goal, blocking = 1)
298 elif c == 't':
299 print "moving toward the robot"
300 current_goal[0] -= small_step
301 move_cartesian_step(cm, current_goal, blocking = 1)
302
303 elif c == 's':
304 print "quitting"
305 break
The above code runs a keyboard interface that allows you to try out the actions and services provided in the pr2_gripper_reactive_approach package. The initial (hard-coded) approach position is above the table surface, with the gripper x-axis pointed down for a grasp from above. When the script is first run, a good starting place is to type 'appm' to go to the approach position using move_arm, or 'appi' or 'appc' to go there open-loop (not considering collisions from the tilt laser) if move_arm fails to get the arm there.
Once any of the other commands are run, the approach position becomes the current pose of the gripper, so that you can re-position the gripper and have the new grasp goal be 10 cm below the current gripper pose. When the gripper is in a satisfactory pre-grasp pose 10 cm above a desired grasp, make sure the object to be grasped is somewhere below the gripper (perhaps off to the side/under one finger, if you're trying out reactive approach). From there, you can try out any of the functions in the keyboard interface: 'rg' to run the full reactive grasp; 'ra' to run just the approach and to not close the gripper; 'cc' to compliantly close the gripper; 'ga' to adjust a grasp; 'rl' to reactively lift an object within the gripper; 'rp' to reactively put it down again (if the slip controller version of the manipulation pipeline is not running, reactive lift and place are just plain Cartesian movements).
Also available are small Cartesian movements to reposition the gripper as desired by moving up, down, left, right, away from the robot, or toward the robot.
