diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index d8146e5..8c8ea3f 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -119,103 +119,106 @@ class GraspObjectNode(hm.HelloNode): self.look_at_surface(scan_time_s = 3.0) grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer) - - if grasp_target is not None: - pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer) - - if (self.lift_position is None): - return TriggerResponse( - success=False, - message='lift position unavailable' - ) - - if actually_move: - rospy.loginfo('Raise tool to pregrasp height.') - lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) - lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m) - pose = {'joint_lift': lift_to_pregrasp_m} - self.move_to_pose(pose) + if grasp_target is None: + return TriggerResponse( + success=False, + message='Failed to find grasp target' + ) - pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer) - print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) - print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) + pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer) - if actually_move: - rospy.loginfo('Rotate the gripper for grasping.') - pose = {'joint_wrist_yaw': pregrasp_yaw} - self.move_to_pose(pose) - - rospy.loginfo('Open the gripper.') - pose = {'gripper_aperture': 0.125} - self.move_to_pose(pose) + if (self.lift_position is None): + return TriggerResponse( + success=False, + message='lift position unavailable' + ) - pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer) + if actually_move: + rospy.loginfo('Raise tool to pregrasp height.') + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m) + pose = {'joint_lift': lift_to_pregrasp_m} + self.move_to_pose(pose) + + pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer) + print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) + print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) + + if actually_move: + rospy.loginfo('Rotate the gripper for grasping.') + pose = {'joint_wrist_yaw': pregrasp_yaw} + self.move_to_pose(pose) - print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) - print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m)) - - if actually_move: - rospy.loginfo('Drive to pregrasp location.') - self.drive(pregrasp_mobile_base_m) - - if pregrasp_wrist_extension_m > 0.0: - extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m) - extension_m = min(extension_m, max_extension_m) - rospy.loginfo('Extend tool above surface.') - pose = {'wrist_extension': extension_m} - self.move_to_pose(pose) - else: - print('negative wrist extension for pregrasp, so not extending or retracting.') - - grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) - print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) - - if actually_move: - rospy.loginfo('Move the grasp pose from the pregrasp pose.') - - lift_m = max(self.lift_position + grasp_lift_m, 0.1) - lift_m = min(lift_m, max_lift_m) - - extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m) + rospy.loginfo('Open the gripper.') + pose = {'gripper_aperture': 0.125} + self.move_to_pose(pose) + + pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer) + + print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) + print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m)) + + if actually_move: + rospy.loginfo('Drive to pregrasp location.') + self.drive(pregrasp_mobile_base_m) + + if pregrasp_wrist_extension_m > 0.0: + extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m) extension_m = min(extension_m, max_extension_m) - - pose = {'translate_mobile_base': grasp_mobile_base_m, - 'joint_lift': lift_m, - 'wrist_extension': extension_m} + rospy.loginfo('Extend tool above surface.') + pose = {'wrist_extension': extension_m} self.move_to_pose(pose) + else: + print('negative wrist extension for pregrasp, so not extending or retracting.') - rospy.loginfo('Attempt to close the gripper on the object.') - gripper_aperture_m = grasp_target['width_m'] - 0.18 - pose = {'gripper_aperture': gripper_aperture_m} - self.move_to_pose(pose) - - # Lifting appears to happen before the gripper has - # finished unless there is this sleep. Need to look - # into this issue. - rospy.sleep(3.0) - - rospy.loginfo('Attempt to lift the object.') - object_lift_height_m = 0.1 - - lift_m = max(self.lift_position + object_lift_height_m, 0.2) - lift_m = min(lift_m, max_lift_m) - - pose = {'joint_lift': lift_m} - self.move_to_pose(pose) + grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) + print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) - rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.') - pose = {'gripper_aperture': gripper_aperture_m + 0.005} - self.move_to_pose(pose) + if actually_move: + rospy.loginfo('Move the grasp pose from the pregrasp pose.') + lift_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_m = min(lift_m, max_lift_m) - if actually_move: - rospy.loginfo('Retract the tool.') - pose = {'wrist_extension': 0.01} - self.move_to_pose(pose) + extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m) + extension_m = min(extension_m, max_extension_m) - rospy.loginfo('Reorient the wrist.') - pose = {'joint_wrist_yaw': 0.0} - self.move_to_pose(pose) + pose = {'translate_mobile_base': grasp_mobile_base_m, + 'joint_lift': lift_m, + 'wrist_extension': extension_m} + self.move_to_pose(pose) + + rospy.loginfo('Attempt to close the gripper on the object.') + gripper_aperture_m = grasp_target['width_m'] - 0.18 + pose = {'gripper_aperture': gripper_aperture_m} + self.move_to_pose(pose) + + # Lifting appears to happen before the gripper has + # finished unless there is this sleep. Need to look + # into this issue. + rospy.sleep(3.0) + + rospy.loginfo('Attempt to lift the object.') + object_lift_height_m = 0.1 + + lift_m = max(self.lift_position + object_lift_height_m, 0.2) + lift_m = min(lift_m, max_lift_m) + + pose = {'joint_lift': lift_m} + self.move_to_pose(pose) + + rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.') + pose = {'gripper_aperture': gripper_aperture_m + 0.005} + self.move_to_pose(pose) + + if actually_move: + rospy.loginfo('Retract the tool.') + pose = {'wrist_extension': 0.01} + self.move_to_pose(pose) + + rospy.loginfo('Reorient the wrist.') + pose = {'joint_wrist_yaw': 0.0} + self.move_to_pose(pose) return TriggerResponse( success=True, diff --git a/stretch_demos/nodes/handover_object b/stretch_demos/nodes/handover_object index c681b98..070965e 100755 --- a/stretch_demos/nodes/handover_object +++ b/stretch_demos/nodes/handover_object @@ -163,12 +163,12 @@ class HandoverObjectNode(hm.HelloNode): def trigger_handover_object_callback(self, request): - with self.move_lock: + with self.move_lock: # First, retract the wrist in preparation for handing out an object. pose = {'wrist_extension': 0.005} self.move_to_pose(pose) - if self.handover_goal_ready: + if self.handover_goal_ready: pose = {'joint_lift': self.lift_goal_m} self.move_to_pose(pose) tolerance_distance_m = 0.01 @@ -177,9 +177,14 @@ class HandoverObjectNode(hm.HelloNode): self.move_to_pose(pose) self.handover_goal_ready = False - return TriggerResponse( - success=True, - message='Completed object handover!' + return TriggerResponse( + success=True, + message='Completed object handover!' + ) + else: + return TriggerResponse( + success=False, + message='Failed to find handover goal' ) diff --git a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py index 84005b4..519736c 100755 --- a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py @@ -15,7 +15,7 @@ from stretch_funmap.numba_height_image import numba_create_segment_image_uint8 import hello_helpers.fit_plane as fp -def find_object_to_grasp(height_image, display_on=False): +def find_object_to_grasp(height_image, display_on=False): h_image = height_image.image m_per_unit = height_image.m_per_height_unit m_per_pix = height_image.m_per_pix @@ -251,9 +251,11 @@ def find_closest_flat_surface(height_image, robot_xy_pix, display_on=False): segments_image, segment_info, height_to_segment_id = segment(image, m_per_unit, zero_height, segmentation_scale, verbose=False) if segment_info is None: return None, None - + floor_id, floor_mask = find_floor(segment_info, segments_image, verbose=False) - + if floor_mask is None: + return None, None + remove_floor = True if remove_floor: segments_image[floor_mask > 0] = 0 @@ -1030,15 +1032,9 @@ def find_floor(segment_info, segments_image, verbose=False): height_m = segment_info[i]['height_m'] bin_value = segment_info[i]['bin_value'] - # Old values changed on June 3, 2021 due to no floor segment - # being within the bounds, which resulted in an error. - - #min_floor_m = -0.02 - #max_floor_m = 0.1 - - # New values set on June 3, 2021. - min_floor_m = -0.05 - max_floor_m = 0.05 + # New values set on June 14, 2022. + min_floor_m = -0.1 + max_floor_m = 0.1 if verbose: print('segment = {0}, histogram bin_value = {1}, height_m = {2}, min_floor_m = {3}, max_floor_m = {4}'.format(i, bin_value, height_m, min_floor_m, max_floor_m)) @@ -1061,8 +1057,7 @@ def find_floor(segment_info, segments_image, verbose=False): print('floor_id =', floor_id) print('max_bin_value =', max_bin_value) else: - if verbose: - print('segment_max_height_image.py : no floor segment found...') + print('segment_max_height_image.py : no floor segment found...') return floor_id, floor_mask