diff --git a/stretch_demos/nodes/grasp_point b/stretch_demos/nodes/grasp_point new file mode 100755 index 0000000..a2e7955 --- /dev/null +++ b/stretch_demos/nodes/grasp_point @@ -0,0 +1,268 @@ +#!/usr/bin/env python3 + +import copy +import numpy as np + +import rospy +from geometry_msgs.msg import PointStamped +from sensor_msgs.msg import PointCloud2, JointState +import hello_helpers.hello_misc as hm + +import stretch_pyfunmap.navigate as nv +import stretch_pyfunmap.manipulation_planning as mp +import stretch_pyfunmap.segment_max_height_image as sm + + +class GraspObjectNode(hm.HelloNode): + + def __init__(self): + hm.HelloNode.__init__(self) + self.update_perception = True + self.publish_markers = True + + self.grasp_targets = None + self.is_holding_object = False + self.location_above_surface_m = None + self.placement_target = None + + self.arm_position = None + self.lift_position = None + self.left_finger_position = None + self.yaw_position = None + self.pitch_position = None + self.roll_position = None + + # TODO: investigate a better place for these to exist + self.yaw_offset = 0.0 + self.mb_offset = 0.01 + self.arm_offset = -0.03 + + def drive_base(self, delta_m): + tolerance_distance_m = 0.005 + if delta_m > 0: + at_goal = self.move_base.forward(delta_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m, floor_tf='map') + else: + at_goal = self.move_base.backward(delta_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m, floor_tf='map') + return at_goal + + def grasp_object(self, target): + target['elongated'] = False + + if self.tool == "tool_stretch_dex_wrist": + self.move_to_pose({'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}) + self.move_to_pose({'joint_wrist_yaw': 0.0}) + self.move_to_pose({'gripper_aperture': 0.125}) + + pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer, tooltip_frame='link_grasp_center_real') + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1) + self.move_to_pose({'joint_lift': lift_to_pregrasp_m}) + + pregrasp_base_m, pregrasp_arm_m = self.manip.get_pregrasp_planar_translation(target, self.tf2_buffer) + self.drive_base(pregrasp_base_m + self.mb_offset) + + arm_to_pregrasp_m = max(self.arm_position + pregrasp_arm_m, 0.01) + arm_to_pregrasp_m = min(arm_to_pregrasp_m, 0.5) + self.move_to_pose({'wrist_extension': arm_to_pregrasp_m + self.arm_offset}) + + rospy.sleep(1) + + grasp_base_m, grasp_lift_m, grasp_arm_m = self.manip.get_grasp_from_pregrasp(target, self.tf2_buffer) + lift_to_grasp_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_to_grasp_m = min(lift_to_grasp_m, 1.1) + arm_to_grasp_m = max(self.arm_position + grasp_arm_m, 0.01) + arm_to_grasp_m = min(arm_to_grasp_m, 0.5) + if self.tool == "tool_stretch_dex_wrist": + lift_to_grasp_m -= 0.03 + arm_to_grasp_m += 0.02 + self.move_to_pose({'translate_mobile_base': grasp_base_m + self.mb_offset, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m + self.arm_offset}) + + gripper_to_grasp_m = target['width_m'] - 0.18 + self.move_to_pose({'gripper_aperture': gripper_to_grasp_m}) + + rospy.sleep(1) + + self.move_to_pose({'joint_lift': lift_to_grasp_m + 0.2}) + + self.location_above_surface_m = target['location_above_surface_m'] + self.is_holding_object = True + + def grasp_point_callback(self, point_msg): + self.grasp_point(point_msg) + + def grasp_clicked_point_callback(self, clicked_point_msg): + if not self.is_holding_object: + self.grasp_point(clicked_point_msg) + else: + self.place_object() + + def grasp_point(self, point_msg): + if self.grasp_targets is None: + print('cannot assign clicked point to object because no grasp targets available') + return + self.update_perception = False + + points_to_image_mat, _ = self.manip.max_height_im.get_points_to_image_mat(point_msg.header.frame_id, self.tf2_buffer) + p_x = point_msg.point.x + p_y = point_msg.point.y + p_z = point_msg.point.z + point_xyz = np.array([p_x, p_y, p_z, 1.0]) + point_pix = np.matmul(points_to_image_mat, point_xyz)[:3] + i_x, i_y, _ = point_pix + + h, w = self.manip.max_height_im.image.shape + if not (i_x >= 0 and i_x < w and i_y >= 0 and i_y < h): + print('point does not fall within the max height image') + self.update_perception = True + return + + min_distance = float('inf') + best_target = None + for target in self.grasp_targets: + dist = np.sqrt((i_x - target['location_xy_pix'][0]) ** 2 + (i_y - target['location_xy_pix'][1]) ** 2) + if dist < min_distance: + min_distance = dist + best_target = target + if best_target is None: + print('couldnt find grasp target close to point') + self.update_perception = True + return + + if self.publish_markers: # TODO: doesn't always appear in Rviz + image_copy = self.manip.max_height_im.image.copy() + image_copy[best_target['object_selector'] <= 0] = 0 + target_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.grasp_target_marker_pub.publish(target_pc) + + self.grasp_object(best_target) + self.update_perception = True + + def place_point_callback(self, point_msg): + self.place_object() + + def place_object(self): + if self.placement_target is None: + print('cannot place object on surface because no placement targets are available') + return + self.update_perception = False + + target = copy.copy(self.placement_target) + pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer) + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1) + self.move_to_pose({'joint_lift': lift_to_pregrasp_m}) + + if self.tool == "tool_stretch_dex_wrist": + self.move_to_pose({'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}) + + pregrasp_yaw = self.manip.get_pregrasp_yaw(target, self.tf2_buffer) + self.move_to_pose({'joint_wrist_yaw': pregrasp_yaw}) + + pregrasp_base_m, pregrasp_arm_m = self.manip.get_pregrasp_planar_translation(target, self.tf2_buffer) + self.drive_base(pregrasp_base_m) + + arm_to_pregrasp_m = max(self.arm_position + pregrasp_arm_m, 0.01) + arm_to_pregrasp_m = min(arm_to_pregrasp_m, 0.5) + self.move_to_pose({'wrist_extension': arm_to_pregrasp_m}) + + rospy.sleep(1) + + grasp_base_m, grasp_lift_m, grasp_arm_m = self.manip.get_grasp_from_pregrasp(target, self.tf2_buffer) + lift_to_grasp_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_to_grasp_m = min(lift_to_grasp_m, 1.1) + arm_to_grasp_m = max(self.arm_position + grasp_arm_m, 0.01) + arm_to_grasp_m = min(arm_to_grasp_m, 0.5) + if self.tool == "tool_stretch_dex_wrist": + lift_to_grasp_m -= 0.03 + arm_to_grasp_m += 0.02 + self.move_to_pose({'translate_mobile_base': grasp_base_m, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m}) + + self.move_to_pose({'gripper_aperture': 0.125}) + + rospy.sleep(1) + + self.move_to_pose({'joint_lift': lift_to_grasp_m + 0.2}) + + self.location_above_surface_m = None + self.is_holding_object = False + self.update_perception = True + + def perceive_objects(self): + if not self.update_perception: + return + + self.manip.update(self.point_cloud, self.tf2_buffer) + surface_mask, plane_parameters = sm.find_closest_flat_surface(self.manip.max_height_im, None) + if surface_mask is None: + return + + if self.publish_markers: + image_copy = self.manip.max_height_im.image.copy() + image_copy[surface_mask <= 0] = 0 + surface_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.surface_marker_pub.publish(surface_pc) + + if not self.is_holding_object: + grasp_targets = sm.find_objects_to_grasp(surface_mask, plane_parameters, self.manip.max_height_im) + if len(grasp_targets) == 0: + return + + if self.publish_markers: + objects_mask = grasp_targets[0]['object_selector'].copy() + for target in grasp_targets: + objects_mask = objects_mask | target['object_selector'] + + image_copy = self.manip.max_height_im.image.copy() + image_copy[objects_mask <= 0] = 0 + objects_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.objects_marker_pub.publish(objects_pc) + self.grasp_targets = grasp_targets + else: + placement_target = sm.find_placement_target_on_surface(surface_mask, self.location_above_surface_m, self.manip.max_height_im) + if placement_target is None: + return + + if self.publish_markers: + image_copy = self.manip.max_height_im.image.copy() + image_copy[placement_target['surface_convex_hull_mask'] <= 0] = 0 + surface_placement_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.surface_placement_marker_pub.publish(surface_placement_pc) + self.placement_target = placement_target + + def joint_states_callback(self, joint_states): + self.arm_position, _, _ = hm.get_wrist_state(joint_states) + self.lift_position, _, _ = hm.get_lift_state(joint_states) + self.left_finger_position, _, _ = hm.get_left_finger_state(joint_states) + i = joint_states.name.index('joint_wrist_yaw') + self.yaw_position = joint_states.position[i] + i = joint_states.name.index('joint_wrist_pitch') + self.pitch_position = joint_states.position[i] + i = joint_states.name.index('joint_wrist_roll') + self.roll_position = joint_states.position[i] + + def main(self): + hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=True) + self.surface_marker_pub = rospy.Publisher('/grasp_object/surface_marker', PointCloud2, queue_size=1) + self.surface_placement_marker_pub = rospy.Publisher('/grasp_object/surface_placement_marker', PointCloud2, queue_size=1) + self.objects_marker_pub = rospy.Publisher('/grasp_object/objects_marker', PointCloud2, queue_size=1) + self.grasp_target_marker_pub = rospy.Publisher('/grasp_object/grasp_target_marker', PointCloud2, queue_size=1) + self.grasp_clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.grasp_clicked_point_callback) + self.joint_states_sub = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) + self.grasp_point_sub = rospy.Subscriber('/grasp_object/grasp_point', PointStamped, self.grasp_point_callback) + self.place_point_sub = rospy.Subscriber('/grasp_object/place_point', PointStamped, self.place_point_callback) + self.manip = mp.ManipulationView(self.tf2_buffer, None, self.get_tool()) + self.move_base = nv.MoveBase(self) + + rate = rospy.Rate(10.0) + while not rospy.is_shutdown(): + self.perceive_objects() + rate.sleep() + + +if __name__ == "__main__": + try: + node = GraspObjectNode() + node.main() + except KeyboardInterrupt: + pass + diff --git a/stretch_demos/nodes/retract_arm.py b/stretch_demos/nodes/retract_arm.py new file mode 100755 index 0000000..ab99bea --- /dev/null +++ b/stretch_demos/nodes/retract_arm.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 + +import rospy +import hello_helpers.hello_misc as hm +from sensor_msgs.msg import BatteryState + + +class RetractArmNode(hm.HelloNode): + def __init__(self): + hm.HelloNode.__init__(self) + + def main(self): + hm.HelloNode.main(self, 'retract_arm', 'retract_arm', wait_for_first_pointcloud=False) + self.move_to_pose({'gripper_aperture': 0.125}) + self.move_to_pose({'joint_lift': 0.8}) + self.move_to_pose({'joint_arm': 0.0}) + self.move_to_pose({'joint_wrist_yaw': 0.0, 'joint_wrist_roll': 0.0}) + self.move_to_pose({'joint_wrist_pitch': -1.0}) + + +if __name__ == "__main__": + node = RetractArmNode() + node.main() +