|
|
|
@ -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 |
Binit Shah
1 year ago