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