$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 # author: Kaijen Hsiao 00035 00036 ## @package pick_and_place_manager 00037 # Functions to grasp objects off flat tables and place them down in 00038 # specified rectangular regions in space 00039 00040 import roslib 00041 roslib.load_manifest('pr2_pick_and_place_demos') 00042 import rospy 00043 from object_manipulation_msgs.msg import PickupAction, PickupGoal, \ 00044 PlaceAction, PlaceGoal, GripperTranslation, ReactivePlaceAction, ReactivePlaceGoal 00045 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest 00046 from object_manipulation_msgs.msg import ManipulationResult, GraspableObject 00047 from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest 00048 from tabletop_object_detector.msg import TabletopDetectionResult 00049 from tabletop_collision_map_processing.srv import \ 00050 TabletopCollisionMapProcessing, TabletopCollisionMapProcessingRequest 00051 from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal 00052 import tf 00053 import actionlib 00054 import scipy 00055 import time 00056 import copy 00057 import math 00058 import pdb 00059 import threading 00060 import sys 00061 import pr2_gripper_reactive_approach.controller_manager as controller_manager 00062 import tabletop_collision_map_processing.collision_map_interface as collision_map_interface 00063 import pr2_gripper_reactive_approach.reactive_grasp as reactive_grasp 00064 import object_manipulator.draw_functions as draw_functions 00065 from object_manipulator.convert_functions import * 00066 from arm_navigation_msgs.msg import JointConstraint, PositionConstraint, OrientationConstraint, ArmNavigationErrorCodes, AllowedContactSpecification, Constraints 00067 from tf_conversions import posemath 00068 00069 ##saved info about one object 00070 class ObjectInfo(): 00071 def __init__(self, object, box_dims, pose, tf_listener, height_above_table, collision_name): 00072 self.object = object #the original GraspableObject message 00073 self.box_dims = box_dims #bounding box dims (only for point clusters) 00074 self.pose = pose #the original pose on the table when detected (PoseStamped) (of bounding box if point cluster) 00075 self.grasp = None #the Grasp object returned by the Pickup service, once the service is called 00076 self.grasp_pose = None #where it was grasped, once it is grasped 00077 self.grasp_type = 'normal' #whether the grasp was normal ('normal') or a slid-off-table grasp ('flat') 00078 self.collision_name = collision_name #the name of the object in the collision space 00079 00080 #for point clusters, record the transform from the cluster origin to the bounding box pose 00081 #(object bounding box to base_link = pose)**-1 * (cluster to base link) = (cluster origin to bounding box) 00082 if len(self.object.potential_models) == 0: 00083 self.type = 'cluster' 00084 else: 00085 self.type = 'mesh' 00086 #self.cluster_origin_to_bounding_box = None 00087 cluster_to_base_link = get_transform(tf_listener, object.cluster.header.frame_id, 'base_link') 00088 pose_mat = pose_to_mat(pose.pose) 00089 self.cluster_origin_to_bounding_box = pose_mat**-1 * cluster_to_base_link #cluster to bounding box transform 00090 00091 #save the height of the object above the current table detection 00092 self.height_above_table = height_above_table 00093 00094 00095 ##Manager for pick and place actions 00096 class PickAndPlaceManager(): 00097 00098 00099 def __init__(self, use_slip_controller = 0, use_slip_detection = 0): 00100 00101 #just "left", just "right", or "both" arms? 00102 use_right_arm = rospy.get_param("~use_right_arm", "true") 00103 use_left_arm = rospy.get_param("~use_left_arm", "true") 00104 self.arms_to_use_list = [0,1] 00105 self.arms_to_use = "both" 00106 if use_right_arm and not use_left_arm: 00107 self.arms_to_use = "right" 00108 self.arms_to_use_list = [0] 00109 elif use_left_arm and not use_right_arm: 00110 self.arms_to_use = "left" 00111 self.arms_to_use_list = [1] 00112 elif not use_left_arm and not use_right_arm: 00113 rospy.logerr("grasp_executive: params say we're not using any arms!! Defaulting to using both") 00114 if self.arms_to_use != "both": 00115 rospy.loginfo("grasp_executive: using only %s arm"%self.arms_to_use) 00116 00117 self.stereo_camera_frame = rospy.get_param("~stereo_camera_frame", "/narrow_stereo_optical_frame") 00118 #self.stereo_camera_frame = rospy.get_param("~stereo_camera_frame", "/openni_rgb_optical_frame") 00119 00120 #should we use the slip detecting gripper controller? 00121 self.use_slip_controller = use_slip_controller 00122 00123 #should we use slip detection, or just normal gripper commands? 00124 self.use_slip_detection = use_slip_detection 00125 00126 #grasp/place action names 00127 self.grasper_grasp_name = 'object_manipulator/object_manipulator_pickup' 00128 self.grasper_place_name = 'object_manipulator/object_manipulator_place' 00129 00130 #grasp/place action clients 00131 self.grasper_grasp_action_client = actionlib.SimpleActionClient(self.grasper_grasp_name, PickupAction) 00132 rospy.loginfo("grasp_executive: waiting for object_manipulator_pickup action") 00133 self.grasper_grasp_action_client.wait_for_server() 00134 rospy.loginfo("grasp_executive: object_manipulator_pickup action found") 00135 self.grasper_place_action_client = actionlib.SimpleActionClient(self.grasper_place_name, PlaceAction) 00136 rospy.loginfo("grasp_executive: waiting for object_manipulator_place action") 00137 self.grasper_place_action_client.wait_for_server() 00138 rospy.loginfo("grasp_executive: object_manipulator_place action found") 00139 00140 #service names 00141 self.grasper_detect_name = 'object_detection' 00142 self.collision_map_processing_name = '/tabletop_collision_map_processing/tabletop_collision_map_processing' 00143 #self.grasper_transfer_name = '/grasping_app_transfer' 00144 self.find_bounding_box_name = '/find_cluster_bounding_box' 00145 00146 #head action client 00147 self.head_action_client = \ 00148 actionlib.SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction) 00149 rospy.loginfo("grasp_executive: waiting for point_head_action client") 00150 self.head_action_client.wait_for_server() 00151 rospy.loginfo("grasp_executive: point_head_action client found") 00152 00153 #reactive place action clients 00154 self.reactive_place_clients = [None, None] 00155 if self.arms_to_use in ["both", "right"]: 00156 rospy.loginfo("grasp_executive: waiting for right reactive place client") 00157 self.reactive_place_clients[0] = actionlib.SimpleActionClient("reactive_place/right", ReactivePlaceAction) 00158 self.reactive_place_clients[0].wait_for_server() 00159 if self.arms_to_use in ["both", "left"]: 00160 rospy.loginfo("grasp_executive: waiting for left reactive place client") 00161 self.reactive_place_clients[1] = actionlib.SimpleActionClient("reactive_place/left", ReactivePlaceAction) 00162 self.reactive_place_clients[1].wait_for_server() 00163 rospy.loginfo("grasp_executive: reactive place clients found") 00164 00165 #start tf listener 00166 self.tf_listener = tf.TransformListener() 00167 00168 #initialize controller managers for both arms 00169 self.cms = [None, None] 00170 if self.arms_to_use in ["both", "right"]: 00171 rospy.loginfo("grasp_executive: initializing controller manager for right arm") 00172 self.cms[0] = controller_manager.ControllerManager('r', self.tf_listener, use_slip_controller, use_slip_detection) 00173 if self.arms_to_use in ["both", "left"]: 00174 rospy.loginfo("grasp_executive: initializing controller manager for left arm") 00175 self.cms[1] = controller_manager.ControllerManager('l', self.tf_listener, use_slip_controller, use_slip_detection) 00176 rospy.loginfo("grasp_executive: done initializing controller managers for both arms") 00177 00178 #wait for services 00179 rospy.loginfo("grasp_executive: waiting for object_detection service") 00180 rospy.wait_for_service(self.grasper_detect_name) 00181 rospy.loginfo("grasp_executive: object_detection service found") 00182 00183 rospy.loginfo("grasp_executive: waiting for collision_map_processing service") 00184 rospy.wait_for_service(self.collision_map_processing_name) 00185 rospy.loginfo("grasp_executive: collision_map_processing service found") 00186 00187 #rospy.loginfo("grasp_executive: waiting for grasping_app_transfer service") 00188 #rospy.wait_for_service(grasper_transfer_name) 00189 #rospy.loginfo("grasp_executive: grasper_transfer service found") 00190 00191 rospy.loginfo("grasp_executive: waiting for find_cluster_bounding_box service") 00192 rospy.wait_for_service(self.find_bounding_box_name) 00193 rospy.loginfo("grasp_executive: find_cluster_bounding_box service found") 00194 00195 #service proxies 00196 self.grasper_detect_srv = rospy.ServiceProxy(self.grasper_detect_name, TabletopDetection) 00197 self.collision_map_processing_srv = rospy.ServiceProxy(self.collision_map_processing_name, \ 00198 TabletopCollisionMapProcessing) 00199 00200 #self.grasper_transfer_srv = rospy.ServiceProxy(self.grasper_transfer_name, GraspingAppTransfer) 00201 self.bounding_box_srv = rospy.ServiceProxy(self.find_bounding_box_name, FindClusterBoundingBox) 00202 00203 #initialize a collision map interface 00204 self.collision_map_interface = collision_map_interface.CollisionMapInterface() 00205 00206 #initialize a DrawFunctions object 00207 self.draw_functions = draw_functions.DrawFunctions('grasp_markers') 00208 00209 #the objects held in the each arm (None if no object) 00210 self.held_objects = [None]*2 00211 00212 #original pose of the object in each hand 00213 self.original_poses = [None]*2 00214 00215 #temporary height and dist until we get a table detection 00216 self.table_front_edge_x = rospy.get_param("~default_table_front_edge_x", .33) 00217 self.table_height = rospy.get_param("~default_table_height", .66) 00218 00219 #rectangle where we can put objects down (defaults to just in front of the robot, on the table) 00220 self.place_rect_dims = [.3, .6] 00221 place_rect_pose_mat = scipy.matrix([[1.,0.,0.,self.table_front_edge_x+self.place_rect_dims[0]/2.+.1], 00222 [0.,1.,0.,0.], 00223 [0.,0.,1.,self.table_height], 00224 [0.,0.,0.,1.]]) 00225 self.place_rect_pose_stamped = stamp_pose(mat_to_pose(place_rect_pose_mat), 'base_link') 00226 00227 #occupancy grid of places to put down an object 00228 self.place_grid_resolution = 5 00229 self.place_grid = [[0]*self.place_grid_resolution for i in range(self.place_grid_resolution)] 00230 00231 #saved table and object detections 00232 self.detected_table = None 00233 self.additional_tables = [] 00234 self.detected_objects = [] 00235 00236 #dictionary of which arm is which 00237 self.arm_dict = {0:'r', 1:'l'} 00238 00239 #arm-away joint angles 00240 r_side_trajectory = rospy.get_param("/arm_configurations/side/trajectory/right_arm") 00241 l_side_trajectory = rospy.get_param("/arm_configurations/side/trajectory/left_arm") 00242 self.arm_above_and_to_side_angles = [r_side_trajectory[0:7], l_side_trajectory[0:7]] 00243 self.arm_to_side_angles = [r_side_trajectory[7:14], l_side_trajectory[7:14]] 00244 # self.arm_above_and_to_side_angles = [[-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501], 00245 # [0.968, 0.729, 0.554, -1.891, 1.786, -1.127, 0.501]] 00246 # self.arm_to_side_angles = [[-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398], 00247 # [2.135, 0.803, 1.732, -1.905, 2.369, -1.680, 1.398]] 00248 00249 #dictionary of grasp/place error codes 00250 #(SUCCESS, UNFEASIBLE, FAILED, ERROR, MOVE_ARM_STUCK, LIFT_FAILED) 00251 self.result_code_dict = {} 00252 for element in dir(ManipulationResult): 00253 if element[0].isupper(): 00254 self.result_code_dict[eval('ManipulationResult.'+element)] = element 00255 00256 #dictionary of tabletop_object_detector error codes 00257 #(NO_CLOUD_RECEIVED, NO_TABLE, OTHER_ERROR, SUCCESS) 00258 self.tabletop_detection_result_dict = {} 00259 for element in dir(TabletopDetectionResult): 00260 if element[0].isupper(): 00261 self.tabletop_detection_result_dict[eval('TabletopDetectionResult.'+element)] = element 00262 00263 #name of the support surface's collision object 00264 self.collision_support_surface_name = "table" 00265 00266 #offset for place override approach (where to come in from when dropping open-loop in base_link frame; z is separate) 00267 self.place_override_approach_offset = [-.15, 0, 0] 00268 00269 00270 ##shift a place pose to be above the current table plane 00271 def shift_place_pose_height(self, object, place_pose, table, z_offset): 00272 00273 #find the height of the place_pose above the current table 00274 new_height = self.find_height_above_table(place_pose, table) 00275 00276 #figure out the desired offset to make it the same height as when it was grasped 00277 #plus the desired z_offset 00278 height_diff = object.height_above_table +z_offset - new_height 00279 00280 #don't move down (which would probably make the desired pose into the actual table) 00281 if height_diff <= 0: 00282 height_diff = 0 00283 00284 #add that offset to the place_pose plus a tiny offset 00285 place_pose.pose.position.z += height_diff + .001 00286 00287 rospy.loginfo("shifting place pose height, height_diff=%5.3f"%height_diff) 00288 00289 00290 ##find the height of an object pose above a detected table plane 00291 def find_height_above_table(self, pose, table): 00292 00293 #shift the pose into the table's header frame 00294 (pos, rot) = pose_stamped_to_lists(self.tf_listener, pose, table.pose.header.frame_id) 00295 00296 #find the pose's location in the table pose frame 00297 pos_mat = scipy.matrix(pos+[1]).T 00298 table_mat = pose_to_mat(table.pose.pose) 00299 pos_in_table_frame = table_mat**-1 * pos_mat 00300 00301 #return the z-position 00302 #rospy.loginfo("height above table: %5.3f"%pos_in_table_frame[2,0]) 00303 return pos_in_table_frame[2,0] 00304 00305 00306 ##point the head to find the front edge of the table and detect 00307 def find_table(self, point = None): 00308 if point == None: 00309 point = [self.table_front_edge_x, 0, self.table_height] 00310 self.point_head(point, 'base_link') 00311 self.call_tabletop_detection(update_table = 1, update_place_rectangle = 1) 00312 00313 00314 ##call find_cluster_bounding_box to get the bounding box for a cluster 00315 def call_find_cluster_bounding_box(self, cluster): 00316 00317 req = FindClusterBoundingBoxRequest() 00318 req.cluster = cluster 00319 try: 00320 res = self.bounding_box_srv(req) 00321 except rospy.ServiceException, e: 00322 rospy.logerr("error when calling %s: %s"%(self.find_bounding_box_name, e)) 00323 self.throw_exception() 00324 return 0 00325 return (res.pose, [res.box_dims.x, res.box_dims.y, res.box_dims.z]) 00326 00327 00328 ##call tabletop object detection and collision_map_processing 00329 #(detects table/objects and adds them to collision map) 00330 def call_tabletop_detection(self, update_table = 0, clear_attached_objects = 1, \ 00331 replace_table_in_collision_map = 1, update_place_rectangle = 0, \ 00332 num_models = 0): 00333 00334 rospy.loginfo("calling tabletop detection") 00335 00336 det_req = TabletopDetectionRequest() 00337 det_req.return_clusters = 1 00338 det_req.return_models = 1 00339 det_req.num_models = num_models 00340 00341 #call tabletop detection, get a detection result 00342 for try_num in range(3): 00343 try: 00344 det_res = self.grasper_detect_srv(det_req) 00345 except rospy.ServiceException, e: 00346 rospy.logerr("error when calling %s: %s"%(self.grasper_detect_name, e)) 00347 self.throw_exception() 00348 return ([], None) 00349 if det_res.detection.result == det_res.detection.SUCCESS: 00350 rospy.loginfo("tabletop detection reports success") 00351 break 00352 else: 00353 rospy.logerr("tabletop detection failed with error %s, trying again"%\ 00354 self.tabletop_detection_result_dict[det_res.detection.result]) 00355 else: 00356 rospy.logerr("tabletop detection failed too many times. Returning.") 00357 return ([], None) 00358 00359 col_req = TabletopCollisionMapProcessingRequest() 00360 col_req.reset_collision_models = 1 00361 col_req.reset_attached_models = clear_attached_objects 00362 col_req.detection_result = det_res.detection 00363 col_req.desired_frame = 'base_link' 00364 00365 #call collision map processing to add the detected objects to the collision map 00366 #and get back a list of GraspableObjects 00367 try: 00368 col_res = self.collision_map_processing_srv(col_req) 00369 except rospy.ServiceException, e: 00370 rospy.logerr("error when calling %s: %s"%(self.collision_map_processing_name, e)) 00371 self.throw_exception() 00372 return ([], None) 00373 00374 table = det_res.detection.table 00375 self.collision_support_surface_name = col_res.collision_support_surface_name 00376 00377 #save the new detected table (already in collision map) 00378 if update_table: 00379 self.detected_table = table 00380 self.update_table_info(update_place_rectangle) 00381 00382 #sort objects by distance from the edge of the table 00383 rospy.loginfo("detection finished, finding bounding boxes for clusters and sorting objects") 00384 dists = [] 00385 object_box_dims = [] 00386 object_poses = [] 00387 heights_above_table = [] 00388 for (ind, object) in enumerate(col_res.graspable_objects): 00389 00390 #convert the pose for database objects into the object reference frame 00391 if len(object.potential_models) != 0: 00392 pose_stamped = object.potential_models[0].pose 00393 obj_frame_pose_stamped = change_pose_stamped_frame(self.tf_listener, pose_stamped, object.reference_frame_id) 00394 (pos, rot) = pose_stamped_to_lists(self.tf_listener, pose_stamped, 'base_link') 00395 object_poses.append(obj_frame_pose_stamped) 00396 object_box_dims.append([.1, .1, .01]) 00397 dists.append(pos[0]) 00398 00399 #find the bounding box for point clusters and store the box pose (should already be in base_link frame) 00400 if len(object.potential_models) == 0: 00401 (pose_stamped, box_dims) = self.call_find_cluster_bounding_box(object.cluster) 00402 object_box_dims.append(box_dims) 00403 object_poses.append(pose_stamped) 00404 dists.append(pose_stamped.pose.position.x) 00405 00406 #find the heights of the objects above the detected table plane 00407 heights_above_table.append(self.find_height_above_table(pose_stamped, table)) 00408 00409 #print "new pose stamped:\n", object.model_pose.pose 00410 00411 indices = scipy.array(dists).argsort() 00412 00413 detected_objects = [ObjectInfo(col_res.graspable_objects[indices[i]], object_box_dims[indices[i]], \ 00414 object_poses[indices[i]], self.tf_listener, heights_above_table[indices[i]], \ 00415 col_res.collision_object_names[indices[i]]) for i in range(len(indices))] 00416 00417 self.detected_objects = detected_objects 00418 00419 #print out the new object list 00420 self.print_object_list() 00421 00422 return (detected_objects, table) 00423 00424 00425 ##adds the point cloud of object2 to object1 and updates the bounding box 00426 def combine_point_cloud_objects(self, object1, object2): 00427 00428 object1.cluster.points += object2.cluster.points 00429 00430 #re-find the bounding box 00431 (pose_stamped, box_dims) = self.call_find_cluster_bounding_box(object1.cluster) 00432 object1.pose = pose_stamped 00433 object1.box_dims.append = box_dims 00434 00435 return object1 00436 00437 00438 ##make sure the joint controllers are on 00439 def check_joint_controllers(self, whicharm = None): 00440 00441 if whicharm: 00442 self.cms[whicharm].check_controllers_ok('joint') 00443 else: 00444 for arm in self.arms_to_use_list: 00445 self.cms[arm].check_controllers_ok('joint') 00446 00447 00448 ##tell the grasp action to grasp an object (whicharm: right = 0, left = 1) 00449 def grasp_object(self, object, whicharm = None, use_reactive_grasp = 1, use_slip_detection = 0): 00450 rospy.loginfo("attempting to grasp an object, whicharm = %s"%str(whicharm)) 00451 00452 if not self.check_arms_to_use(whicharm): 00453 return (0, None, None) 00454 00455 #make sure the joint controllers are on 00456 self.check_joint_controllers(whicharm) 00457 00458 #open the gripper 00459 self.open_gripper(whicharm) 00460 00461 # #tell reactive grasping not to close too hard 00462 # if whicharm == 0: 00463 # rospy.set_param('reactive_grasp_node_right/close_force', 20) 00464 # else: 00465 # rospy.set_param('reactive_grasp_node_left/close_force', 20) 00466 00467 #put together the grasp goal 00468 goal = PickupGoal() 00469 00470 #tell the grasp action how to lift 00471 goal.lift = GripperTranslation() 00472 goal.lift.desired_distance = .1 00473 goal.lift.min_distance = 0. 00474 goal.lift.direction = create_vector3_stamped([0.,0.,1.], 'base_link') 00475 00476 goal.target = object.object 00477 goal.use_reactive_execution = use_reactive_grasp 00478 goal.use_reactive_lift = use_slip_detection 00479 goal.collision_object_name = object.collision_name 00480 goal.collision_support_surface_name = self.collision_support_surface_name 00481 goal.max_contact_force = 50. 00482 00483 print "sending object collision name of:", object.collision_name 00484 if whicharm == 0: 00485 goal.arm_name = "right_arm" 00486 else: 00487 goal.arm_name = "left_arm" 00488 00489 #send the goal 00490 try: 00491 self.grasper_grasp_action_client.send_goal(goal) 00492 except rospy.ServiceException, e: 00493 rospy.logerr("error when calling %s"%self.grasper_grasp_name) 00494 self.throw_exception() 00495 return ("ERROR", None, None) 00496 00497 #wait for result 00498 finished_within_time = self.grasper_grasp_action_client.wait_for_result(rospy.Duration(240)) 00499 if not finished_within_time: 00500 self.grasper_grasp_action_client.cancel_goal() 00501 rospy.logerr("timed out when asking the grasp action client to grasp") 00502 return (0, None, None) 00503 00504 #return the result 00505 result = self.grasper_grasp_action_client.get_result() 00506 resultval = self.result_code_dict[result.manipulation_result.value] 00507 rospy.loginfo("grasp result: %s"%resultval) 00508 00509 return (resultval, whicharm, result.grasp) 00510 00511 00512 ##distance from a point (2-list) to a bounding box (projected onto table plane) 00513 #returns 0 if the point is inside; positive if it's outside 00514 def point_to_box_dist(self, point, box_pose, box_dims): 00515 00516 #convert point to the object (pose) frame 00517 box_mat = pose_to_mat(box_pose.pose) 00518 box_mat[2, 3] = 0. 00519 point4 = scipy.matrix([point[0], point[1], 0, 1]).T 00520 box_frame_point = (box_mat**-1 * point4).T.tolist()[0] 00521 00522 #find the distance from the rectangle in 2-D (inside rectangle has dist of 0) 00523 dist = 0 00524 for dim in range(2): 00525 if box_frame_point[dim] < -box_dims[dim]/2: 00526 dist += (-box_dims[dim]/2 - box_frame_point[dim])**2 00527 elif box_frame_point[dim] > box_dims[dim]/2: 00528 dist += (box_frame_point[dim] - box_dims[dim]/2)**2 00529 dist = dist**.5 00530 00531 return dist 00532 00533 00534 ##return the sign of only the (x,y) components of (p1-p2)x(p3-p2) for convex hull inclusion testing 00535 #p1, p2, and p3 are lists of coords 00536 def find_cross_direction(self, p1, p2, p3): 00537 z = p1[0] * (p2[1] - p3[1]) + p2[0] * (p3[1] - p1[1]) + p3[0] * (p1[1] - p2[1]) 00538 return z > 0 00539 00540 00541 ##fills in place grid spots that are already taken by objects seen by the last tabletop detection 00542 def fill_in_taken_place_grid_spots(self, buffer = .05): 00543 00544 #go through each grid point 00545 for i in range(self.place_grid_resolution): 00546 for j in range(self.place_grid_resolution): 00547 00548 self.place_grid[i][j] = 0 00549 00550 #2-D location of the grid point 00551 point = self.place_grid_location(i, j) 00552 00553 #check if it's within buffer distance of a detected object's bounding box 00554 for (ind, object) in enumerate(self.detected_objects): 00555 dist = self.point_to_box_dist(point, object.pose, object.box_dims) 00556 00557 #if so, mark that spot as taken 00558 if dist < buffer: 00559 rospy.loginfo("grid point %d %d is within %4.2f of detected object %d, dist=%4.2f"\ 00560 %(i, j, buffer, ind, dist)) 00561 self.place_grid[i][j] = 1 00562 # else: 00563 # rospy.loginfo("grid point %d %d is clear of object %d, dist=%4.2f" %(i, j, ind, dist)) 00564 00565 #check if the point is above the table's convex hull 00566 if self.detected_table and len(self.detected_table.convex_hull.vertices) >= 2: 00567 vertices = self.detected_table.convex_hull.vertices 00568 direction = self.find_cross_direction(get_xyz(vertices[0]), get_xyz(vertices[1]), point) 00569 for ind in range(1, len(vertices)-2): 00570 next_dir = self.find_cross_direction(get_xyz(vertices[ind]), get_xyz(vertices[ind+1]), point) 00571 if direction != next_dir: 00572 00573 #if the point is not above the table, mark it as taken 00574 self.place_grid[i][j] = 1 00575 rospy.loginfo("grid point %d %d is not above the table"%(i,j)) 00576 break 00577 00578 00579 00580 ##tell the place service to place an object held by whicharm at a particular location (right = 0, left = 1) 00581 #expressed as a PoseStamped 00582 def place_object(self, whicharm, pose, padding = .05, constrained = False): 00583 if not self.check_arms_to_use(whicharm): 00584 return "ERROR" 00585 00586 rospy.loginfo("attempting to place object") 00587 if constrained == True: 00588 rospy.loginfo("using constrained place"); 00589 #print "requested pose:", pose 00590 00591 # #recognized object, just send the object pose 00592 # if self.held_objects[whicharm].type == 'mesh': 00593 # msg_pose = pose 00594 00595 #point cluster, send back the cluster frame origin relative to the new desired pose 00596 #(cluster origin to base_link) = (new object to base_link) * (cluster to object) 00597 # else: 00598 pose_mat = pose_to_mat(pose.pose) 00599 cluster_to_base_link_mat = pose_mat * self.held_objects[whicharm].cluster_origin_to_bounding_box 00600 cluster_to_base_link_pose = mat_to_pose(cluster_to_base_link_mat) 00601 msg_pose = stamp_pose(cluster_to_base_link_pose, 'base_link') 00602 # print "cluster_origin_to_bounding_box:\n", ppmat(self.held_objects[whicharm].cluster_origin_to_bounding_box) 00603 # print "pose_mat:\n", ppmat(pose_mat) 00604 # print "cluster_to_base_link_mat:\n", ppmat(cluster_to_base_link_mat) 00605 00606 #re-stamp the pose 00607 msg_pose.header.stamp = rospy.Time.now() 00608 00609 goal = PlaceGoal() 00610 if whicharm == 0: 00611 goal.arm_name = "right_arm" 00612 rospy.loginfo("asking the right arm to place") 00613 00614 elif whicharm == 1: 00615 goal.arm_name = "left_arm" 00616 rospy.loginfo("asking the left arm to place") 00617 00618 goal.place_locations = [msg_pose] 00619 goal.grasp = self.held_objects[whicharm].grasp 00620 goal.desired_retreat_distance = 0.1 00621 goal.min_retreat_distance = 0.05 00622 00623 goal.approach = GripperTranslation() 00624 goal.approach.desired_distance = .1 00625 goal.approach.min_distance = 0.05 00626 goal.approach.direction = create_vector3_stamped([0.,0.,-1.], 'base_link') 00627 00628 goal.collision_object_name = self.held_objects[whicharm].collision_name 00629 goal.collision_support_surface_name = self.collision_support_surface_name 00630 goal.place_padding = padding 00631 #goal.use_reactive_place = False 00632 goal.use_reactive_place = self.use_slip_detection 00633 if constrained == True: 00634 current_pose = self.cms[whicharm].get_current_wrist_pose_stamped('base_link') 00635 orientation_constraint = self.get_keep_object_level_constraint(whicharm,current_pose) 00636 goal.path_constraints.orientation_constraints.append(orientation_constraint) 00637 00638 #send the goal 00639 try: 00640 self.grasper_place_action_client.send_goal(goal) 00641 except rospy.ServiceException, e: 00642 rospy.logerr("error when calling %s", self.grasper_place_name) 00643 self.throw_exception() 00644 return "ERROR" 00645 00646 #wait for result 00647 finished_within_time = self.grasper_place_action_client.wait_for_result(rospy.Duration(240)) 00648 if not finished_within_time: 00649 self.grasper_place_action_client.cancel_goal() 00650 rospy.logerr("timed out when asking the place action client to place") 00651 return "FAILED" 00652 00653 #return the result 00654 result = self.grasper_place_action_client.get_result() 00655 resultval = self.result_code_dict[result.manipulation_result.value] 00656 rospy.loginfo("place result: %s"%resultval) 00657 return resultval 00658 00659 00660 ##find an IK solution for a pose mat 00661 def find_IK_solution(self, whicharm, pose_mat, start_angles, collision_aware = 1): 00662 pose_stamped = stamp_pose(mat_to_pose(pose_mat), 'base_link') 00663 (joint_angles, error_code) = self.cms[whicharm].ik_utilities.run_ik(pose_stamped, \ 00664 start_angles, self.cms[whicharm].ik_utilities.link_name, \ 00665 collision_aware = collision_aware) 00666 return (joint_angles, error_code) 00667 00668 00669 #make sure whicharm is consistent with the arms being used 00670 def check_arms_to_use(self, whicharm): 00671 if whicharm == 0 and self.arms_to_use not in ["right", "both"]: 00672 rospy.logerr("not using right arm!") 00673 return 0 00674 if whicharm == 1 and self.arms_to_use not in ["left", "both"]: 00675 rospy.logerr("not using left arm!") 00676 return 0 00677 return 1 00678 00679 00680 ##place the object open-loop, shoving other objects aside 00681 def place_object_override(self, whicharm, pose, z_dist = .1, use_joint_open_loop = 0): 00682 rospy.loginfo("last-resort place") 00683 00684 if not self.check_arms_to_use(whicharm): 00685 return 0 00686 00687 00688 #figure out where the wrist has to be to make the object be at pose 00689 if self.held_objects[whicharm] and not self.held_objects[whicharm].grasp_type == 'flat': 00690 00691 # #recognized object, saved grasp pose is wrist pose in object frame 00692 # if self.held_objects[whicharm].type == 'mesh': 00693 00694 # #(wrist to base_link) = (object to base_link) * (wrist to object) 00695 # wrist_to_object_mat = pose_to_mat(self.held_objects[whicharm].grasp_pose) 00696 # object_to_base_link_mat = pose_to_mat(pose.pose) 00697 # wrist_to_base_link_mat = object_to_base_link_mat * wrist_to_object_mat 00698 00699 #point cluster, saved grasp pose is wrist pose in cluster frame 00700 # else: 00701 #(wrist to base link) = (object to base_link) * (cluster origin to object) * (wrist to cluster) 00702 wrist_to_cluster_mat = pose_to_mat(self.held_objects[whicharm].grasp_pose) 00703 cluster_to_object_mat = self.held_objects[whicharm].cluster_origin_to_bounding_box 00704 object_to_base_link_mat = pose_to_mat(pose.pose) 00705 wrist_to_base_link_mat = object_to_base_link_mat * cluster_to_object_mat * wrist_to_cluster_mat 00706 00707 wrist_pose = stamp_pose(mat_to_pose(wrist_to_base_link_mat), 'base_link') 00708 00709 #draw the wrist pose 00710 self.draw_functions.draw_rviz_axes(wrist_to_base_link_mat, 'base_link', id = 0, duration = 60.) 00711 00712 00713 #if we don't think there's an object in the hand (or if we're dropping a flat object), 00714 #assume the gripper orientation should be held fixed 00715 #and let go of the object above pose (pose orientation should contain the current gripper orientation already) 00716 else: 00717 #figure out how high to raise the gripper to put the middle of the fingertips just above the loc 00718 if self.held_objects[whicharm] and self.held_objects[whicharm].grasp_type == 'flat': 00719 00720 #use the longest box dim plus a bit for flat objects, and rotate the gripper by 90 degrees 00721 pose.pose.position.z += self.held_objects[whicharm].box_dims[0]+.05 00722 pose = stamp_pose(mat_to_pose(pose_to_mat(pose.pose) * \ 00723 scipy.matrix(tf.transformations.rotation_matrix(math.pi/2, [1,0,0]))), 'base_link') 00724 else: 00725 pose.pose.position.z += .1 00726 fingertip_to_base_link_mat = pose_to_mat(pose.pose) 00727 wrist_to_fingertip_mat = scipy.matrix(tf.transformations.translation_matrix([-.167, 0, 0])) 00728 wrist_to_base_link_mat = fingertip_to_base_link_mat * wrist_to_fingertip_mat 00729 wrist_pose = pose 00730 set_xyz(wrist_pose.pose.position, wrist_to_base_link_mat[0:3, 3].T.tolist()[0]) 00731 00732 # #attach a medium-sized box to the gripper so the object isn't guaranteed to hit the table 00733 # box_dims = [.15]*3 00734 # current_wrist_pose = self.cms[whicharm].get_current_wrist_pose_stamped('base_link') 00735 # box_to_wrist = scipy.matrix(tf.transformations.translation_matrix([.12+box_dims[0], 0, 0])) 00736 # box_pose = mat_to_pose(pose_to_mat(current_wrist_pose.pose) * box_to_wrist) 00737 # self.collision_map_interface.add_collision_box(box_pose, box_dims, 'base_link', 'temporary_box') 00738 # self.attach_object(whicharm, 'temporary_box') 00739 00740 #come in from the front, just above the table 00741 front_above_pose = copy.deepcopy(wrist_pose) 00742 00743 #temporary, while deepcopy of rospy.Time is broken 00744 front_above_pose.header.stamp = rospy.Time(wrist_pose.header.stamp.secs) 00745 front_above_pose.pose.position.x += self.place_override_approach_offset[0] 00746 front_above_pose.pose.position.y += self.place_override_approach_offset[1] 00747 front_above_pose.pose.position.z += self.place_override_approach_offset[2] + z_dist 00748 00749 #draw the wrist_pose and front_above_pose 00750 self.draw_functions.draw_rviz_axes(wrist_to_base_link_mat, 'base_link', id = 0, duration = 60.) 00751 front_above_mat = pose_to_mat(front_above_pose.pose) 00752 self.draw_functions.draw_rviz_axes(front_above_mat, 'base_link', id = 1, duration = 60.) 00753 00754 #ask move arm to get us there, or move open-loop if move arm fails and we can find an IK solution 00755 result = self.try_hard_to_move_pose(whicharm, front_above_pose, use_joint_open_loop = use_joint_open_loop, \ 00756 use_cartesian = 0, try_constrained = 1) 00757 if not result: 00758 return 0 00759 00760 #now move to the place pose using the Cartesian controllers 00761 #self.cms[whicharm].move_cartesian(wrist_pose, settling_time = rospy.Duration(10)) 00762 00763 #now move to the place pose using reactive place 00764 self.call_reactive_place(whicharm, wrist_pose) 00765 00766 #drop the object, if we haven't already opened the gripper 00767 self.open_gripper(whicharm) 00768 00769 #detach the object (and add it back where it was dropped if we knew what object it was) 00770 if self.held_objects[whicharm]: 00771 self.detach_and_add_back_object(whicharm) 00772 else: 00773 self.detach_object(whicharm) 00774 00775 #lift the arm 00776 wrist_pose.pose.position.z += .2 00777 self.cms[whicharm].move_cartesian(wrist_pose, settling_time = rospy.Duration(10), timeout = rospy.Duration(3)) 00778 00779 #get out of there 00780 self.move_arm_to_side(whicharm) 00781 return 1 00782 00783 00784 ##call the reactive place service for the appropriate arm 00785 def call_reactive_place(self, whicharm, place_pose): 00786 00787 goal = ReactivePlaceGoal() 00788 goal.final_place_pose = place_pose 00789 00790 self.reactive_place_clients[whicharm].send_goal(goal) 00791 self.reactive_place_clients[whicharm].wait_for_result() 00792 result = self.reactive_place_clients[whicharm].get_result() 00793 00794 return result 00795 00796 00797 ##attach the object in the hand to the gripper (right = 0, left = 1) 00798 def attach_object(self, whicharm, object_name): 00799 rospy.loginfo("attaching object %s to gripper %s"%(object_name, self.arm_dict[whicharm])) 00800 self.collision_map_interface.attach_object_to_gripper(object_name, self.arm_dict[whicharm]) 00801 00802 00803 ##detach the object in the hand from the gripper (right = 0, left = 1) 00804 def detach_object(self, whicharm): 00805 rospy.loginfo("detaching object from %s gripper"%self.arm_dict[whicharm]) 00806 self.collision_map_interface.detach_all_objects_from_gripper(self.arm_dict[whicharm]) 00807 00808 00809 ##detach and add the object back to the collision map 00810 def detach_and_add_back_object(self, whicharm, collision_name = None): 00811 if collision_name == None: 00812 if self.held_objects[whicharm]: 00813 rospy.loginfo("detaching object %s from %s gripper and adding it back to the collision map" \ 00814 %(self.held_objects[whicharm].collision_name, self.arm_dict[whicharm])) 00815 self.collision_map_interface.detach_and_add_back_objects_attached_to_gripper(self.arm_dict[whicharm], \ 00816 self.held_objects[whicharm].collision_name) 00817 else: 00818 rospy.loginfo("gripper doesn't think it's holding any objects; detaching all objects without re-adding") 00819 self.collision_map_interface.detach_all_objects_from_gripper(self.arm_dict[whicharm]) 00820 else: 00821 self.collision_map_interface.detach_and_add_back_objects_attached_to_gripper(self.arm_dict[whicharm], collision_name) 00822 00823 00824 ##remove an object from the collision map 00825 def remove_object(self, collision_name): 00826 rospy.loginfo("removing object %s from the collision map"%collision_name) 00827 self.collision_map_interface.remove_collision_object(collision_name) 00828 00829 00830 # ##remove an object and add it back in as the current bounding box 00831 # def update_object_box(self, object_info): 00832 00833 # self.remove_object(object_info.collision_name) 00834 # self.collision_map_interface.add_collision_box(object_info.pose, object_info.box_dims, \ 00835 # 'base_link', object_info.collision_name) 00836 00837 00838 ##point the head (really the narrow-stereo camera frame) at a location 00839 def point_head(self, point, frame, pause = 1): 00840 goal = PointHeadGoal() 00841 goal.target = create_point_stamped(point, frame) 00842 goal.pointing_frame = "/narrow_stereo_optical_frame" 00843 goal.max_velocity = 1.0 00844 00845 self.head_action_client.send_goal(goal) 00846 finished_within_time = self.head_action_client.wait_for_result(rospy.Duration(3)) 00847 if not finished_within_time: 00848 self.head_action_client.cancel_goal() 00849 rospy.logerr("timed out when asking the head to point to a new goal") 00850 return 0 00851 00852 #sleep a bit so a new point cloud has time to be generated 00853 if pause: 00854 time.sleep(1.0) 00855 return 1 00856 00857 00858 ##use the table detection to update the table information 00859 #if adjust_place_rectangle is 1, adjusts a place rectangle on the table (assumed base_link frame) 00860 #with the new table params 00861 def update_table_info(self, adjust_place_rectangle = 0): 00862 00863 #find the table's front edge and height 00864 base_link_pose_stamped = change_pose_stamped_frame(self.tf_listener, self.detected_table.pose, 'base_link') 00865 table_mat = pose_to_mat(base_link_pose_stamped.pose) 00866 corners = scipy.matrix([[self.detected_table.x_min, self.detected_table.y_min,0,1], 00867 [self.detected_table.x_min, self.detected_table.y_max,0,1], 00868 [self.detected_table.x_max, self.detected_table.y_min,0,1], 00869 [self.detected_table.x_max, self.detected_table.y_max,0,1]]).T 00870 transformed_corners = table_mat * corners 00871 front_edge_x = transformed_corners[0,:].min() 00872 height = transformed_corners[2,:].max() 00873 self.table_front_edge_x = front_edge_x 00874 self.table_height = height 00875 rospy.loginfo("table front edge x: %5.3f"%front_edge_x) 00876 rospy.loginfo("table height: %5.3f"%height) 00877 00878 #adjust the place rectangle with the new table params 00879 if adjust_place_rectangle: 00880 #print "self.table_height before update:", self.table_height 00881 place_rect_pose_mat = pose_to_mat(self.transform_place_rect_pose()) 00882 #print "place_rect_pose_mat before update:\n", ppmat(place_rect_pose_mat) 00883 place_rect_pose_mat[0, 3] = self.table_front_edge_x+self.place_rect_dims[0]/2.+.1 00884 place_rect_pose_mat[2, 3] = self.table_height 00885 self.place_rect_pose_stamped = stamp_pose(mat_to_pose(place_rect_pose_mat), 'base_link') 00886 #print "place_rect_pose_mat:\n", ppmat(place_rect_pose_mat) 00887 00888 #draw the new place grid 00889 self.set_place_area(self.place_rect_pose_stamped, self.place_rect_dims) 00890 00891 00892 ##convert the place rectangle pose to base_link frame if it isn't already (returns pose, not pose_stamped) 00893 def transform_place_rect_pose(self): 00894 if self.place_rect_pose_stamped.header.frame_id != 'base_link': 00895 transformed_rect_pose = change_pose_stamped_frame(self.tf_listener, \ 00896 self.place_rect_pose_stamped, 'base_link').pose 00897 else: 00898 transformed_rect_pose = self.place_rect_pose_stamped.pose 00899 return transformed_rect_pose 00900 00901 00902 ##set a rectangle centered at rect_pose_stamped of dims (x, y) to be the desired place area 00903 def set_place_area(self, rect_pose_stamped, dims): 00904 self.place_rect_dims = dims 00905 self.place_rect_pose_stamped = rect_pose_stamped 00906 00907 self.draw_place_area() 00908 00909 00910 ##draw the current place area as a set of boxes 00911 def draw_place_area(self): 00912 #convert the place rectangle pose to base_link frame if it isn't already (just for drawing) 00913 transformed_rect_pose = self.transform_place_rect_pose() 00914 place_grid_loc_mat = pose_to_mat(transformed_rect_pose) 00915 self.draw_functions.draw_rviz_box(place_grid_loc_mat, [.01, .01, .01], duration = 0,\ 00916 frame = 'base_link', \ 00917 color = [1,0,0], id=101) 00918 00919 #draw boxes at each place location 00920 for i in range(self.place_grid_resolution): 00921 for j in range(self.place_grid_resolution): 00922 location = self.place_grid_location(i,j) 00923 place_grid_loc_mat[0,3] = location[0] 00924 place_grid_loc_mat[1,3] = location[1] 00925 self.draw_functions.draw_rviz_box(place_grid_loc_mat, [.01, .01, .01], duration = 0,\ 00926 frame = 'base_link', color = [0,0,1], id=1000+self.place_grid_resolution*i+j) 00927 00928 00929 ##return the location of a point in the place grid 00930 def place_grid_location(self, xind, yind): 00931 00932 grid_x_size = self.place_rect_dims[0]/self.place_grid_resolution 00933 grid_y_size = self.place_rect_dims[1]/self.place_grid_resolution 00934 grid_center = int(math.floor(self.place_grid_resolution/2.)) #make place_grid_resolution odd for now 00935 00936 #convert the place rectangle pose to base_link frame if it isn't already 00937 transformed_rect_pose = self.transform_place_rect_pose() 00938 00939 #convert the place grid relative location to a location in the base_link frame (returned as list) 00940 rel_location = scipy.matrix([(grid_center-xind)*grid_x_size, (grid_center-yind)*grid_y_size, 0, 1]).T 00941 location = pose_to_mat(transformed_rect_pose) * rel_location 00942 location = location.T.tolist()[0] 00943 return location 00944 00945 00946 ##return the pose of the object at a place grid location 00947 def place_grid_pose(self, xind, yind, whicharm, add_noise = 0): 00948 00949 #find the place grid location for xind, yind 00950 location = self.place_grid_location(xind, yind) 00951 rospy.loginfo("proposed place location: %5.3f, %5.3f"%(location[0], location[1])) 00952 00953 #create a pose_stamped for the object at that location based on its original pose 00954 if self.held_objects[whicharm] and not self.held_objects[whicharm].grasp_type == 'flat': 00955 place_pose = copy.deepcopy(self.held_objects[whicharm].pose) 00956 00957 #temporary, while deepcopy of rospy.Time is broken 00958 #place_pose.header.stamp = rospy.Time(self.held_objects[whicharm].pose.header.stamp.secs) 00959 00960 #no object in the hand, use the current wrist orientation and change the location 00961 else: 00962 place_pose = self.cms[whicharm].get_current_wrist_pose_stamped('base_link') 00963 00964 #set the place_pose z to be the place grid center z 00965 transformed_rect_pose = self.transform_place_rect_pose() 00966 place_pose.pose.position.z = transformed_rect_pose.position.z 00967 00968 #and the x and y to be the current grid location x and y 00969 place_pose.pose.position.x = location[0] 00970 place_pose.pose.position.y = location[1] 00971 00972 #add noise to the place pose, if requested 00973 if add_noise: 00974 rospy.loginfo("adding noise to place pose") 00975 place_pose.pose.position.x += scipy.rand()*.05 00976 place_pose.pose.position.y += scipy.rand()*.05 00977 00978 #draw the object's visualization box (bounding or at-the-base) at the place location 00979 self.draw_object_pose(self.held_objects[whicharm], place_pose, [0,1,1], 100) 00980 00981 return place_pose 00982 00983 00984 ##draw a visualization box (bounding or at-the-base) for an object at a PoseStamped 00985 def draw_object_pose(self, object_info, pose_stamped, color, id, duration = 300.): 00986 pose_mat = pose_to_mat(pose_stamped.pose) 00987 if object_info: 00988 box_dims = object_info.box_dims 00989 else: 00990 box_dims = [.05, .05, .05] 00991 self.draw_functions.draw_rviz_box(pose_mat, box_dims, duration = duration,\ 00992 frame = pose_stamped.header.frame_id, color = color, id = id) 00993 00994 00995 ##choose a new location to try to place an object held by whicharm (right = 0, left = 1) 00996 #if override_pose >=0, then pick an override pose from front to back 00997 def choose_place_location(self, whicharm, override_pose = -1, z_offset = 0): 00998 00999 #start from the back and work your way to the front 01000 i = j = 0 01001 01002 #unless this is an open-loop place, in which case start from the front and work your way back 01003 num_poses_to_skip = 0 01004 if override_pose >= 0: 01005 i = self.place_grid_resolution-1 01006 num_poses_to_skip = override_pose 01007 01008 #go through the grid, looking for a good spot 01009 for grid_ind in range(self.place_grid_resolution**2+num_poses_to_skip): 01010 if self.place_grid[i][j] != 0: 01011 01012 #we only want empty/not previously colliding grid spots (=0) 01013 #unless it's an override pose, in which case we'll settle for anything that 01014 #isn't explictly taken (=1) 01015 if override_pose < 0 or self.place_grid[i][j] == 1: 01016 01017 if num_poses_to_skip == 0: 01018 rospy.loginfo("place_grid spot %d %d taken with value %d"%(i,j, self.place_grid[i][j])) 01019 j = j+1 01020 if j > self.place_grid_resolution - 1: 01021 j = 0 01022 if override_pose < 0: 01023 i = (i+1)%self.place_grid_resolution 01024 else: 01025 i = (i-1)%self.place_grid_resolution 01026 continue 01027 01028 #for an override pose, skip over good poses that we've tried before 01029 if num_poses_to_skip > 0: 01030 num_poses_to_skip -= 1 01031 j = j+1 01032 if j > self.place_grid_resolution - 1: 01033 j = 0 01034 i = (i-1)%self.place_grid_resolution 01035 continue 01036 01037 rospy.loginfo("returning grid location %d %d"%(i, j)) 01038 01039 #compute the relevant pose 01040 if override_pose >= 0 or z_offset > 0: 01041 pose = self.place_grid_pose(i,j, whicharm, add_noise = 0) 01042 else: 01043 pose = self.place_grid_pose(i,j, whicharm) 01044 01045 #add the desired z-offset to make sure we're not just grazing the table 01046 pose.pose.position.z += z_offset 01047 01048 return (i, j, pose, 0) 01049 01050 rospy.logerr("all grid locations are taken! Returning") 01051 pose = self.place_grid_pose(i,j, whicharm) 01052 return (i,j, pose, 1) 01053 01054 01055 ##reset all the -1s (temporary collision entries) in the place grid 01056 def reset_place_grid_temporary_collisions(self): 01057 for i in range(self.place_grid_resolution): 01058 for j in range(self.place_grid_resolution): 01059 if self.place_grid[i][j] == -1: 01060 self.place_grid[i][j] = 0 01061 01062 01063 ##rotate the pose_stamped about the base-link z-axis by rotation rad 01064 #but keep the position the same 01065 def rotate_pose(self, pose_stamped, rotation): 01066 if rotation == 0: 01067 return pose_stamped 01068 01069 rot_mat = scipy.matrix(tf.transformations.rotation_matrix(rotation, [0,0,1])) 01070 rotated_pose_stamped = transform_pose_stamped(pose_stamped, rot_mat, "pre") 01071 rotated_pose_stamped.pose.position = copy.deepcopy(pose_stamped.pose.position) 01072 01073 return rotated_pose_stamped 01074 01075 01076 ##point the head at the place rectangle 01077 def point_head_at_place_rect(self, offset = 0): 01078 01079 rospy.loginfo("pointing the head at the place rectangle") 01080 01081 #convert the place rect pose to base_link if it isn't there already 01082 transformed_rect_pose = self.transform_place_rect_pose() 01083 01084 #grab the center 01085 (x,y,z) = get_xyz(transformed_rect_pose.position) 01086 01087 #point the head just off of that position 01088 if offset: 01089 self.point_head([x-.15, y*.7, z], 'base_link') 01090 else: 01091 self.point_head([x,y,z], 'base_link') 01092 01093 01094 ##put down an object held in whicharm (right = 0, left = 1) 01095 def put_down_object(self, whicharm, max_place_tries = None, use_place_override = 0, move_to_side = True, constrained = False, 01096 update_table = True, update_place_rectangle = True, point_head = True): 01097 01098 if not self.check_arms_to_use(whicharm): 01099 return 0 01100 01101 padding = .02 #initial padding around recognized objects (cm) 01102 01103 #make sure the joint controllers are on 01104 self.check_joint_controllers(whicharm) 01105 01106 #check if there's actually something still in the gripper (if not, give up) 01107 if not self.check_grasp_successful(whicharm): 01108 rospy.loginfo("no object in gripper! Stopping put-down") 01109 01110 #detach the object and zero out held objects 01111 self.held_objects[whicharm] = 0 01112 self.detach_object(whicharm) 01113 return 1 01114 01115 self.check_preempted() 01116 01117 #point head at the place rectangle 01118 if point_head: 01119 self.point_head_at_place_rect() 01120 01121 self.check_preempted() 01122 01123 #detect objects and table (and clear out extra collision points) 01124 (objects, table) = self.call_tabletop_detection(update_table = update_table, 01125 update_place_rectangle = update_place_rectangle, 01126 clear_attached_objects = 0) 01127 01128 self.check_preempted() 01129 01130 #mark spots that are too close to detected objects as taken 01131 if self.held_objects[whicharm]: 01132 self.fill_in_taken_place_grid_spots(buffer = min(self.held_objects[whicharm].box_dims[0:2])+.03) 01133 else: 01134 self.fill_in_taken_place_grid_spots(buffer = padding) 01135 01136 self.check_preempted() 01137 01138 #how many times to do a regular place before trying the override place 01139 if max_place_tries == None and self.held_objects[whicharm] and not self.held_objects[whicharm].grasp_type == 'flat': 01140 place_tries_left = self.place_grid_resolution**2*2 01141 elif not self.held_objects[whicharm] or self.held_objects[whicharm].grasp_type == 'flat': 01142 place_tries_left = 0 #no object in the hand, or flat object, go directly to override place 01143 else: 01144 place_tries_left = max_place_tries 01145 01146 #try a bunch of times to place carefully 01147 success = 0 01148 z_offset = 0.01 01149 max_z_offset = 0.02 01150 while(place_tries_left > 0): 01151 rospy.loginfo("place_tries_left: %d"%place_tries_left) 01152 place_tries_left -= 1 01153 01154 self.check_preempted() 01155 01156 #check to make sure there's still something in the gripper (if not, give up) 01157 if not self.check_grasp_successful(whicharm): 01158 rospy.loginfo("no object in gripper! Stopping put-down") 01159 01160 #detach the object and zero out held objects 01161 self.held_objects[whicharm] = 0 01162 self.detach_object(whicharm) 01163 return 1 01164 01165 #search for a place to put the object 01166 (i, j, pose, full) = self.choose_place_location(whicharm, -1, z_offset) 01167 01168 #choose_place_location didn't find any suitable place locations, so raise z_offset (quit if already too high) 01169 if full == 1: 01170 if z_offset >= max_z_offset: 01171 break 01172 01173 #raise the z-offset and try again 01174 else: 01175 z_offset += .01 01176 padding = .01 01177 rospy.loginfo("grid was full, raising the z_offset to %5.3f and lowering the padding"%z_offset) 01178 self.fill_in_taken_place_grid_spots(buffer = padding) 01179 (i, j, pose, full) = self.choose_place_location(whicharm, -1, z_offset) 01180 01181 #all grid locations were 1, never mind 01182 if full == 1: 01183 rospy.loginfo("all grid locations were 1, moving on to last-resort place") 01184 break 01185 01186 self.check_preempted() 01187 01188 #shift the object to be above the current detected table, if there is one 01189 if table: 01190 self.shift_place_pose_height(self.held_objects[whicharm], pose, table, z_offset) 01191 01192 self.check_preempted() 01193 01194 #try to place it there at one of several rotations 01195 inc = math.pi/4. 01196 rotationlist = [0.,] 01197 for mult in range(1, int(math.floor(math.pi/inc))+1): 01198 rotationlist.append(mult*inc) 01199 rotationlist.append(-mult*inc) 01200 01201 success = 0 01202 for rotation in rotationlist: 01203 rotated_pose = self.rotate_pose(pose, rotation) 01204 self.draw_object_pose(self.held_objects[whicharm], rotated_pose, [0,1,1], 100) 01205 01206 #call the place object service 01207 result = self.place_object(whicharm, rotated_pose, padding, constrained) 01208 if result == "SUCCESS": 01209 success = 1 01210 break 01211 elif result == "ERROR": 01212 #for some reason, the retreat trajectory tends to time out when the object is dropped before placing 01213 #which causes an error to be thrown (but we don't need to worry about it) 01214 success = 1 01215 #self.throw_exception() 01216 break 01217 elif result == "FAILED": 01218 rospy.logerr("place object returned failed, arm is probably stuck (opening the gripper and resetting map objects before moving back to the side)") 01219 self.open_gripper(whicharm) 01220 self.reset_collision_map() 01221 success = 1 01222 break 01223 elif result == "RETREAT_FAILED": 01224 rospy.loginfo("place object returned retreat failed; doing an open-loop retreat") 01225 wrist_pose = self.cms[whicharm].get_current_wrist_pose_stamped(frame = 'base_link') 01226 wrist_mat = pose_to_mat(wrist_pose.pose) 01227 shift = scipy.matrix(tf.transformations.translation_matrix([-.1, 0, 0])) 01228 retreat_mat = wrist_mat * shift 01229 retreat_pose = stamp_pose(mat_to_pose(retreat_mat), 'base_link') 01230 self.move_cartesian_step(whicharm, retreat_pose, timeout = 5.0, \ 01231 settling_time = 3.0, blocking = 1) 01232 success = 1 01233 break 01234 elif result == "MOVE_ARM_STUCK": 01235 rospy.loginfo("grasp object returned move arm stuck; resetting map objects and moving to side before trying next place") 01236 self.reset_collision_map() 01237 self.move_arm_to_side(whicharm, try_constrained = 1) 01238 01239 self.check_preempted() 01240 01241 #succeeded, stop 01242 if success: 01243 break 01244 01245 #didn't succeed at any rotation, go on to the next place 01246 else: 01247 rospy.loginfo("marking place grid spot %d %d as colliding" %(i,j)) 01248 self.place_grid[i][j] = -1 01249 01250 01251 #place failed, put it down open-loop at an override position 01252 if not success and use_place_override: 01253 self.check_preempted() 01254 01255 for tries in range(self.place_grid_resolution**2*2): 01256 rospy.loginfo("place object override try number:%d"%tries) 01257 01258 (i,j,pose,z_offset) = self.choose_place_location(whicharm, override_pose = tries) 01259 01260 #try to place it there at one of several rotations 01261 rotationlist = [0, math.pi/4, -math.pi/4, math.pi/2, -math.pi/2] 01262 for rotation in rotationlist: 01263 rotated_pose = self.rotate_pose(pose, rotation) 01264 if self.held_objects[whicharm]: 01265 self.draw_object_pose(self.held_objects[whicharm], rotated_pose, [1,0,0], 100) 01266 01267 z_dist = .1 * (math.floor(tries/10.)+1) 01268 success = self.place_object_override(whicharm, rotated_pose, z_dist) 01269 if success: 01270 #record that something was placed there 01271 self.place_grid[i][j] = 1 01272 break 01273 else: 01274 rospy.loginfo("Place object override failed!") 01275 if success: 01276 break 01277 01278 01279 #if we managed to put it down, record that the hand is empty 01280 if success: 01281 rospy.loginfo("removing object from held_objects for the %s hand"%self.arm_dict[whicharm]) 01282 self.held_objects[whicharm] = None 01283 self.reset_place_grid_temporary_collisions() 01284 01285 #move the arm back out of the way 01286 if move_to_side == True: 01287 self.move_arm_to_side(whicharm) 01288 return 1 01289 01290 else: 01291 rospy.loginfo("place failed") 01292 return 0 01293 01294 01295 ##grasp the object and check if there's something in the hand 01296 def grasp_object_and_check_success(self, object, whicharm = None): 01297 01298 if not self.check_arms_to_use(whicharm): 01299 return("error", None) 01300 01301 #try picking it up 01302 (grasp_result, arm_used, executed_grasp) = self.grasp_object(object, whicharm, 1, \ 01303 use_slip_detection = self.use_slip_detection) 01304 01305 self.check_preempted() 01306 01307 #if succeeded, record that the arm has an object 01308 if grasp_result == "SUCCESS": 01309 01310 #check if there's an object in the hand 01311 if self.check_grasp_successful(arm_used): 01312 01313 self.held_objects[arm_used] = object 01314 object.grasp_pose = executed_grasp.grasp_pose 01315 object.grasp = executed_grasp 01316 rospy.loginfo("grasp succeeded, adding object to held_objects for the %s arm and moving it to the side"%self.arm_dict[arm_used]) 01317 self.move_arm_to_side(arm_used, try_constrained = 1) 01318 01319 return ("succeeded", arm_used) 01320 01321 #if failed and we actually tried to grasp, detach the object and return that the attempt failed 01322 else: 01323 self.detach_object(arm_used) 01324 rospy.loginfo("grasp attempt failed and probably moved the object, detaching the object and returning") 01325 self.move_arm_to_side(arm_used, try_constrained = 1) 01326 return ("attempt failed", None) 01327 01328 #object grasper failed 01329 elif grasp_result == "FAILED": 01330 01331 #open all the way 01332 self.open_gripper(arm_used) 01333 01334 #move up to make sure we're not in collision, then move to the side 01335 up_goal = self.return_current_pose_as_list(arm_used) 01336 up_goal[2] += .1 01337 self.move_cartesian_step(arm_used, up_goal, blocking = 1) 01338 self.move_arm_to_side(arm_used) 01339 return ("attempt failed", None) 01340 01341 #grasp succeeded, but lift failed, which we can do instead 01342 elif grasp_result == "LIFT_FAILED": 01343 01344 if self.check_grasp_successful(arm_used): 01345 rospy.loginfo("just lift failed; lifting with Cartesian controllers") 01346 self.held_objects[arm_used] = object 01347 object.grasp_pose = executed_grasp.grasp_pose 01348 object.grasp = executed_grasp 01349 up_goal = self.return_current_pose_as_list(arm_used) 01350 up_goal[2] += .1 01351 self.move_cartesian_step(arm_used, up_goal, blocking = 1) 01352 01353 #still something in the gripper after lifting, so we succeeded 01354 if self.check_grasp_successful(arm_used): 01355 rospy.loginfo("grasp succeeded, adding object to held_objects for the %s arm"%self.arm_dict[arm_used]) 01356 self.move_arm_to_side(arm_used, try_constrained = 1) 01357 return ("succeeded", arm_used) 01358 01359 #dropped whatever it was, give up 01360 self.detach_object(arm_used) 01361 self.open_gripper(arm_used) 01362 self.move_arm_to_side(arm_used) 01363 rospy.loginfo("lift attempt failed, detaching object and returning to the side") 01364 return ("attempt failed", None) 01365 01366 #infeasible grasp, but nothing was disturbed 01367 elif grasp_result == "UNFEASIBLE": 01368 return ("no feasible grasp", None) 01369 01370 #start state was in collision even after trying to add allowed contact regions, better take a new map 01371 elif grasp_result == "MOVE_ARM_STUCK": 01372 rospy.loginfo("grasp object returned move arm stuck; resetting map objects and moving to side") 01373 self.reset_collision_map() 01374 self.move_arm_to_side(arm_used) 01375 return ("attempt failed", None) 01376 01377 #grasper returned "ERROR", better ask a human for help 01378 else: 01379 self.throw_exception() 01380 return ("error", None) 01381 01382 01383 ##refine an object detection by pointing the head at it (input the object's position on the table) 01384 def refine_object_detection(self, objectx, objecty): 01385 rospy.loginfo("refining object detection") 01386 self.point_head([objectx, objecty, self.table_height], 'base_link') 01387 (objects, table) = self.call_tabletop_detection(update_table = 0) 01388 01389 #find the object closest to the original location and return it 01390 closest_object = None 01391 closest_object_dist = 1e6 01392 for new_object in objects: 01393 xdist = new_object.pose.pose.position.x-objectx 01394 ydist = new_object.pose.pose.position.y-objecty 01395 if (xdist**2+ydist**2) < closest_object_dist: 01396 closest_object_dist = xdist**2+ydist**2 01397 closest_object = new_object 01398 01399 return closest_object 01400 01401 01402 ##distance between the stereo camera's center ray and a pose_stamped's center 01403 def pose_to_narrow_stereo_center_dist(self, pose_stamped): 01404 01405 #convert the pose to the stereo camera frame 01406 transformed_pose = change_pose_stamped_frame(self.tf_listener, pose_stamped, self.stereo_camera_frame) 01407 01408 #find the distance of the pose from the z-axis (dist in x-y plane) 01409 pose_dist = math.sqrt(transformed_pose.pose.position.x**2 + transformed_pose.pose.position.y**2) 01410 01411 return pose_dist 01412 01413 01414 ##refine the view of the object, if it's too far from the center of the narrow stereo center 01415 def refine_if_not_centered(self, object): 01416 01417 #if the object is too far away from where the head is pointing, point the head at it and re-detect first 01418 dist_from_point = self.pose_to_narrow_stereo_center_dist(object.pose) 01419 object_to_grasp = object 01420 rospy.loginfo("dist_from_point:%5.3f"%dist_from_point) 01421 if dist_from_point > .1: 01422 refined_object = self.refine_object_detection(object.pose.pose.position.x, object.pose.pose.position.y) 01423 if refined_object != None: 01424 object_to_grasp = refined_object 01425 rospy.loginfo("refined object's collision name: %s"%refined_object.collision_name) 01426 else: 01427 rospy.logerr("refining object detection resulted in no object detections! Adding old object back to collision map") 01428 self.collision_map_interface.add_collision_box(object.pose.pose, object.box_dims, object.pose.header.frame_id, object.collision_name) 01429 return object_to_grasp 01430 01431 01432 ##pick up the object_num-th nearest feasible object (tries each one in turn) 01433 #arms_to_try indicates which arms to try in what order (0=right, 1=left) 01434 def pick_up_nearest_object(self, object_num = 0, arms_to_try = [0,1]): 01435 01436 #objects are already sorted by distance to edge of table 01437 for object in self.detected_objects[object_num:]: 01438 01439 object_to_grasp = self.refine_if_not_centered(object) 01440 01441 #try one arm and then the other 01442 for whicharm in arms_to_try: 01443 01444 self.check_preempted() 01445 01446 (result, arm_used) = self.grasp_object_and_check_success(object_to_grasp, whicharm) 01447 01448 self.check_preempted() 01449 01450 if result == "succeeded": 01451 return (1, arm_used) 01452 elif result == "attempt failed": 01453 return (0, None) 01454 01455 #if we didn't succeed but didn't try to grasp, just continue 01456 elif result == "no feasible grasp": 01457 rospy.loginfo("no feasible grasp for this object with the %s arm, trying the next"%self.arm_dict[whicharm]) 01458 01459 #didn't manage to pick up any object 01460 rospy.loginfo("no feasible grasps found for any object") 01461 return (0, None) 01462 01463 01464 ##open the gripper (whicharm is 0 for right, 1 for left) 01465 def open_gripper(self, whicharm): 01466 self.cms[whicharm].command_gripper(.1, -1, 1) 01467 01468 01469 ##close the gripper (whicharm is 0 for right, 1 for left) 01470 def close_gripper(self, whicharm): 01471 self.cms[whicharm].command_gripper(0, 100, 1) 01472 01473 01474 ##reset the collision map and take a new one (without tabletop or objects) 01475 def reset_collision_map(self): 01476 self.collision_map_interface.reset_collision_map() 01477 01478 01479 ##try to move to a set of joint angles using move_arm, ignoring current collisions if necessary, and if that fails, 01480 #move open-loop 01481 def try_hard_to_move_joint(self, whicharm, trajectory, max_tries = 5, use_open_loop = 1): 01482 01483 if not self.check_arms_to_use(whicharm): 01484 return 0 01485 01486 reset_collision_map = 0 01487 for tries in range(max_tries): 01488 rospy.loginfo("try_hard_to_move_joint try number: %d"%tries) 01489 error_code = self.cms[whicharm].move_arm_joint(trajectory[-1], blocking = 1) 01490 01491 if error_code == 1: 01492 rospy.loginfo("move arm reported success") 01493 return 1 01494 elif error_code == 0: 01495 if not reset_collision_map: 01496 rospy.loginfo("problem with collision map! Resetting collision objects") 01497 self.reset_collision_map() 01498 reset_collision_map = 1 01499 else: 01500 rospy.loginfo("resetting collision map didn't work, move arm is still stuck!") 01501 break 01502 rospy.loginfo("move arm reports failure with error code %d"%error_code) 01503 01504 if use_open_loop: 01505 rospy.loginfo("moving open-loop to desired joint angles") 01506 self.cms[whicharm].command_joint_trajectory(trajectory, blocking = 1) 01507 return 1 01508 01509 return 0 01510 01511 ##move open-loop through trajectory 01512 def try_hard_to_move_joint_open_loop(self, whicharm, trajectory): 01513 01514 if not self.check_arms_to_use(whicharm): 01515 return 0 01516 01517 rospy.loginfo("moving open-loop to desired joint angles") 01518 self.cms[whicharm].command_joint_trajectory(trajectory, blocking = 1) 01519 return 1 01520 01521 ##try to move to a pose using move_arm, ignoring current collisions if necessary, and if that fails, 01522 #move open-loop (either to the IK solution if there is one, or using the Cartesian controllers if there isn't) 01523 def try_hard_to_move_pose(self, whicharm, pose_stamped, max_tries = 3, use_joint_open_loop = 0, use_cartesian = 0, try_constrained = 0): 01524 01525 if not self.check_arms_to_use(whicharm): 01526 return 0 01527 01528 #set the planning scene 01529 self.collision_map_interface.set_planning_scene() 01530 01531 current_angles = self.cms[whicharm].get_current_arm_angles() 01532 start_angles = current_angles 01533 ik_solution = None 01534 01535 #try max_tries times to make move_arm get us there 01536 for tries in range(max_tries): 01537 rospy.loginfo("try_hard_to_move_pose try number: %d"%tries) 01538 01539 #look for a collision-free IK solution at the goal 01540 (solution, error_code) = self.cms[whicharm].ik_utilities.run_ik(pose_stamped, start_angles, \ 01541 self.cms[whicharm].ik_utilities.link_name) 01542 if solution: 01543 ik_solution = solution 01544 else: 01545 start_angles = self.cms[whicharm].ik_utilities.start_angles_list[tries] 01546 continue 01547 01548 #try to keep the gripper approximately level while moving 01549 if try_constrained: 01550 location = get_xyz(pose_stamped.pose.position) 01551 current_pose = self.cms[whicharm].get_current_wrist_pose_stamped('torso_lift_link') 01552 orientation_constraint = self.get_keep_object_level_constraint(whicharm,current_pose) 01553 constraint = Constraints() 01554 constraint.orientation_constraints.append(orientation_constraint) 01555 result = self.try_to_move_constrained(whicharm,constraint,3,solution,location,pose_stamped.header.frame_id) 01556 #result = self.try_to_move_constrained(whicharm, 3, solution, location) 01557 if result == 1: 01558 return 1 01559 01560 #having found IK goal angles that are not in collision, try hard to get there 01561 success = self.try_hard_to_move_joint(whicharm, [solution,], 2, use_open_loop = 0) 01562 if success: 01563 rospy.loginfo("try_hard_to_move_joint reported success") 01564 return 1 01565 rospy.loginfo("try_hard_to_move_joint reported failure") 01566 start_angles = self.cms[whicharm].ik_utilities.start_angles_list[tries] 01567 01568 #if we found an IK solution, move there open-loop, going to arm_above_and_to_side angles first 01569 if ik_solution and use_joint_open_loop: 01570 rospy.loginfo("ran out of try_hard_to_move_joint tries using move_arm, moving to the joint angles open-loop") 01571 self.cms[whicharm].command_joint_trajectory([self.arm_above_and_to_side_angles[whicharm], ik_solution], blocking = 1) 01572 return 1 01573 01574 #no collision-free ik solution available, either use the Cartesian controllers or just return 01575 if use_cartesian: 01576 rospy.logerr("no IK solution found for goal pose! Using Cartesian controllers to move") 01577 self.cms[whicharm].move_cartesian(pose_stamped, settling_time = rospy.Duration(10)) 01578 01579 return 0 01580 01581 01582 ##move to a nearby Cartesian pose using the Cartesian controllers 01583 def move_cartesian_step(self, whicharm, pose, timeout = 10.0, settling_time = 3.0, blocking = 0): 01584 if type(pose) == list: 01585 pose = create_pose_stamped(pose, 'base_link') 01586 self.cms[whicharm].move_cartesian(pose, blocking = blocking, \ 01587 pos_thres = .003, rot_thres = .05, \ 01588 timeout = rospy.Duration(timeout), \ 01589 settling_time = rospy.Duration(settling_time)) 01590 01591 01592 ##is an object in the hand? (is the gripper not closed all the way?) 01593 def check_grasp_successful(self, whicharm, min_gripper_opening = .0021, max_gripper_opening = .1): 01594 gripper_opening = self.cms[whicharm].get_current_gripper_opening() 01595 if gripper_opening > min_gripper_opening and gripper_opening < max_gripper_opening: 01596 rospy.loginfo("gripper opening acceptable: %5.3f"%gripper_opening) 01597 return 1 01598 rospy.loginfo("gripper opening unacceptable: %5.3f"%gripper_opening) 01599 return 0 01600 01601 01602 ##detect objects and try to pick up the nearest one 01603 def detect_and_pick_up_object(self, point_head_loc, frame = 'base_link', arms_to_try = [0,1], constrained = 0): 01604 01605 #point the head at the desired location 01606 self.point_head(point_head_loc, frame) 01607 01608 self.check_preempted() 01609 01610 #detect objects and clear out extra collision points 01611 self.call_tabletop_detection(update_table = 0, clear_attached_objects = 1) 01612 01613 #no objects left! Quit 01614 if self.count_objects() == 0: 01615 rospy.loginfo("no objects in view!") 01616 return ("no objects left", None) 01617 01618 self.check_preempted() 01619 01620 #try to pick up the nearest object 01621 (success, arm_used) = self.pick_up_nearest_object(arms_to_try = arms_to_try) 01622 01623 #succeeded 01624 if success: 01625 return ("succeeded", arm_used) 01626 else: 01627 return ("grasp failed", None) 01628 01629 01630 ##how many relevant objects did we detect? 01631 def count_objects(self): 01632 return len(self.detected_objects) 01633 01634 01635 ##try to move to a location while constraining the gripper to remain approximately level 01636 def try_to_move_constrained(self, whicharm, constraint, max_tries = 3, start_angles = None, location = None, frame_id = 'torso_lift_link'): 01637 reset_collision_map = 0 01638 for tries in range(max_tries): 01639 rospy.loginfo("constrained move try number: %d"%tries) 01640 error_code = self.cms[whicharm].move_arm_constrained(constraint, start_angles, location, frame_id) 01641 if error_code == 1: 01642 rospy.loginfo("move arm constrained reported success") 01643 return 1 01644 rospy.loginfo("move arm constrained reports failure with error code %d"%error_code) 01645 return 0 01646 01647 01648 ##move whicharm off to the side 01649 def move_arm_to_side(self, whicharm, try_constrained = 0): 01650 01651 #check if arms are already there 01652 current_arm_angles = self.cms[whicharm].get_current_arm_angles() 01653 desired_arm_angles = self.cms[whicharm].normalize_trajectory([self.arm_to_side_angles[whicharm]])[0] 01654 for (current_angle, desired_angle) in zip(current_arm_angles, desired_arm_angles): 01655 if math.fabs(current_angle-desired_angle) > 0.05: 01656 break 01657 else: 01658 rospy.loginfo("arm is already at the side") 01659 return 1 01660 01661 #trying to keep the object orientation the same as current orientation 01662 if try_constrained: 01663 #default search-starting arm angles are arm-to-the-side 01664 if whicharm == 1: 01665 start_angles = [2.135, 0.803, 1.732, -1.905, 2.369, -1.680, 1.398] 01666 else: 01667 start_angles = [-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398] 01668 01669 #default location is arm-to-the-side 01670 if whicharm == 1: 01671 rospy.loginfo("Planning for the left arm") 01672 location = [0.05, 0.65, -0.05] 01673 else: 01674 rospy.loginfo("Planning for the right arm") 01675 location = [0.05, -0.65, -0.05] 01676 01677 current_pose = self.cms[whicharm].get_current_wrist_pose_stamped('torso_lift_link') 01678 orientation_constraint = self.get_keep_object_level_constraint(whicharm,current_pose) 01679 constraint = Constraints() 01680 constraint.orientation_constraints.append(orientation_constraint) 01681 result = self.try_to_move_constrained(whicharm,constraint,3,start_angles,location,'torso_lift_link') 01682 if result == 1: 01683 return 1 01684 01685 #either constrained move didn't work or we didn't request a constrained move 01686 result = self.try_hard_to_move_joint(whicharm, [self.arm_above_and_to_side_angles[whicharm],self.arm_to_side_angles[whicharm]], use_open_loop = 1) 01687 return result 01688 01689 01690 ## create an OrientationConstraint to keep the object level 01691 def get_keep_object_level_constraint(self, whicharm, current_pose): 01692 orientation_constraint = OrientationConstraint() 01693 orientation_constraint.header = current_pose.header 01694 orientation_constraint.orientation = current_pose.pose.orientation 01695 orientation_constraint.type = OrientationConstraint.HEADER_FRAME 01696 orientation_constraint.link_name = self.cms[whicharm].ik_utilities.link_name 01697 orientation_constraint.absolute_roll_tolerance = 0.2 01698 orientation_constraint.absolute_pitch_tolerance = 0.2 01699 orientation_constraint.absolute_yaw_tolerance = math.pi 01700 orientation_constraint.weight = 1.0 01701 01702 [roll,pitch,yaw] = posemath.fromMsg(current_pose.pose).M.GetRPY() 01703 if math.fabs(pitch - math.pi/2.0) < 0.3: 01704 orientation_constraint.absolute_roll_tolerance = math.pi 01705 rospy.loginfo("roll is unconstrained") 01706 return orientation_constraint 01707 01708 01709 ##move whicharm off to the side open-loop 01710 def move_arm_to_side_open_loop(self, whicharm, try_constrained = 0): 01711 01712 result = self.try_hard_to_move_joint_open_loop(whicharm, [self.arm_above_and_to_side_angles[whicharm], \ 01713 self.arm_to_side_angles[whicharm]]) 01714 return result 01715 01716 01717 ##pick up the object nearest to a PointStamped with whicharm (0=right, 1=left) 01718 def pick_up_object_near_point(self, point_stamped, whicharm, refine_view = True): 01719 01720 if not self.check_arms_to_use(whicharm): 01721 return 0 01722 01723 #convert point to base_link frame 01724 base_link_point = point_stamped_to_list(self.tf_listener, point_stamped, 'base_link') 01725 01726 #find the closest object 01727 nearest_dist = 1e6 01728 nearest_object_ind = None 01729 for (ind, object) in enumerate(self.detected_objects): 01730 (object_point, object_rot) = pose_stamped_to_lists(self.tf_listener, object.pose, 'base_link') 01731 dist = sum([(x-y)**2 for (x,y) in list(zip(object_point, base_link_point))])**.5 01732 rospy.loginfo("dist: %0.3f"%dist) 01733 if dist < nearest_dist: 01734 nearest_dist = dist 01735 nearest_object_ind = ind 01736 01737 if nearest_object_ind != None: 01738 rospy.loginfo("nearest object ind: %d"%nearest_object_ind) 01739 else: 01740 rospy.logerr('No nearby objects. Unable to select grasp target') 01741 return 0 01742 01743 #refine the view of the object if it's too far from the narrow stereo center 01744 if refine_view: 01745 object_to_grasp = self.refine_if_not_centered(self.detected_objects[nearest_object_ind]) 01746 else: 01747 object_to_grasp = self.detected_objects[nearest_object_ind] 01748 01749 #try to pick it up with the requested arm 01750 (result, arm_used) = self.grasp_object_and_check_success(object_to_grasp, whicharm) 01751 01752 if result == "succeeded": 01753 return 1 01754 elif result == "attempt failed": 01755 return 0 01756 01757 #if we didn't succeed but didn't try to grasp, just continue 01758 elif result == "no feasible grasp": 01759 rospy.loginfo("no feasible grasp for this object with the %s arm"%self.arm_dict[whicharm]) 01760 return 0 01761 01762 01763 ##print the list of possible objects to grasp 01764 def print_object_list(self): 01765 01766 #draw colors on the clusters and objects so we know which one is which, and print out the list of objects and their colors 01767 colors = [[1,0,0], 01768 [0,1,0], 01769 [0,0,1], 01770 [0,1,1], 01771 [1,0,1], 01772 [1,1,0], 01773 [1, 1, 1], 01774 [.5, .5, .5], 01775 [0, 0, 0]] 01776 colornames = ["red", "green", "blue", "cyan", "magenta", "yellow", "white", "grey", "black"] 01777 01778 self.draw_functions.clear_rviz_points('grasp_markers') 01779 color_ind = 0 01780 for (ind, object) in enumerate(self.detected_objects): 01781 01782 self.draw_object_pose(object, object.pose, colors[color_ind], ind) 01783 if object.type == 'mesh': 01784 rospy.loginfo("object %d, %s: recognized object with id %d and col name %s"%(ind, colornames[color_ind], object.object.potential_models[0].model_id, object.collision_name)) 01785 else: 01786 rospy.loginfo("object %d, %s: point cluster with %d points and col name %s"%(ind, colornames[color_ind], len(object.object.cluster.points), object.collision_name)) 01787 # points = self.point_cloud_to_mat(cluster) 01788 # self.draw_functions.draw_rviz_points(points, color = colors[color_ind], duration = 0., id = ind) 01789 # time.sleep(.05) 01790 01791 color_ind = (color_ind + 1) % len(colors) 01792 01793 01794 ##check for a preempt request (overload this for your application) 01795 def check_preempted(self): 01796 pass 01797 01798 01799 ##saw a serious error (problem with service call), overload this for your application 01800 def throw_exception(self): 01801 pass 01802 01803 01804 ##pause for input 01805 def keypause(self): 01806 print "press enter to continue" 01807 input = raw_input() 01808 return input 01809 01810 01811 ##get the number of an object to pick up from the user 01812 def input_object_num(self): 01813 01814 if not self.detected_objects: 01815 print "no objects detected!" 01816 return -1 01817 01818 print "which object number?" 01819 input = self.keypause() 01820 try: 01821 object_ind = int(input) 01822 except: 01823 print "bad input: ", input 01824 return -1 01825 01826 if object_ind > len(self.detected_objects): 01827 print "object number too high" 01828 return -1 01829 01830 return object_ind 01831 01832 01833 ##get which arm/side to use from the user 01834 def input_side(self): 01835 c = raw_input() 01836 if c != 'r' and c != 'l': 01837 print "bad input: %s"%c 01838 return -1 01839 if c == 'r': 01840 whicharm = 0 01841 print "using right side" 01842 elif c == 'l': 01843 whicharm = 1 01844 print "using left side" 01845 return whicharm 01846 01847 01848 ##return the current pos and rot of the wrist for whicharm as a 7-list (pos, quaternion rot) 01849 def return_current_pose_as_list(self, whicharm): 01850 (pos, rot) = self.cms[whicharm].return_cartesian_pose() 01851 return pos+rot 01852 01853 01854 ##print instructions for using extensions to the keyboard interface here 01855 def print_keyboard_extensions(self): 01856 print "pr, pl, or pc to change the place rectangle to be on the robot's right/left/center" 01857 01858 01859 ##add extensions to the keyboard interface here, return 1 to continue and 0 to go on down the possibilities 01860 def keyboard_extensions(self, input): 01861 if input == 'pr' or input == 'pl' or input == 'pc': 01862 y = 0 01863 if input == 'pr': 01864 y = -.3 01865 elif input == 'pl': 01866 y = .3 01867 01868 self.place_rect_dims = [.3, .3] 01869 place_rect_pose_mat = scipy.matrix([[1.,0.,0.,self.table_front_edge_x+self.place_rect_dims[0]/2.+.1], 01870 [0.,1.,0.,y], 01871 [0.,0.,1.,self.table_height], 01872 [0.,0.,0.,1.]]) 01873 self.place_rect_pose_stamped = stamp_pose(mat_to_pose(place_rect_pose_mat), 'base_link') 01874 self.draw_place_area() 01875 01876 return 0 01877 01878 01879 def pick_side(self): 01880 if self.arms_to_use == "both": 01881 print "which arm? r for right, l for left" 01882 whicharm = self.input_side() 01883 elif self.arms_to_use == "right": 01884 whicharm = 0 01885 elif self.arms_to_use == "left": 01886 whicharm = 1 01887 return whicharm 01888 01889 01890 ##keyboard interface for starting up the demo and overriding with manual commands 01891 def keyboard_interface(self): 01892 01893 #set the current positions of the wrists as the current 'goals' relative to which Cartesian movements are expressed 01894 currentgoals = [0]*2 01895 for arm_ind in self.arms_to_use_list: 01896 currentgoals[arm_ind] = self.return_current_pose_as_list(arm_ind) 01897 01898 #keyboard loop 01899 while not rospy.is_shutdown(): 01900 01901 print "type:" 01902 self.print_keyboard_extensions() 01903 if self.arms_to_use in ["right", "both"]: 01904 print "r to control the right arm" 01905 if self.arms_to_use in ["left", "both"]: 01906 print "l to control the left arm" 01907 print "d to detect objects, da to detect and clear attached objects" 01908 print "dp to detect several possible models for each object, and clear attached objects" 01909 print "p to pick up an object" 01910 print "w to place the object in the place rectangle, wo to place the object where it came from" 01911 print "h to point the head at the current place rectangle and draw it" 01912 print "t to find the table" 01913 print "det to detach the object in the gripper" 01914 print "q to quit" 01915 01916 input = self.keypause() 01917 01918 #extensions to the interface get checked for here 01919 cont = self.keyboard_extensions(input) 01920 if cont: 01921 continue 01922 01923 #detect objects 01924 if input == 'd': 01925 rospy.loginfo("detecting objects") 01926 self.call_tabletop_detection(update_table = 0, clear_attached_objects = 0) 01927 elif input == 'da': 01928 rospy.loginfo("detecting objects and clearing attached objects") 01929 self.call_tabletop_detection(update_table = 0, clear_attached_objects = 1) 01930 elif input == 'dp': 01931 rospy.loginfo("detecting several models for each object") 01932 self.call_tabletop_detection(update_table = 0, clear_attached_objects = 1, num_models = 5) 01933 01934 #pick up an object 01935 elif input == 'p': 01936 01937 self.print_object_list() 01938 object_num = self.input_object_num() 01939 if object_num == -1: 01940 continue 01941 01942 whicharm = self.pick_side() 01943 if whicharm not in [0,1]: 01944 print "invalid arm" 01945 continue 01946 01947 (result, arm_used) = self.grasp_object_and_check_success(self.detected_objects[object_num], whicharm) 01948 01949 #put down an object where it was originally picked up 01950 elif input == 'w' or input == 'wo': 01951 01952 #if only one arm is holding the object, that's the one we mean 01953 if not any(self.held_objects): 01954 print "the robot doesn't think it's holding any objects!\nPick an arm to do an open-loop place anyway: r for right and l for left" 01955 if self.held_objects[0] and not self.held_objects[1]: 01956 whicharm = 0 01957 print "only holding an object in the right arm, dropping the right arm object" 01958 elif self.held_objects[1] and not self.held_objects[0]: 01959 whicharm = 1 01960 print "only holding an object in the left arm, dropping the left arm object" 01961 else: 01962 whicharm = self.pick_side() 01963 if whicharm != 0 and whicharm != 1: 01964 continue 01965 01966 if input == 'wo': 01967 if not self.held_objects[whicharm]: 01968 print "no recorded pose for an object in that gripper!" 01969 continue 01970 self.place_object(whicharm, self.held_objects[whicharm].pose) 01971 else: 01972 self.put_down_object(whicharm, use_place_override = 1) 01973 01974 #point the head at the current place location 01975 elif input == 'h': 01976 rospy.loginfo("pointing the head at the current place location and drawing the place area") 01977 self.point_head_at_place_rect() 01978 self.draw_place_area() 01979 01980 #find the table 01981 elif input == 't': 01982 rospy.loginfo("finding the table") 01983 self.find_table() 01984 01985 #move an arm 01986 elif input == 'r' or input == 'l': 01987 if input == 'r': 01988 if self.arms_to_use not in ["right", "both"]: 01989 rospy.loginfo("right arm not allowed") 01990 continue 01991 whicharm = 0 01992 else: 01993 if self.arms_to_use not in ["left", "both"]: 01994 rospy.loginfo("left arm not allowed") 01995 continue 01996 whicharm = 1 01997 01998 while not rospy.is_shutdown(): 01999 print "c to close, o to open the gripper" 02000 print "u to go up, d to go down, rt to go right, lt to go left" 02001 print "ay to go away from the robot, td to go toward the robot" 02002 print "jm to go to arm-away-on-the-side angles using move_arm" 02003 print "jc to go to arm-away-on-the-side angles while trying to constrain the pose" 02004 print "j to go to arm-away-on-the-side angles open-loop using the joint trajectory action" 02005 print "e to exit the arm-control menu for this arm (%s)"%input 02006 c = self.keypause() 02007 02008 if c == 'c': 02009 self.close_gripper(whicharm) 02010 elif c == 'o': 02011 self.open_gripper(whicharm) 02012 if self.held_objects[whicharm]: 02013 self.detach_object(whicharm) 02014 self.held_objects[whicharm] = 0 02015 02016 elif c == 'jm': 02017 #try moving collision-free using move_arm 02018 self.move_arm_to_side(whicharm) 02019 currentgoals[whicharm] = self.return_current_pose_as_list(whicharm) 02020 02021 elif c == 'jc': 02022 #try moving collision-free using move_arm, keeping the pose constrained 02023 self.move_arm_to_side(whicharm, try_constrained = 1) 02024 currentgoals[whicharm] = self.return_current_pose_as_list(whicharm) 02025 02026 elif c == 'j': 02027 #just move the arm open-loop 02028 self.cms[whicharm].command_joint_trajectory([self.arm_above_and_to_side_angles[whicharm], self.arm_to_side_angles[whicharm]], blocking = 1) 02029 currentgoals[whicharm] = self.return_current_pose_as_list(whicharm) 02030 02031 elif c == 'j2': 02032 self.cms[whicharm].command_joint_trajectory([self.arm_to_side_angles[whicharm],], blocking = 1) 02033 currentgoals[whicharm] = self.return_current_pose_as_list(whicharm) 02034 02035 elif c == 'u': 02036 print "going up" 02037 currentgoals[whicharm][2] += .1 02038 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02039 elif c == 'us': 02040 print "going up a small amount" 02041 currentgoals[whicharm][2] += .02 02042 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02043 elif c == 'd': 02044 print "going down" 02045 currentgoals[whicharm][2] -= .1 02046 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02047 elif c == 'ds': 02048 print "going down a small amount" 02049 currentgoals[whicharm][2] -= .02 02050 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02051 elif c == 'rt': 02052 print "moving right" 02053 currentgoals[whicharm][1] -= .02 02054 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02055 elif c == 'lt': 02056 print "moving left" 02057 currentgoals[whicharm][1] += .02 02058 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02059 elif c == 'ay': 02060 print "moving away from robot" 02061 currentgoals[whicharm][0] += .02 02062 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02063 elif c == 'td': 02064 print "moving toward the robot" 02065 currentgoals[whicharm][0] -= .02 02066 self.move_cartesian_step(whicharm, currentgoals[whicharm], blocking = 1, timeout = 5.0) 02067 02068 elif c == 'e': 02069 break 02070 02071 elif input == 'det': 02072 print "detach the object in which gripper? r for right and l for left" 02073 whicharm = self.input_side() 02074 if whicharm != None: 02075 self.detach_object(whicharm) 02076 02077 elif input == 'q': 02078 return 02079 02080 02081 #update the current arm goals after each action 02082 for arm_ind in self.arms_to_use_list: 02083 currentgoals[arm_ind] = self.return_current_pose_as_list(arm_ind) 02084 02085 02086 if __name__ == '__main__': 02087 02088 rospy.init_node('pick_and_place_manager', anonymous=True) 02089 pick_and_place_manager = PickAndPlaceManager() 02090 pick_and_place_manager.keyboard_interface()