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