$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_stereo') 00009 import rospy 00010 import math 00011 import scipy 00012 from geometry_msgs.msg import PoseStamped, PointStamped, \ 00013 QuaternionStamped, Pose, Point, Quaternion 00014 from object_manipulation_msgs.msg import ReactiveGraspAction, \ 00015 ReactiveGraspGoal, ReactiveLiftAction, ReactiveLiftGoal, \ 00016 GripperTranslation, ReactivePlaceAction, ReactivePlaceGoal 00017 import actionlib 00018 from trajectory_msgs.msg import JointTrajectory 00019 from pr2_gripper_reactive_approach import controller_manager 00020 from std_srvs.srv import Empty 00021 from object_manipulator.convert_functions import * 00022 import tabletop_collision_map_processing.collision_map_interface as collision_map_interface 00023 from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest 00024 from tabletop_object_detector.msg import TabletopDetectionResult 00025 import object_manipulator.draw_functions 00026 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest 00027 00028 #call reactive_grasp to do a reactive grasp using the fingertip sensors 00029 #(backs up and move to the side if the tip contacts on the way to the grasp, 00030 #does a compliant grasp if it gets there, tries the approach and grasp 00031 #several times if the gripper opening isn't within bounds) 00032 def call_reactive_grasp(ac, grasp_pose, trajectory): 00033 00034 if trajectory == None: 00035 trajectory = JointTrajectory() 00036 00037 goal = ReactiveGraspGoal() 00038 goal.final_grasp_pose = grasp_pose 00039 goal.trajectory = trajectory 00040 goal.collision_support_surface_name = "table" 00041 00042 ac.send_goal(goal) 00043 ac.wait_for_result() 00044 result = ac.get_result() 00045 print "reactive grasp result:", result 00046 00047 return result.manipulation_result 00048 00049 00050 #call reactive_grasp to do a reactive approach using the fingertip sensors 00051 #(backs up and move to the side if the tip contacts on the way to the grasp) 00052 def call_reactive_approach(ac, grasp_pose, trajectory): 00053 00054 if trajectory == None: 00055 trajectory = JointTrajectory() 00056 00057 goal = ReactiveGraspGoal() 00058 goal.final_grasp_pose = grasp_pose 00059 goal.trajectory = trajectory 00060 goal.collision_support_surface_name = "table" 00061 00062 ac.send_goal(goal) 00063 ac.wait_for_result() 00064 result = ac.get_result() 00065 print "reactive approach result:", result 00066 00067 return result.manipulation_result 00068 00069 00070 #call reactive lift (starts up slip servoing if using the slip controllers, 00071 #otherwise just a Cartesian move) 00072 def call_reactive_lift(ac, lift): 00073 00074 goal = ReactiveLiftGoal() 00075 goal.lift = lift 00076 00077 ac.send_goal(goal) 00078 ac.wait_for_result() 00079 result = ac.get_result() 00080 print "reactive lift result:", result 00081 00082 return result 00083 00084 00085 #call reactive place (uses the slip controllers to detect when the object 00086 #hits the table, and opens the gripper) 00087 def call_reactive_place(ac, place_pose): 00088 00089 goal = ReactivePlaceGoal() 00090 goal.final_place_pose = place_pose 00091 00092 ac.send_goal(goal) 00093 ac.wait_for_result() 00094 result = ac.get_result() 00095 print "reactive place result:", result 00096 00097 return result 00098 00099 00100 def do_adjust(): 00101 00102 print "adjusting pregrasp position" 00103 00104 #do a tabletop detection and find the farthest cluster point along farthest_point_axis 00105 if approach_type == 'top': 00106 farthest_point_axis = [0,0,1] 00107 else: 00108 farthest_point_axis = [0,-1,0] 00109 box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv) 00110 if box_poses == []: 00111 rospy.logerr("No clusters returned!") 00112 return 00113 farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis) 00114 if farthest_point == None: 00115 rospy.logerr("No farthest point returned!") 00116 return 00117 00118 #draw the farthest_point 00119 draw_sphere_mat = scipy.matrix(scipy.identity(4)) 00120 draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T 00121 draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link') 00122 00123 #set the pregrasp between-fingers point to be the farthest_point + 7 cm 00124 if approach_type == 'top': 00125 approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)] 00126 pregrasp_pose = create_pose_stamped(approachpos+approachquat) 00127 else: 00128 sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]] 00129 pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat) 00130 00131 #draw the new pregrasp pose 00132 draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link') 00133 00134 00135 00136 #pause for input 00137 def keypause(): 00138 print "press enter to continue" 00139 input = raw_input() 00140 return input 00141 00142 00143 #move to a Cartesian pose goal 00144 def move_cartesian_step(cm, pose, timeout = 10.0, 00145 settling_time = 3.0, blocking = 0): 00146 if type(pose) == list: 00147 pose = create_pose_stamped(pose, 'base_link') 00148 cm.move_cartesian(pose, blocking = blocking, \ 00149 pos_thres = .0025, rot_thres = .05, \ 00150 timeout = rospy.Duration(timeout), \ 00151 settling_time = rospy.Duration(settling_time)) 00152 00153 00154 #used to call compliant close or grasp adjustment 00155 def call_empty_service(service_name, serv): 00156 try: 00157 serv() 00158 except rospy.ServiceException, e: 00159 rospy.logerr("error when calling %s,%s"%(service_name,e)) 00160 return 0 00161 return 1 00162 00163 00164 #return the current pos and rot of the wrist for the right arm 00165 #as a 7-list (pos, quaternion rot) 00166 def return_current_pose_as_list(cm): 00167 (pos, rot) = cm.return_cartesian_pose() 00168 return pos+rot 00169 00170 00171 ##convert a relative vector in frame to a pose in the base_link frame 00172 #if start_pose is not specified, uses current pose of the wrist 00173 def return_rel_pose(cm, vector, frame, start_pose = None): 00174 00175 #if start_pose is not specified, use the current Cartesian pose of the wrist 00176 if start_pose == None: 00177 (start_trans, start_rot) = cm.return_cartesian_pose('base_link') 00178 else: 00179 start_pose.header.stamp = rospy.Time(0) 00180 (start_trans, start_rot) = pose_stamped_to_lists(cm.tf_listener, start_pose, 'base_link') 00181 00182 #convert the vector in frame to base_link frame 00183 if frame != 'base_link': 00184 vector3_stamped = create_vector3_stamped(vector, frame) 00185 base_link_vector = vector3_stamped_to_list(cm.tf_listener, vector3_stamped, 'base_link') 00186 else: 00187 base_link_vector = vector 00188 00189 #add the desired vector to the position 00190 new_trans = [x+y for (x,y) in zip(start_trans, base_link_vector)] 00191 00192 #create a new poseStamped 00193 pose_stamped = create_pose_stamped(new_trans+start_rot) 00194 00195 return pose_stamped 00196 00197 00198 #try to move to a set of joint angles using move_arm, ignoring current collisions if necessary, and if that fails, move open-loop 00199 def try_hard_to_move_joint(cm, col_map_interface, trajectory, max_tries = 5, use_open_loop = 1): 00200 reset_collision_map = 0 00201 for angles in trajectory: 00202 success = 0 00203 for tries in range(max_tries): 00204 rospy.loginfo("try_hard_to_move_joint try number: %d"%tries) 00205 error_code = cm.move_arm_joint(angles, blocking = 1) 00206 00207 if error_code == 1: 00208 rospy.loginfo("move arm reported success") 00209 success = 1 00210 break 00211 elif error_code == 0: 00212 if not reset_collision_map: 00213 rospy.loginfo("problem with collision map! Resetting collision map and taking a new one") 00214 col_map_interface.reset_collision_map() 00215 col_map_interface.take_static_map() 00216 reset_collision_map = 1 00217 else: 00218 rospy.loginfo("resetting collision map didn't work, move arm is still stuck!") 00219 break 00220 rospy.loginfo("move arm reports failure with error code %d"%error_code) 00221 if not success and use_open_loop: 00222 rospy.loginfo("moving open-loop to desired joint angles") 00223 cm.command_joint_trajectory([angles], blocking = 1) 00224 00225 00226 #move the arm to the side 00227 def move_arm_to_side(cm, col_map_interface): 00228 arm_above_and_to_side_angles = [-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501] 00229 arm_to_side_angles = [-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398] 00230 try_hard_to_move_joint(cm, col_map_interface, [arm_above_and_to_side_angles, \ 00231 arm_to_side_angles], use_open_loop = 1) 00232 00233 00234 #move the arm from the side to the approach 00235 def side_to_approach_joint(cm, col_map_interface): 00236 arm_above_and_to_side_angles = [-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501] 00237 # [-0.309647, 0.346556, -1.203788, -1.641639, -1.907762, -1.941233, -0.357047] 00238 try_hard_to_move_joint(cm, col_map_interface, 00239 [arm_above_and_to_side_angles], 00240 use_open_loop = 1) 00241 00242 ##call find_cluster_bounding_box to get the bounding box for a cluster 00243 def call_find_cluster_bounding_box(bounding_box_srv, cluster): 00244 00245 req = FindClusterBoundingBoxRequest() 00246 req.cluster = cluster 00247 try: 00248 res = bounding_box_srv(req) 00249 except rospy.ServiceException, e: 00250 rospy.logerr("error when calling find_cluster_bounding_box: %s"%e) 00251 return (None, []) 00252 00253 return (res.pose, [res.box_dims.x, res.box_dims.y, res.box_dims.z]) 00254 00255 00256 #call tabletop object detection to detect table/objects 00257 def call_tabletop_detection(detect_srv, bounding_box_srv): 00258 00259 rospy.loginfo("calling tabletop detection") 00260 00261 det_req = TabletopDetectionRequest() 00262 det_req.return_clusters = 1 00263 det_req.return_models = 0 00264 det_req.num_models = 0 00265 00266 #call tabletop detection, get a detection result 00267 for try_num in range(3): 00268 try: 00269 det_res = detect_srv(det_req) 00270 except rospy.ServiceException, e: 00271 rospy.logerr("error when calling tabletop_detection: %s"%e) 00272 return ([], []) 00273 if det_res.detection.result == det_res.detection.SUCCESS: 00274 rospy.loginfo("tabletop detection reports success") 00275 break 00276 else: 00277 rospy.logerr("tabletop detection failed with error:"+str(det_res.detection.result)) 00278 else: 00279 rospy.logerr("tabletop detection failed too many times. Returning.") 00280 return ([], []) 00281 00282 #find bounding boxes for found clusters (should be in base_link frame) 00283 box_poses = [] 00284 box_dims = [] 00285 for cluster in det_res.detection.clusters: 00286 (pose, dims) = call_find_cluster_bounding_box(bounding_box_srv, cluster) 00287 box_poses.append(pose) 00288 box_dims.append(dims) 00289 return (box_poses, box_dims) 00290 00291 00292 #find the position of the farthest corner of any cluster bounding box along axis in base_link frame 00293 def find_farthest_box(box_poses, box_dims, axis): 00294 axis_mat = scipy.matrix([axis[0], axis[1], axis[2], 0]) 00295 00296 farthest_box_points = [] 00297 farthest_dists = [] 00298 for (box_pose, dims) in zip(box_poses, box_dims): 00299 00300 transform = pose_to_mat(box_pose.pose) 00301 ranges = [[-dims[0]/2, -dims[1]/2, -dims[2]/2], [dims[0]/2, dims[1]/2, dims[2]/2]] 00302 corners = [scipy.matrix([ranges[xind][0], ranges[yind][1], ranges[zind][2], 1]).T for \ 00303 (xind, yind, zind) in scipy.ndindex(2,2,2)] 00304 corners = scipy.concatenate(corners, axis=1) 00305 transformed_corners = transform*corners #4xn 00306 #print "transformed_corners:", transformed_corners 00307 00308 dists = axis_mat * transformed_corners 00309 max_dist = dists.max() 00310 #print "max_dist:", max_dist 00311 ind = scipy.argmax(dists) 00312 #print "ind:", ind 00313 #print "dists:", dists 00314 farthest_dists.append(max_dist) 00315 farthest_point = list(scipy.array(transformed_corners[0:3, ind].T)[0]) 00316 #print "farthest_point:", farthest_point 00317 farthest_box_points.append(farthest_point) 00318 00319 farthest_ind = scipy.argmax(scipy.array(farthest_dists)) 00320 farthest_point = farthest_box_points[farthest_ind] 00321 print "farthest_box_points:", farthest_box_points 00322 print "farthest_dists:", farthest_dists 00323 print "overall farthest_ind:", farthest_ind 00324 print "overall farthest_point:", farthest_point 00325 return farthest_point 00326 00327 00328 #run the test 00329 if __name__ == "__main__": 00330 00331 rospy.init_node('test_reactive_grasp_server', anonymous = True) 00332 00333 use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', 0) 00334 use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', 0) 00335 rospy.loginfo("use_slip_controller:"+str(use_slip_controller)) 00336 rospy.loginfo("use_slip_detection:"+str(use_slip_detection)) 00337 00338 rospy.loginfo("waiting for compliant_close and grasp_adjustment services") 00339 rospy.wait_for_service("r_reactive_grasp/compliant_close") 00340 rospy.wait_for_service("r_reactive_grasp/grasp_adjustment") 00341 rospy.wait_for_service("object_detection") 00342 rospy.wait_for_service('/find_cluster_bounding_box') 00343 rospy.loginfo("service found") 00344 00345 rg_ac = actionlib.SimpleActionClient("reactive_grasp/right", \ 00346 ReactiveGraspAction) 00347 ra_ac = actionlib.SimpleActionClient("reactive_approach/right", \ 00348 ReactiveGraspAction) 00349 rl_ac = actionlib.SimpleActionClient("reactive_lift/right", \ 00350 ReactiveLiftAction) 00351 rp_ac = actionlib.SimpleActionClient("reactive_place/right", \ 00352 ReactivePlaceAction) 00353 cc_srv = rospy.ServiceProxy("r_reactive_grasp/compliant_close", Empty) 00354 ga_srv = rospy.ServiceProxy("r_reactive_grasp/grasp_adjustment", Empty) 00355 detect_srv = rospy.ServiceProxy("object_detection", TabletopDetection) 00356 bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox) 00357 00358 rospy.loginfo("waiting for reactive grasp action") 00359 rg_ac.wait_for_server() 00360 rospy.loginfo("reactive grasp action found") 00361 00362 rospy.loginfo("waiting for reactive approach action") 00363 ra_ac.wait_for_server() 00364 rospy.loginfo("reactive approach action found") 00365 00366 rospy.loginfo("waiting for reactive lift action") 00367 rl_ac.wait_for_server() 00368 rospy.loginfo("reactive lift action found") 00369 00370 rospy.loginfo("waiting for reactive place action") 00371 rp_ac.wait_for_server() 00372 rospy.loginfo("reactive place action found") 00373 00374 cm = controller_manager.ControllerManager('r', \ 00375 using_slip_controller = use_slip_controller, \ 00376 using_slip_detection = use_slip_detection) 00377 00378 #collision map interface for using move_arm 00379 col_map_interface = collision_map_interface.CollisionMapInterface() 00380 00381 #draw_functions object for drawing stuff in rviz 00382 draw_functions = object_manipulator.draw_functions.DrawFunctions('grasp_markers') 00383 00384 #joint names for the right arm 00385 joint_names = ["r_shoulder_pan_joint", 00386 "r_shoulder_lift_joint", 00387 "r_upper_arm_roll_joint", 00388 "r_elbow_flex_joint", 00389 "r_forearm_roll_joint", 00390 "r_wrist_flex_joint", 00391 "r_wrist_roll_joint"] 00392 00393 #reactive approach from above 00394 #table_height = .55 #simulated table 00395 table_height = .7239 #round table 00396 tip_dist_to_table = .18 00397 wrist_height = table_height + tip_dist_to_table + .02 00398 approachpos = [.52, -.05, wrist_height+.1] 00399 approachquat = [0.5, 0.5, -0.5, 0.5] #from the top 00400 sideangles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, -1.787] 00401 side_tip_dist_to_table = .06 00402 sideapproachpos = [.50, -.3, table_height-.035+side_tip_dist_to_table] 00403 sideapproachquat = [.707, .707, 0, 0] 00404 current_goal = approachpos[:] + approachquat[:] 00405 small_step = .02 00406 approach_type = 'top' 00407 00408 while(not rospy.is_shutdown()): 00409 00410 #update current goal to current pose 00411 current_goal = return_current_pose_as_list(cm) 00412 00413 #pregrasp_pose = create_pose_stamped(current_goal) 00414 if approach_type == 'top': 00415 pregrasp_pose = create_pose_stamped(approachpos+approachquat) 00416 else: 00417 pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat) 00418 00419 print "enter:" 00420 print "side to switch to side approach, top to switch to top approach" 00421 print "appi to go to the approach position with interpolated IK" 00422 print "appc to go to the approach position with Cartesian controllers" 00423 print "appm to go to the approach position with move_arm" 00424 print "appj to move from the side to the top approach with joint angles" 00425 print "ms to move the arm to the side" 00426 print "rg to grasp reactively, ra to just approach reactively, gl to grasp and lift" 00427 print "cc to do a compliant close" 00428 print "rl to do a reactive lift" 00429 print "rp to do a reactive place" 00430 print "ga to do a grasp adjustment" 00431 print "adj to adjust the pre-grasp position (arms should be at side)" 00432 print "c to close, o to open" 00433 print "u to go up 10 cm, d to go down 10 cm" 00434 print "small steps: us to go up, ds to go down, r to go right, l to go left," 00435 print " a to go away from the robot, t to go toward the robot" 00436 print "j to go to a set of nearish-side-grasp angles" 00437 print "reset to reset the collision map" 00438 print "map to take a static collision map" 00439 print "s to stop" 00440 c = keypause() 00441 if c == 'side': 00442 approach_type = 'side' 00443 elif c == 'top': 00444 approach_type = 'top' 00445 elif c == 'appi': 00446 #Go to the approach position using the interpolated 00447 #IK controllers (collision-aware) 00448 cm.command_interpolated_ik(pregrasp_pose) 00449 elif c == 'appc': 00450 #Go to the approach position using the Cartesian controllers 00451 cm.command_cartesian(pregrasp_pose) 00452 elif c == 'appm': 00453 #Go to the approach position using move_arm 00454 for tries in range(3): 00455 error_code = cm.move_arm_pose(pregrasp_pose, blocking = 1) 00456 if error_code == 1: 00457 print "move_arm reported success" 00458 break 00459 00460 elif c == 'appj': 00461 side_to_approach_joint(cm, col_map_interface) 00462 elif c == 'ms': 00463 move_arm_to_side(cm, col_map_interface) 00464 elif c == 'j': 00465 print "moving to near-side-grasp angles" 00466 cm.command_joint(sideangles) 00467 elif c == 'adj': 00468 00469 print "adjusting pregrasp position" 00470 00471 #do a tabletop detection and find the farthest cluster point along farthest_point_axis 00472 if approach_type == 'top': 00473 farthest_point_axis = [0,0,1] 00474 else: 00475 farthest_point_axis = [0,-1,0] 00476 box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv) 00477 if box_poses == []: 00478 rospy.logerr("No clusters returned!") 00479 continue 00480 farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis) 00481 if farthest_point == None: 00482 rospy.logerr("No farthest point returned!") 00483 continue 00484 00485 #draw the farthest_point 00486 draw_sphere_mat = scipy.matrix(scipy.identity(4)) 00487 draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T 00488 draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link') 00489 00490 #set the pregrasp between-fingers point to be the farthest_point + 7 cm 00491 if approach_type == 'top': 00492 approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)] 00493 pregrasp_pose = create_pose_stamped(approachpos+approachquat) 00494 else: 00495 sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]] 00496 pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat) 00497 00498 #draw the new pregrasp pose 00499 draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link') 00500 00501 00502 00503 elif c == 'res': 00504 print "restoring original approach positions" 00505 approachpos = [.52, -.05, wrist_height+.1] 00506 sideapproachpos = [.50, -.3, table_height-.035+side_tip_dist_to_table] 00507 00508 elif c == 'rg' or c == 'ra' or c == 'gl': 00509 00510 #set the goal to be 10 cm along the current gripper x-axis 00511 grasp_pose = return_rel_pose(cm, [.1, 0, 0], 'r_wrist_roll_link') 00512 00513 # grasp_goal = current_goal[:] 00514 # grasp_goal[2] -= .1 00515 # grasp_pose = create_pose_stamped(grasp_goal) 00516 00517 start_angles = [0.]*7 00518 trajectory = None 00519 if c == 'rg' or c == 'gl': 00520 print "calling reactive grasp" 00521 result = call_reactive_grasp(rg_ac, grasp_pose, trajectory) 00522 else: 00523 print "calling reactive approach" 00524 result = call_reactive_approach(ra_ac, grasp_pose, trajectory) 00525 print "result:", result 00526 00527 if c == 'gl': 00528 if result.value == 1: 00529 print "calling reactive lift" 00530 lift = GripperTranslation() 00531 lift.direction = create_vector3_stamped([0,0,1]) 00532 lift.desired_distance = .1 00533 lift.min_distance = 0. 00534 result = call_reactive_lift(rl_ac, lift) 00535 move_arm_to_side(cm, col_map_interface) 00536 cm.command_gripper(.087, -1) 00537 ''' 00538 print "adjusting pregrasp position" 00539 00540 #do a tabletop detection and find the farthest cluster point along farthest_point_axis 00541 if approach_type == 'top': 00542 farthest_point_axis = [0,0,1] 00543 else: 00544 farthest_point_axis = [0,-1,0] 00545 box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv) 00546 if box_poses == []: 00547 rospy.logerr("No clusters returned!") 00548 continue 00549 farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis) 00550 if farthest_point == None: 00551 rospy.logerr("No farthest point returned!") 00552 continue 00553 00554 #draw the farthest_point 00555 draw_sphere_mat = scipy.matrix(scipy.identity(4)) 00556 draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T 00557 draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link') 00558 00559 #set the pregrasp between-fingers point to be the farthest_point + 7 cm 00560 if approach_type == 'top': 00561 approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)] 00562 pregrasp_pose = create_pose_stamped(approachpos+approachquat) 00563 else: 00564 sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]] 00565 pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat) 00566 00567 #draw the new pregrasp pose 00568 draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link') 00569 side_to_approach_joint(cm, col_map_interface) 00570 cm.command_cartesian(pregrasp_pose) 00571 ''' 00572 else: 00573 print "grasp didn't return success, opening gripper" 00574 cm.command_gripper(.087, -1) 00575 00576 elif c == 'rl': 00577 print "calling reactive lift" 00578 lift = GripperTranslation() 00579 lift.direction = create_vector3_stamped([0,0,1]) 00580 lift.desired_distance = .1 00581 lift.min_distance = 0. 00582 result = call_reactive_lift(rl_ac, lift) 00583 00584 elif c == 'rp': 00585 print "calling reactive place" 00586 place_goal = current_goal[:] 00587 place_goal[2] -= .1 00588 place_pose = create_pose_stamped(place_goal) 00589 result = call_reactive_place(rp_ac, place_pose) 00590 00591 elif c == 'cc': 00592 print "calling compliant close" 00593 call_empty_service("compliant close", cc_srv) 00594 elif c == 'ga': 00595 print "calling grasp adjustment" 00596 call_empty_service("grasp adjustment", ga_srv) 00597 elif c == 'c': 00598 print "closing gripper" 00599 cm.command_gripper(0, 40.0) 00600 elif c == 'o': 00601 print "opening gripper" 00602 cm.command_gripper(.087, -1) 00603 elif c == 'u': 00604 print "going up" 00605 current_goal[2] += .1 00606 move_cartesian_step(cm, current_goal, blocking = 1) 00607 elif c == 'us': 00608 print "going up a small amount" 00609 current_goal[2] += small_step 00610 move_cartesian_step(cm, current_goal, blocking = 1) 00611 elif c == 'd': 00612 print "going down" 00613 current_goal[2] -= .05 00614 move_cartesian_step(cm, current_goal, blocking = 1) 00615 elif c == 'ds': 00616 print "going down a small amount" 00617 current_goal[2] -= small_step 00618 move_cartesian_step(cm, current_goal, blocking = 1) 00619 elif c == 'r': 00620 print "moving right" 00621 current_goal[1] -= small_step 00622 move_cartesian_step(cm, current_goal, blocking = 1) 00623 elif c == 'l': 00624 print "moving left" 00625 current_goal[1] += small_step 00626 move_cartesian_step(cm, current_goal, blocking = 1) 00627 elif c == 'a': 00628 print "moving away from robot" 00629 current_goal[0] += small_step 00630 move_cartesian_step(cm, current_goal, blocking = 1) 00631 elif c == 't': 00632 print "moving toward the robot" 00633 current_goal[0] -= small_step 00634 move_cartesian_step(cm, current_goal, blocking = 1) 00635 elif c == 'reset': 00636 col_map_interface.reset_collision_map() 00637 elif c == 'map': 00638 col_map_interface.take_static_map() 00639 elif c == 's': 00640 print "quitting" 00641 break 00642 00643 #update current goal to current pose 00644 current_goal = return_current_pose_as_list(cm) 00645 00646