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, 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
00066 class ObjectInfo():
00067 def __init__(self, object, box_dims, pose, tf_listener, height_above_table, collision_name):
00068 self.object = object
00069 self.box_dims = box_dims
00070 self.pose = pose
00071 self.grasp = None
00072 self.grasp_pose = None
00073 self.grasp_type = 'normal'
00074 self.collision_name = collision_name
00075
00076
00077
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
00082 else:
00083 self.cluster_origin_to_bounding_box = None
00084
00085
00086 self.height_above_table = height_above_table
00087
00088
00089
00090 class PickAndPlaceManager():
00091
00092
00093 def __init__(self, use_slip_detection = 0):
00094
00095
00096 self.use_slip_detection = use_slip_detection
00097
00098
00099 self.grasper_grasp_name = 'object_manipulator/object_manipulator_pickup'
00100 self.grasper_place_name = 'object_manipulator/object_manipulator_place'
00101
00102
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
00113 self.grasper_detect_name = 'object_detection'
00114 self.collision_map_processing_name = '/tabletop_collision_map_processing/tabletop_collision_map_processing'
00115
00116 self.find_bounding_box_name = '/find_cluster_bounding_box'
00117
00118
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
00126 self.tf_listener = tf.TransformListener()
00127
00128
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
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
00143
00144
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
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
00154 self.bounding_box_srv = rospy.ServiceProxy(self.find_bounding_box_name, FindClusterBoundingBox)
00155
00156
00157 self.collision_map_interface = collision_map_interface.CollisionMapInterface()
00158
00159
00160 self.draw_functions = draw_functions.DrawFunctions('grasp_markers')
00161
00162
00163 self.held_objects = [None]*2
00164
00165
00166 self.original_poses = [None]*2
00167
00168
00169 self.table_front_edge_x = .33
00170
00171 self.table_height = .885
00172
00173
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
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
00186 self.detected_table = None
00187 self.additional_tables = []
00188 self.detected_objects = []
00189
00190
00191 self.arm_dict = {0:'r', 1:'l'}
00192
00193
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
00200
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
00207
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
00214 self.collision_support_surface_name = "table"
00215
00216
00217 self.place_override_approach_offset = [-.15, 0, 0]
00218
00219
00220
00221 def shift_place_pose_height(self, object, place_pose, table, z_offset):
00222
00223
00224 new_height = self.find_height_above_table(place_pose, table)
00225
00226
00227
00228 height_diff = object.height_above_table +z_offset - new_height
00229
00230
00231 if height_diff <= 0:
00232 height_diff = 0
00233
00234
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
00241 def find_height_above_table(self, pose, table):
00242
00243
00244 (pos, rot) = pose_stamped_to_lists(self.tf_listener, pose, table.pose.header.frame_id)
00245
00246
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
00252
00253 return pos_in_table_frame[2,0]
00254
00255
00256
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
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
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
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
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
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
00330 if update_table:
00331 self.detected_table = table
00332 self.update_table_info(update_place_rectangle)
00333
00334
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
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
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
00361 heights_above_table.append(self.find_height_above_table(pose_stamped, table))
00362
00363
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
00375 self.print_object_list()
00376
00377 return (detected_objects, table)
00378
00379
00380
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
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
00395 self.check_joint_controllers(whicharm)
00396
00397
00398 self.open_gripper(whicharm)
00399
00400
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
00407 goal = PickupGoal()
00408
00409
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
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
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
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
00452
00453 def point_to_box_dist(self, point, box_pose, box_dims):
00454
00455
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
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
00474 def fill_in_taken_place_grid_spots(self, buffer = .05):
00475
00476
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
00483 point = self.place_grid_location(i, j)
00484
00485
00486 for (ind, object) in enumerate(self.detected_objects):
00487 dist = self.point_to_box_dist(point, object.pose, object.box_dims)
00488
00489
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
00494
00495
00496
00497
00498
00499 def place_object(self, whicharm, pose, padding = .05):
00500 rospy.loginfo("attempting to place object")
00501
00502
00503
00504
00505 if self.held_objects[whicharm].object.type == 1:
00506 msg_pose = pose
00507
00508
00509
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
00516
00517
00518
00519
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
00545 goal.use_reactive_place = self.use_slip_detection
00546
00547
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
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
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
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
00579 def place_object_override(self, whicharm, pose, z_dist = .1, use_joint_open_loop = 0):
00580 rospy.loginfo("last-resort place")
00581
00582
00583 if self.held_objects[whicharm] and not self.held_objects[whicharm].grasp_type == 'flat':
00584
00585
00586 if self.held_objects[whicharm].object.type == 1:
00587
00588
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
00594 else:
00595
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
00604 self.draw_functions.draw_rviz_axes(wrist_to_base_link_mat, 'base_link', id = 0, duration = 60.)
00605
00606
00607
00608
00609 else:
00610
00611 if self.held_objects[whicharm] and self.held_objects[whicharm].grasp_type == 'flat':
00612
00613
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
00625
00626
00627
00628
00629
00630
00631
00632
00633 front_above_pose = copy.deepcopy(wrist_pose)
00634
00635
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
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
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
00653 self.cms[whicharm].move_cartesian(wrist_pose, settling_time = rospy.Duration(10))
00654
00655
00656 self.open_gripper(whicharm)
00657
00658
00659 if self.held_objects[whicharm]:
00660 self.detach_and_add_back_object(whicharm)
00661 else:
00662 self.detach_object(whicharm)
00663
00664
00665 wrist_pose.pose.position.z += .2
00666 self.cms[whicharm].move_cartesian(wrist_pose, settling_time = rospy.Duration(10))
00667
00668
00669 self.move_arm_to_side(whicharm)
00670 return 1
00671
00672
00673
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
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
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
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
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
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
00729 time.sleep(1.0)
00730 return 1
00731
00732
00733
00734
00735
00736 def update_table_info(self, adjust_place_rectangle = 0):
00737
00738
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
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
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
00769 self.set_place_area(self.place_rect_pose_stamped, self.place_rect_dims)
00770
00771
00772
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
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
00791 def draw_place_area(self):
00792
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
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
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.))
00815
00816
00817 transformed_rect_pose = self.transform_place_rect_pose()
00818
00819
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
00827 def place_grid_pose(self, xind, yind, whicharm, add_noise = 0):
00828
00829
00830 location = self.place_grid_location(xind, yind)
00831 rospy.loginfo("proposed place location: %5.3f, %5.3f"%(location[0], location[1]))
00832
00833
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
00838 place_pose.header.stamp = rospy.Time(self.held_objects[whicharm].pose.header.stamp.secs)
00839
00840
00841 else:
00842 place_pose = self.cms[whicharm].get_current_wrist_pose_stamped('base_link')
00843
00844
00845 transformed_rect_pose = self.transform_place_rect_pose()
00846 place_pose.pose.position.z = transformed_rect_pose.position.z
00847
00848
00849 place_pose.pose.position.x = location[0]
00850 place_pose.pose.position.y = location[1]
00851
00852
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
00859 self.draw_object_pose(self.held_objects[whicharm], place_pose, [0,1,1], 100)
00860
00861 return place_pose
00862
00863
00864
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
00876
00877 def choose_place_location(self, whicharm, override_pose = -1, z_offset = 0):
00878
00879
00880 i = j = 0
00881
00882
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
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
00893
00894
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
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
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
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
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
00944
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
00957 def transfer_object_to_other_hand(self, holding_arm, object):
00958
00959
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
00979 if result == "SUCCESS":
00980 rospy.loginfo("transfer reported success, updating the grasp info")
00981
00982
00983 self.held_objects[(1-holding_arm)] = self.held_objects[holding_arm]
00984 self.held_objects[holding_arm] = 0
00985
00986
00987 self.held_objects[(1-holding_arm)].grasp_pose = res.grasp_pose
00988
00989
00990 self.move_arm_to_side(holding_arm)
00991 self.move_arm_to_side(1-holding_arm)
00992
00993
00994 if result == "SUCCESS":
00995 return 1
00996 return 0
00997
00998
00999
01000 def point_head_at_place_rect(self, offset = 1):
01001
01002 rospy.loginfo("pointing the head at the place rectangle")
01003
01004
01005 transformed_rect_pose = self.transform_place_rect_pose()
01006
01007
01008 (x,y,z) = get_xyz(transformed_rect_pose.position)
01009
01010
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
01018 def put_down_object(self, whicharm, max_place_tries = None, use_place_override = 0, move_to_side = True):
01019
01020 padding = .05
01021
01022
01023 self.check_joint_controllers(whicharm)
01024
01025
01026 if not self.check_grasp_successful(whicharm):
01027 rospy.loginfo("no object in gripper! Stopping put-down")
01028
01029
01030 self.held_objects[whicharm] = 0
01031 self.detach_object(whicharm)
01032 return 1
01033
01034 self.check_preempted()
01035
01036
01037 self.point_head_at_place_rect()
01038
01039 self.check_preempted()
01040
01041
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
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
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
01059 else:
01060 place_tries_left = max_place_tries
01061
01062
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
01073 if not self.check_grasp_successful(whicharm):
01074 rospy.loginfo("no object in gripper! Stopping put-down")
01075
01076
01077 self.held_objects[whicharm] = 0
01078 self.detach_object(whicharm)
01079 return 1
01080
01081
01082 (i, j, pose, full) = self.choose_place_location(whicharm, -1, z_offset)
01083
01084
01085 if full == 1:
01086 if z_offset >= max_z_offset:
01087 break
01088
01089
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
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
01105 if table:
01106 self.shift_place_pose_height(self.held_objects[whicharm], pose, table, z_offset)
01107
01108 self.check_preempted()
01109
01110
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
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
01155 if success:
01156 break
01157
01158
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
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
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
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
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
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
01209 def grasp_object_and_check_success(self, object, whicharm = None):
01210
01211
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
01218 if grasp_result == "SUCCESS":
01219
01220
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
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
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
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
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
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
01267 elif grasp_result == "UNFEASIBLE":
01268 return ("no feasible grasp", None)
01269
01270
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
01278 else:
01279 self.throw_exception()
01280 return ("error", None)
01281
01282
01283
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
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
01303 def pose_to_narrow_stereo_center_dist(self, pose_stamped):
01304
01305
01306 transformed_pose = change_pose_stamped_frame(self.tf_listener, pose_stamped, 'narrow_stereo_optical_frame')
01307
01308
01309 pose_dist = math.sqrt(transformed_pose.pose.position.x**2 + transformed_pose.pose.position.y**2)
01310
01311 return pose_dist
01312
01313
01314 def refine_if_not_centered(self, object):
01315
01316
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
01330
01331 def pick_up_nearest_object(self, object_num = 0, arms_to_try = [0,1]):
01332
01333
01334 for object in self.detected_objects[object_num:]:
01335
01336 object_to_grasp = self.refine_if_not_centered(object)
01337
01338
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
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
01357 rospy.loginfo("no feasible grasps found for any object")
01358 return (0, None)
01359
01360
01361
01362 def open_gripper(self, whicharm):
01363 self.cms[whicharm].command_gripper(.1, -1, 1)
01364
01365
01366
01367 def close_gripper(self, whicharm):
01368 self.cms[whicharm].command_gripper(0, 100, 1)
01369
01370
01371
01372 def reset_collision_map(self):
01373 self.collision_map_interface.reset_collision_map()
01374
01375
01376
01377 def take_static_map(self):
01378 self.collision_map_interface.take_static_map()
01379
01380
01381
01382
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
01411
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
01418
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
01425 for tries in range(max_tries):
01426 rospy.loginfo("try_hard_to_move_pose try number: %d"%tries)
01427
01428
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
01438
01439
01440
01441
01442
01443
01444
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
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
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
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
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
01487 def detect_and_pick_up_object(self, point_head_loc, frame = 'base_link', arms_to_try = [0,1]):
01488
01489
01490 self.point_head(point_head_loc, frame)
01491
01492 self.check_preempted()
01493
01494
01495 self.call_tabletop_detection(take_static_collision_map = 1, update_table = 0, clear_attached_objects = 1)
01496
01497
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
01505 (success, arm_used) = self.pick_up_nearest_object(arms_to_try = arms_to_try)
01506
01507
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
01516 def count_objects(self):
01517 return len(self.detected_objects)
01518
01519
01520
01521
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
01535 def move_arm_to_side(self, whicharm, try_constrained = 0):
01536
01537
01538
01539
01540
01541
01542
01543
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
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
01558 def pick_up_object_near_point(self, point_stamped, whicharm):
01559
01560
01561 base_link_point = point_stamped_to_list(self.tf_listener, point_stamped, 'base_link')
01562
01563
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
01581 object_to_grasp = self.refine_if_not_centered(self.detected_objects[nearest_object_ind])
01582
01583
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
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
01598 def print_object_list(self):
01599
01600
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
01622
01623
01624
01625 color_ind = (color_ind + 1) % len(colors)
01626
01627
01628
01629 def check_preempted(self):
01630 pass
01631
01632
01633
01634 def throw_exception(self):
01635 pass
01636
01637
01638
01639 def keypause(self):
01640 print "press enter to continue"
01641 input = raw_input()
01642 return input
01643
01644
01645
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
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
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
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
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
01717 def keyboard_interface(self):
01718
01719
01720 currentgoals = [0]*2
01721 for i in range(2):
01722 currentgoals[i] = self.return_current_pose_as_list(i)
01723
01724
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
01742 cont = self.keyboard_extensions(input)
01743 if cont:
01744 continue
01745
01746
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
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
01772 elif input == 'w' or input == 'wo':
01773
01774
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
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
01802 elif input == 't':
01803 rospy.loginfo("finding the table")
01804 self.find_table()
01805
01806
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
01833 self.move_arm_to_side(whicharm)
01834 currentgoals[whicharm] = self.return_current_pose_as_list(whicharm)
01835
01836 elif c == 'jc':
01837
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
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
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()