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