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