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