$search
00001 #!/usr/bin/python 00002 """test client for pr2_gripper_reactive_approach_server 00003 requires pr2_gripper_reactive_approach_server.py r (right arm) to be running 00004 """ 00005 00006 from __future__ import division 00007 import roslib 00008 roslib.load_manifest('pr2_gripper_reactive_approach') 00009 import rospy 00010 import math 00011 from geometry_msgs.msg import PoseStamped, PointStamped, \ 00012 QuaternionStamped, Pose, Point, Quaternion 00013 from object_manipulation_msgs.msg import ReactiveGraspAction, \ 00014 ReactiveGraspGoal, ReactiveLiftAction, ReactiveLiftGoal, \ 00015 GripperTranslation, ReactivePlaceAction, ReactivePlaceGoal 00016 import actionlib 00017 from trajectory_msgs.msg import JointTrajectory 00018 from pr2_gripper_reactive_approach import controller_manager 00019 from std_srvs.srv import Empty 00020 from object_manipulator.convert_functions import * 00021 00022 00023 #call reactive_grasp to do a reactive grasp using the fingertip sensors 00024 #(backs up and move to the side if the tip contacts on the way to the grasp, 00025 #does a compliant grasp if it gets there, tries the approach and grasp 00026 #several times if the gripper opening isn't within bounds) 00027 def call_reactive_grasp(ac, grasp_pose, trajectory): 00028 00029 if trajectory == None: 00030 trajectory = JointTrajectory() 00031 00032 goal = ReactiveGraspGoal() 00033 goal.final_grasp_pose = grasp_pose 00034 goal.trajectory = trajectory 00035 goal.collision_support_surface_name = "table" 00036 00037 ac.send_goal(goal) 00038 ac.wait_for_result() 00039 result = ac.get_result() 00040 print "reactive grasp result:", result 00041 00042 return result.manipulation_result 00043 00044 00045 #call reactive_grasp to do a reactive approach using the fingertip sensors 00046 #(backs up and move to the side if the tip contacts on the way to the grasp) 00047 def call_reactive_approach(ac, grasp_pose, trajectory): 00048 00049 if trajectory == None: 00050 trajectory = JointTrajectory() 00051 00052 goal = ReactiveGraspGoal() 00053 goal.final_grasp_pose = grasp_pose 00054 goal.trajectory = trajectory 00055 goal.collision_support_surface_name = "table" 00056 00057 ac.send_goal(goal) 00058 ac.wait_for_result() 00059 result = ac.get_result() 00060 print "reactive approach result:", result 00061 00062 return result.manipulation_result 00063 00064 00065 #call reactive lift (starts up slip servoing if using the slip controllers, 00066 #otherwise just a Cartesian move) 00067 def call_reactive_lift(ac, lift): 00068 00069 goal = ReactiveLiftGoal() 00070 goal.lift = lift 00071 00072 ac.send_goal(goal) 00073 ac.wait_for_result() 00074 result = ac.get_result() 00075 print "reactive lift result:", result 00076 00077 return result 00078 00079 00080 #call reactive place (uses the slip controllers to detect when the object 00081 #hits the table, and opens the gripper) 00082 def call_reactive_place(ac, place_pose): 00083 00084 goal = ReactivePlaceGoal() 00085 goal.final_place_pose = place_pose 00086 00087 ac.send_goal(goal) 00088 ac.wait_for_result() 00089 result = ac.get_result() 00090 print "reactive place result:", result 00091 00092 return result 00093 00094 00095 #pause for input 00096 def keypause(): 00097 print "press enter to continue" 00098 input = raw_input() 00099 return input 00100 00101 00102 #move to a Cartesian pose goal 00103 def move_cartesian_step(cm, pose, timeout = 10.0, 00104 settling_time = 3.0, blocking = 0): 00105 if type(pose) == list: 00106 pose = create_pose_stamped(pose, 'base_link') 00107 cm.move_cartesian(pose, blocking = blocking, \ 00108 pos_thres = .0025, rot_thres = .05, \ 00109 timeout = rospy.Duration(timeout), \ 00110 settling_time = rospy.Duration(settling_time)) 00111 00112 00113 #used to call compliant close or grasp adjustment 00114 def call_empty_service(service_name, serv): 00115 try: 00116 serv() 00117 except rospy.ServiceException, e: 00118 rospy.logerr("error when calling %s,%s"%(service_name,e)) 00119 return 0 00120 return 1 00121 00122 00123 #return the current pos and rot of the wrist for the right arm 00124 #as a 7-list (pos, quaternion rot) 00125 def return_current_pose_as_list(cm): 00126 (pos, rot) = cm.return_cartesian_pose() 00127 return pos+rot 00128 00129 00130 ##convert a relative vector in frame to a pose in the base_link frame 00131 #if start_pose is not specified, uses current pose of the wrist 00132 def return_rel_pose(cm, vector, frame, start_pose = None): 00133 00134 #if start_pose is not specified, use the current Cartesian pose of the wrist 00135 if start_pose == None: 00136 (start_trans, start_rot) = cm.return_cartesian_pose('base_link') 00137 else: 00138 start_pose.header.stamp = rospy.Time(0) 00139 (start_trans, start_rot) = pose_stamped_to_lists(cm.tf_listener, start_pose, 'base_link') 00140 00141 #convert the vector in frame to base_link frame 00142 if frame != 'base_link': 00143 vector3_stamped = create_vector3_stamped(vector, frame) 00144 base_link_vector = vector3_stamped_to_list(cm.tf_listener, vector3_stamped, 'base_link') 00145 else: 00146 base_link_vector = vector 00147 00148 #add the desired vector to the position 00149 new_trans = [x+y for (x,y) in zip(start_trans, base_link_vector)] 00150 00151 #create a new poseStamped 00152 pose_stamped = create_pose_stamped(new_trans+start_rot) 00153 00154 return pose_stamped 00155 00156 00157 #run the test 00158 if __name__ == "__main__": 00159 00160 rospy.init_node('test_reactive_grasp_server', anonymous = True) 00161 00162 use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', 0) 00163 use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', 0) 00164 rospy.loginfo("use_slip_controller:"+str(use_slip_controller)) 00165 rospy.loginfo("use_slip_detection:"+str(use_slip_detection)) 00166 00167 rospy.loginfo("waiting for compliant_close and grasp_adjustment services") 00168 rospy.wait_for_service("r_reactive_grasp/compliant_close") 00169 rospy.wait_for_service("r_reactive_grasp/grasp_adjustment") 00170 rospy.loginfo("service found") 00171 00172 rg_ac = actionlib.SimpleActionClient("reactive_grasp/right", \ 00173 ReactiveGraspAction) 00174 ra_ac = actionlib.SimpleActionClient("reactive_approach/right", \ 00175 ReactiveGraspAction) 00176 rl_ac = actionlib.SimpleActionClient("reactive_lift/right", \ 00177 ReactiveLiftAction) 00178 rp_ac = actionlib.SimpleActionClient("reactive_place/right", \ 00179 ReactivePlaceAction) 00180 cc_srv = rospy.ServiceProxy("r_reactive_grasp/compliant_close", Empty) 00181 ga_srv = rospy.ServiceProxy("r_reactive_grasp/grasp_adjustment", Empty) 00182 00183 rospy.loginfo("waiting for reactive grasp action") 00184 rg_ac.wait_for_server() 00185 rospy.loginfo("reactive grasp action found") 00186 00187 rospy.loginfo("waiting for reactive approach action") 00188 ra_ac.wait_for_server() 00189 rospy.loginfo("reactive approach action found") 00190 00191 rospy.loginfo("waiting for reactive lift action") 00192 rl_ac.wait_for_server() 00193 rospy.loginfo("reactive lift action found") 00194 00195 rospy.loginfo("waiting for reactive place action") 00196 rp_ac.wait_for_server() 00197 rospy.loginfo("reactive place action found") 00198 00199 cm = controller_manager.ControllerManager('r', \ 00200 using_slip_controller = use_slip_controller, \ 00201 using_slip_detection = use_slip_detection) 00202 00203 #joint names for the right arm 00204 joint_names = ["r_shoulder_pan_joint", 00205 "r_shoulder_lift_joint", 00206 "r_upper_arm_roll_joint", 00207 "r_elbow_flex_joint", 00208 "r_forearm_roll_joint", 00209 "r_wrist_flex_joint", 00210 "r_wrist_roll_joint"] 00211 00212 #reactive approach from above 00213 #table_height = .55 #simulated table 00214 table_height = .7239 #round table 00215 tip_dist_to_table = .165 00216 wrist_height = table_height + tip_dist_to_table + .02 00217 00218 approachpos = [.52, -.05, wrist_height+.1] 00219 approachquat = [0.5, 0.5, -0.5, 0.5] #from the top 00220 sideangles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, -1.787] 00221 side_tip_dist_to_table = .06 00222 sideapproachpos = [.63, -.3, table_height-.035+side_tip_dist_to_table] 00223 sideapproachquat = [.707, .707, 0, 0] 00224 current_goal = approachpos[:] + approachquat[:] 00225 small_step = .02 00226 approach_type = 'top' 00227 00228 while(not rospy.is_shutdown()): 00229 00230 #update current goal to current pose 00231 current_goal = return_current_pose_as_list(cm) 00232 00233 #pregrasp_pose = create_pose_stamped(current_goal) 00234 if approach_type == 'top': 00235 pregrasp_pose = create_pose_stamped(approachpos+approachquat) 00236 else: 00237 pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat) 00238 00239 print "enter:" 00240 print "side to switch to side approach, top to switch to top approach" 00241 print "appi to go to the approach position with interpolated IK" 00242 print "appc to go to the approach position with Cartesian controllers" 00243 print "appm to go to the approach position with move_arm" 00244 print "rg to grasp reactively, ra to just approach reactively, gl to grasp and lift" 00245 print "cc to do a compliant close" 00246 print "rl to do a reactive lift" 00247 print "rp to do a reactive place" 00248 print "ga to do a grasp adjustment" 00249 print "c to close, o to open" 00250 print "u to go up 10 cm, d to go down 10 cm" 00251 print "small steps: us to go up, ds to go down, r to go right, l to go left," 00252 print " a to go away from the robot, t to go toward the robot" 00253 print "j to go to a set of nearish-side-grasp angles" 00254 print "s to stop" 00255 c = keypause() 00256 if c == 'side': 00257 approach_type = 'side' 00258 elif c == 'top': 00259 approach_type = 'top' 00260 elif c == 'appi': 00261 #Go to the approach position using the interpolated 00262 #IK controllers (collision-aware) 00263 cm.command_interpolated_ik(pregrasp_pose) 00264 elif c == 'appc': 00265 #Go to the approach position using the Cartesian controllers 00266 cm.command_cartesian(pregrasp_pose) 00267 elif c == 'appm': 00268 #Go to the approach position using move_arm 00269 cm.move_arm_pose(pregrasp_pose, blocking = 1) 00270 elif c == 'j': 00271 print "moving to near-side-grasp angles" 00272 cm.command_joint(sideangles) 00273 00274 elif c == 'rg' or c == 'ra' or c == 'gl': 00275 00276 #set the goal to be 10 cm along the current gripper x-axis 00277 grasp_pose = return_rel_pose(cm, [.1, 0, 0], 'r_wrist_roll_link') 00278 00279 # grasp_goal = current_goal[:] 00280 # grasp_goal[2] -= .1 00281 # grasp_pose = create_pose_stamped(grasp_goal) 00282 00283 start_angles = [0.]*7 00284 trajectory = None 00285 if c == 'rg' or c == 'gl': 00286 print "calling reactive grasp" 00287 result = call_reactive_grasp(rg_ac, grasp_pose, trajectory) 00288 else: 00289 print "calling reactive approach" 00290 result = call_reactive_approach(ra_ac, grasp_pose, trajectory) 00291 print "result:", result 00292 00293 if c == 'gl': 00294 if result.value == 1: 00295 print "calling reactive lift" 00296 lift = GripperTranslation() 00297 lift.direction = create_vector3_stamped([0,0,1]) 00298 lift.desired_distance = .1 00299 lift.min_distance = 0. 00300 result = call_reactive_lift(rl_ac, lift) 00301 else: 00302 print "grasp didn't return success, opening gripper" 00303 cm.command_gripper(.087, -1) 00304 00305 elif c == 'rl': 00306 print "calling reactive lift" 00307 lift = GripperTranslation() 00308 lift.direction = create_vector3_stamped([0,0,1]) 00309 lift.desired_distance = .1 00310 lift.min_distance = 0. 00311 result = call_reactive_lift(rl_ac, lift) 00312 00313 elif c == 'rp': 00314 print "calling reactive place" 00315 place_goal = current_goal[:] 00316 place_goal[2] -= .1 00317 place_pose = create_pose_stamped(place_goal) 00318 result = call_reactive_place(rp_ac, place_pose) 00319 00320 elif c == 'cc': 00321 print "calling compliant close" 00322 call_empty_service("compliant close", cc_srv) 00323 elif c == 'ga': 00324 print "calling grasp adjustment" 00325 call_empty_service("grasp adjustment", ga_srv) 00326 elif c == 'c': 00327 print "closing gripper" 00328 cm.command_gripper(0, 40.0) 00329 elif c == 'o': 00330 print "opening gripper" 00331 cm.command_gripper(.087, -1) 00332 elif c == 'u': 00333 print "going up" 00334 current_goal[2] += .1 00335 move_cartesian_step(cm, current_goal, blocking = 1) 00336 elif c == 'us': 00337 print "going up a small amount" 00338 current_goal[2] += small_step 00339 move_cartesian_step(cm, current_goal, blocking = 1) 00340 elif c == 'd': 00341 print "going down" 00342 current_goal[2] -= .05 00343 move_cartesian_step(cm, current_goal, blocking = 1) 00344 elif c == 'ds': 00345 print "going down a small amount" 00346 current_goal[2] -= small_step 00347 move_cartesian_step(cm, current_goal, blocking = 1) 00348 elif c == 'r': 00349 print "moving right" 00350 current_goal[1] -= small_step 00351 move_cartesian_step(cm, current_goal, blocking = 1) 00352 elif c == 'l': 00353 print "moving left" 00354 current_goal[1] += small_step 00355 move_cartesian_step(cm, current_goal, blocking = 1) 00356 elif c == 'a': 00357 print "moving away from robot" 00358 current_goal[0] += small_step 00359 move_cartesian_step(cm, current_goal, blocking = 1) 00360 elif c == 't': 00361 print "moving toward the robot" 00362 current_goal[0] -= small_step 00363 move_cartesian_step(cm, current_goal, blocking = 1) 00364 elif c == 's': 00365 print "quitting" 00366 break 00367 00368 #update current goal to current pose 00369 current_goal = return_current_pose_as_list(cm)