Click-to-Pregrasp

.gitignore

@@ -33,3 +33,6 @@ mkcert*
 dist/*
 
 outcomes/*
+
+# Compiled Python
+*/__pycache__

CMakeLists.txt

@@ -9,9 +9,20 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(rosbridge_server REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 # find_package(tf2_web_republisher REQUIRED)
 
+##############################
+## Generate Custom Messages ##
+##############################
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/MoveToPregrasp.action"
+  DEPENDENCIES geometry_msgs
+)
+
+
 #############
 ## Install ##
 #############
@@ -24,6 +35,7 @@ find_package(rosbridge_server REQUIRED)
 install(PROGRAMS
   nodes/compressed_image_visualizer.py
   nodes/configure_video_streams.py
+  nodes/move_to_pregrasp.py
   nodes/navigation_camera.py
   DESTINATION lib/${PROJECT_NAME}
 )

action/MoveToPregrasp.action

@@ -0,0 +1,29 @@
+# MoveToPregrasp takes in the scaled (u,v) coordinates of a click on the Realsense camera,
+# as well as some configuration such as the direction of the pregrasp. It then moves
+# the robot to the pregrasp configuration, giving feedback on the way.
+
+# These should be in the range [0.0, 1.0]
+float64 scaled_u
+float64 scaled_v
+
+uint8 PREGRASP_DIRECTION_AUTO=0
+uint8 PREGRASP_DIRECTION_HORIZONTAL=1
+uint8 PREGRASP_DIRECTION_VERTICAL=2
+
+uint8 pregrasp_direction
+
+---
+# Result
+
+uint8 STATUS_SUCCESS=0
+uint8 STATUS_FAILURE=1
+uint8 STATUS_CANCELLED = 2
+uint8 STATUS_TIMEOUT = 3
+uint8 STATUS_GOAL_NOT_REACHABLE = 4
+
+uint8 status
+---
+# Feedback
+float32 initial_distance_m
+float32 remaining_distance_m
+builtin_interfaces/Duration elapsed_time

launch/multi_camera.launch.py

@@ -12,6 +12,9 @@
     get_package_share_directory("stretch_core"), "config", "HighAccuracyPreset.json"
 )
 
+D435_RESOLUTION = "424x240x15"
+D405_RESOLUTION = "480x270x15"
+
 configurable_parameters = [
     {"name": "camera_namespace1", "default": "", "description": "namespace for camera"},
     {"name": "camera_name1", "default": "camera", "description": "camera unique name"},
@@ -23,18 +26,18 @@
     },
     {
         "name": "depth_module.depth_profile1",
-        "default": "424x240x15",
+        "default": D435_RESOLUTION,
         "description": "depth module profile",
     },
     {
         "name": "depth_module.infra_profile1",
-        "default": "424x240x15",
+        "default": D435_RESOLUTION,
         "description": "depth module infrared profile",
     },
     {"name": "enable_depth1", "default": "true", "description": "enable depth stream"},
     {
         "name": "rgb_camera.color_profile1",
-        "default": "424x240x15",
+        "default": D435_RESOLUTION,
         "description": "color image width",
     },
     {"name": "enable_color1", "default": "true", "description": "enable color stream"},
