From 4ce97cbf5490d0d4c5e7a1cc434c01e204a5883f Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 29 Sep 2023 05:03:17 -0700 Subject: [PATCH] place point works --- stretch_demos/nodes/grasp_point | 3 +- stretch_demos/nodes/place_point | 252 +++++++------------------------- 2 files changed, 50 insertions(+), 205 deletions(-) mode change 100644 => 100755 stretch_demos/nodes/place_point diff --git a/stretch_demos/nodes/grasp_point b/stretch_demos/nodes/grasp_point index d036c4f..7bb5447 100755 --- a/stretch_demos/nodes/grasp_point +++ b/stretch_demos/nodes/grasp_point @@ -9,11 +9,10 @@ import numpy as np import rospy from std_msgs.msg import Empty, Float32 from geometry_msgs.msg import PointStamped -from sensor_msgs.msg import PointCloud2, JointState +from sensor_msgs.msg import PointCloud2 from visualization_msgs.msg import Marker 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 diff --git a/stretch_demos/nodes/place_point b/stretch_demos/nodes/place_point old mode 100644 new mode 100755 index 41fd913..d15fa2f --- a/stretch_demos/nodes/place_point +++ b/stretch_demos/nodes/place_point @@ -1,142 +1,50 @@ #!/usr/bin/env python3 import copy +import pickle +import pathlib +import subprocess import numpy as np import rospy +from std_msgs.msg import Empty, Float32 from geometry_msgs.msg import PointStamped -from sensor_msgs.msg import PointCloud2, JointState +from sensor_msgs.msg import PointCloud2 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): +class PlacePointNode(hm.HelloNode): def __init__(self): hm.HelloNode.__init__(self) - self.update_perception = True - self.publish_markers = True + self.active = False + self.update_perception = False - 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.09 - self.mb_offset = 0.02 - - 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 - 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 + self.yaw_offset}) - - self.move_to_pose({'gripper_aperture': 0.125}) - - 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}) - - 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}) + pc_pkl_fpath = str(pathlib.Path(subprocess.Popen("rospack find stretch_demos", shell=True, stdout=subprocess.PIPE).stdout.read().decode()[:-1]) / 'empty_pc.pkl') + with open(pc_pkl_fpath, 'rb') as inp: + self.empty_pc = pickle.load(inp) - 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 enable_callback(self, msg): + self.location_above_surface_m = msg.data + print(self.location_above_surface_m) + self.placement_target = None + self.update_perception = True + self.active = True - 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 + def disable_callback(self, msg): + self.active = False self.update_perception = False + self.location_above_surface_m = None + self.placement_target = None - 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 clicked_point_callback(self, clicked_point_msg): + self.place_object() def place_point_callback(self, point_msg): self.place_object() @@ -148,45 +56,13 @@ class GraspObjectNode(hm.HelloNode): 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.manip.perform_cartesian_grasp(target, self, tooltip_frame='link_grasp_center_real', placing=True) + msg = Empty() + self.result_pub.publish(msg) self.location_above_surface_m = None - self.is_holding_object = False - self.update_perception = True + self.update_perception = False + self.active = False def perceive_objects(self): if not self.update_perception: @@ -197,72 +73,42 @@ class GraspObjectNode(hm.HelloNode): 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'] + 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 + self.placement_target = placement_target - 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 + def publish_markers(self): + if self.placement_target is not None: + image_copy = self.manip.max_height_im.image.copy() + image_copy[self.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_target_marker_pub.publish(surface_placement_pc) 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 + self.surface_target_marker_pub.publish(self.empty_pc) - 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 main(self): + hm.HelloNode.main(self, 'place_point', 'place_point', wait_for_first_pointcloud=True) + self.surface_target_marker_pub = rospy.Publisher('/place_point/surface_target_marker', PointCloud2, queue_size=1) - 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] + self.clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_callback) + self.trigger_sub = rospy.Subscriber('/place_point/trigger_place_point', PointStamped, self.place_point_callback) + self.enable_sub = rospy.Subscriber('/place_point/enable', Float32, self.enable_callback) + self.disable_sub = rospy.Subscriber('/place_point/disable', Empty, self.disable_callback) + self.result_pub = rospy.Publisher('/place_point/result', Empty, queue_size=1) - 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() + self.publish_markers() rate.sleep() if __name__ == "__main__": try: - node = GraspObjectNode() + node = PlacePointNode() node.main() except KeyboardInterrupt: pass