@@ -87,7 +90,7 @@
     },
     {
         "name": "depth_module.depth_profile2",
-        "default": "480x270x15",
+        "default": D405_RESOLUTION,
         "description": "depth module profile",
     },
     {
@@ -98,7 +101,7 @@
     {"name": "enable_depth2", "default": "true", "description": "enable depth stream"},
     {
         "name": "depth_module.color_profile2",
-        "default": "480x270x15",
+        "default": D405_RESOLUTION,
         "description": "color image width",
     },
     {"name": "enable_color2", "default": "true", "description": "enable color stream"},

launch/web_interface.launch.py

@@ -500,4 +500,15 @@ def generate_launch_description():
     #         shell=True,
     #     )
     # )
+
+    # Move To Pre-grasp Action Server
+    move_to_pregrasp_node = Node(
+        package="stretch_web_teleop",
+        executable="move_to_pregrasp.py",
+        output="screen",
+        arguments=[LaunchConfiguration("params")],
+        parameters=[],
+    )
+    ld.add_action(move_to_pregrasp_node)
+
     return ld

nodes/configure_video_streams.py

@@ -23,6 +23,8 @@
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image, JointState, PointCloud2
 from std_srvs.srv import SetBool
 
+# TODO: Add docstrings to this file.
+
 
 class ConfigureVideoStreams(Node):
     BACKGROUND_COLOR = (200, 200, 200)
@@ -65,6 +67,9 @@ def __init__(self, params_file, has_beta_teleop_kit):
         self.expanded_gripper_camera_rgb_image = None
         self.cv_bridge = CvBridge()
 
+        # Stores the camera projection matrix
+        self.P = None
+
         # Compressed Image publishers
         self.publisher_realsense_cmp = self.create_publisher(
             CompressedImage, "/camera/color/image_raw/rotated/compressed", 15
@@ -165,6 +170,12 @@ def camera_info_cb(self, msg):
 
     def pc_callback(self, msg, img):
         # Only keep points that are within 0.01m to 1.5m from the camera
+        if self.P is None:
+            self.get_logger().warn(
+                "Camera projection matrix is not available. Skipping point cloud processing."
+            )
+            return img
+
         pc_in_camera = ros2_numpy.point_cloud2.pointcloud2_to_xyz_array(msg)
         pcl_cloud = pcl.PointCloud(np.array(pc_in_camera, dtype=np.float32))
         passthrough = pcl_cloud.make_passthrough_filter()

nodes/constants.py

@@ -0,0 +1,177 @@
+"""
+This file contains various constants relevant to stretch:
+joint names, hard-coded configurations, speed profiles, control modes,
+frames of reference, etc.
+"""
+
+# Standard Imports
+from enum import Enum
+from typing import Dict, List
+
+# Third-Party Imports
+import numpy as np
+
+
+class Joint(Enum):
+    """
+    Joint names of Stretch.
+    """
+
+    BASE_ROTATION = "joint_mobile_base_rotation"
+    ARM_LIFT = "joint_lift"
+    ARM_L0 = "joint_arm_l0"
+    ARM_L1 = "joint_arm_l1"
+    ARM_L2 = "joint_arm_l2"
+    ARM_L3 = "joint_arm_l3"
+    COMBINED_ARM = "joint_arm"
+    WRIST_YAW = "joint_wrist_yaw"
+    WRIST_PITCH = "joint_wrist_pitch"
+    WRIST_ROLL = "joint_wrist_roll"
+    GRIPPER_RIGHT = "joint_gripper_finger_right"
+    GRIPPER_LEFT = "joint_gripper_finger_left"
+    RIGHT_WHEEL = "joint_right_wheel"
+    LEFT_WHEEL = "joint_left_wheel"
+    HEAD_PAN = "joint_head_pan"
+    HEAD_TILT = "joint_head_tilt"
+
+    @staticmethod
+    def get_arm_joints():
+        """
+        Get the list of telescoping arm joints.
+        """
+        return [Joint.ARM_L0, Joint.ARM_L1, Joint.ARM_L2, Joint.ARM_L3]
+
+    @staticmethod
+    def get_wrist_joints():
+        """
+        Get the list of wrist joints.
+        """
+        return [Joint.WRIST_YAW, Joint.WRIST_PITCH, Joint.WRIST_ROLL]
+
+
+class Frame(Enum):
+    """
+    Key frame names of reference for Stretch.
+    """
+
+    BASE_LINK = "base_link"
+    END_EFFECTOR_LINK = "link_grasp_center"
+    ODOM = "odom"
+
+
+class ControlMode(Enum):
+    """
+    The control modes for the Stretch Driver.
+    """
+
+    POSITION = "position"
+    NAVIGATION = "navigation"
+
+    def get_service_name(self):
+        """
+        Get the service name for switching to this control mode.
+        """
+        return f"switch_to_{self.value}_mode"
+
+
+class SpeedProfile(Enum):
+    """
+    The speed profile to use to get max velocities and accelerations. This
+    should correspond to the speed profile in robot params.
+    """
+
+    SLOW = "slow"
+    DEFAULT = "default"
+    FAST = "fast"
+    MAX = "max"
+
+
+def get_stow_configuration(
+    joints: List[Joint], partial: bool = False
+) -> Dict[Joint, float]:
+    """
+    Get the joint configuration for stowing the arm.
+
+    Note that in practice, commanding all these joints at the same time can create
+    a motion with a larger footprint than desired, resulting in collisions. It is
+    typically best practice to first command arm length, then command wrist/gripper,
+    then command lift.
+
+    Parameters
+    ----------
+    joints: The joints return.
+    partial: If True, make the arm length stop slightly before the robot base, so that if the
+        wrist is vertically down, it won't collide with the base.
+
+    Returns
+    -------
+    Dict[Joint, float]: The joint configuration.
+    """
+    retval = {}
+    for joint in joints:
+        if joint == Joint.ARM_L0:
+            retval[joint] = 0.1675 if partial else 0.0
+        elif joint == Joint.ARM_LIFT:
+            retval[
+                joint
+            ] = 0.40  # This is chosen so even when the gripper is pointing down, it doesn't hit the base.
+        elif joint == Joint.WRIST_YAW:
+            retval[joint] = 3.19579  # Should match src/shared/util.tsx
+        elif joint == Joint.WRIST_PITCH:
+            retval[joint] = -0.497  # Should match src/shared/util.tsx
+        elif joint == Joint.WRIST_ROLL:
+            retval[joint] = 0.0  # Should match src/shared/util.tsx
+        elif joint == Joint.GRIPPER_LEFT:
+            retval[
+                joint
+            ] = 0.0  # close gripper when stowed. An open gripper can sometimes get caught in the mast.
+    return retval
+
+
+def get_pregrasp_wrist_configuration(horizontal: bool) -> Dict[Joint, float]:
+    """
+    Get the wrist rotation for the pregrasp position.
+
+    TODO: Add the option to specify 0 or 90 degree roll.
+
+    Parameters
+    ----------
+    horizontal_grasp: Whether the pregrasp position is horizontal.
+
+    Returns
+    -------
+    Dict[Joint, float]: The joint configuration.
+    """
+    if horizontal:
+        return {
+            Joint.WRIST_YAW: 0.0,
+            Joint.WRIST_PITCH: 0.0,
+            Joint.WRIST_ROLL: 0.0,
+        }
+    else:
+        return {
+            Joint.WRIST_YAW: 0.0,
+            Joint.WRIST_PITCH: -np.pi / 2.0,
+            Joint.WRIST_ROLL: 0.0,
+        }
+
+
+def get_gripper_configuration(closed: bool) -> Dict[Joint, float]:
+    """
+    Get the gripper configuration.
+
+    Parameters
+    ----------
+    closed: Whether the gripper is closed. If false, returns the configuration
+        where the gripper is fully open.
+
+    Returns
+    -------
+    Dict[Joint, float]: The joint configuration.
+    """
+    return {
+        # We only need to command one gripper joint, as the other is coupled.
+        Joint.GRIPPER_LEFT: 0.0
+        if closed
+        else 0.84,
+    }