feat: manipulation demo improvements (#619)

Author: maciejmajek

docs/API_documentation/langchain_integration/ROS_2_tools.md

@@ -85,7 +85,8 @@ RAI provides specific ROS 2 tools for certain ROS 2 packages.
 
 ### Custom Tools
 
-| Tool Name                | Description                                  |
-| ------------------------ | -------------------------------------------- |
-| `MoveToPointTool`        | Tool for moving to a point                   |
-| `GetObjectPositionsTool` | Tool for retrieving the positions of objects |
+| Tool Name                | Description                                         |
+| ------------------------ | --------------------------------------------------- |
+| `MoveToPointTool`        | Tool for moving to a point                          |
+| `MoveObjectFromToTool`   | Tool for moving an object from one point to another |
+| `GetObjectPositionsTool` | Tool for retrieving the positions of objects        |

examples/manipulation-demo.py

@@ -24,7 +24,11 @@
 from rai.agents.langchain.core import create_conversational_agent
 from rai.communication.ros2 import wait_for_ros2_services, wait_for_ros2_topics
 from rai.communication.ros2.connectors import ROS2Connector
-from rai.tools.ros2.manipulation import GetObjectPositionsTool, MoveToPointTool
+from rai.tools.ros2.manipulation import (
+    GetObjectPositionsTool,
+    MoveObjectFromToTool,
+    ResetArmTool,
+)
 from rai.tools.ros2.simple import GetROS2ImageConfiguredTool
 from rai_open_set_vision.tools import GetGrabbingPointTool
 
@@ -35,7 +39,7 @@
 
 def create_agent():
     rclpy.init()
-    connector = ROS2Connector()
+    connector = ROS2Connector(executor_type="single_threaded")
 
     required_services = ["/grounded_sam_segment", "/grounding_dino_classify"]
     required_topics = ["/color_image5", "/depth_image5", "/color_camera_info5"]
@@ -55,7 +59,8 @@ def create_agent():
             camera_info_topic="/color_camera_info5",
             get_grabbing_point_tool=GetGrabbingPointTool(connector=connector),
         ),
-        MoveToPointTool(connector=connector, manipulator_frame="panda_link0"),
+        MoveObjectFromToTool(connector=connector, manipulator_frame="panda_link0"),
+        ResetArmTool(connector=connector, manipulator_frame="panda_link0"),
         GetROS2ImageConfiguredTool(connector=connector, topic="/color_image5"),
     ]
 

src/rai_core/rai/tools/ros2/__init__.py

@@ -50,6 +50,8 @@
 )
 from .manipulation.custom import (
     GetObjectPositionsTool,
+    MoveObjectFromToTool,
+    MoveObjectFromToToolInput,
     MoveToPointTool,
     MoveToPointToolInput,
 )
@@ -83,6 +85,8 @@
     "GetROS2TopicsNamesAndTypesTool",
     "GetROS2TransformConfiguredTool",
     "GetROS2TransformTool",
+    "MoveObjectFromToTool",
+    "MoveObjectFromToToolInput",
     "MoveToPointTool",
     "MoveToPointToolInput",
     "Nav2Toolkit",

src/rai_core/rai/tools/ros2/manipulation/__init__.py

@@ -19,10 +19,20 @@
         "This is a ROS2 feature. Make sure ROS2 is installed and sourced."
     )
 
-from .custom import GetObjectPositionsTool, MoveToPointTool, MoveToPointToolInput
+from .custom import (
+    GetObjectPositionsTool,
+    MoveObjectFromToTool,
+    MoveObjectFromToToolInput,
+    MoveToPointTool,
+    MoveToPointToolInput,
+    ResetArmTool,
+)
 
 __all__ = [
     "GetObjectPositionsTool",
+    "MoveObjectFromToTool",
+    "MoveObjectFromToToolInput",
     "MoveToPointTool",
     "MoveToPointToolInput",
+    "ResetArmTool",
 ]

src/rai_core/rai/tools/ros2/manipulation/custom.py

@@ -127,6 +127,132 @@ def _run(
             return f"Failed to position end effector at coordinates ({x:.2f}, {y:.2f}, {z:.2f})."
 
 
+class MoveObjectFromToToolInput(BaseModel):
+    x: float = Field(description="The x coordinate of the point to move to")
+    y: float = Field(description="The y coordinate of the point to move to")
+    z: float = Field(description="The z coordinate of the point to move to")
+    x1: float = Field(description="The x coordinate of the point to move to")
+    y1: float = Field(description="The y coordinate of the point to move to")
+    z1: float = Field(description="The z coordinate of the point to move to")
+
+
+class MoveObjectFromToTool(BaseROS2Tool):
+    name: str = "move_object_from_to"
+    description: str = (
+        "Move an object from one point to another. "
+        "The tool will grab the object from the first point and then release it at the second point. "
+        "The tool will not confirm the success of grabbing or releasing objects. Use additional sensors (e.g. camera) or tools for that information."
+    )
+
+    manipulator_frame: str = Field(..., description="Manipulator frame")
+    min_z: float = Field(default=0.135, description="Minimum z coordinate [m]")
+    calibration_x: float = Field(default=0.0, description="Calibration x [m]")
+    calibration_y: float = Field(default=0.0, description="Calibration y [m]")
+    calibration_z: float = Field(default=0.1, description="Calibration z [m]")
+    additional_height: float = Field(
+        default=0.05, description="Additional height for the place task [m]"
+    )
+
+    # constant quaternion
+    quaternion: Quaternion = Field(
+        default=Quaternion(x=0.9238795325112867, y=-0.3826834323650898, z=0.0, w=0.0),
+        description="Constant quaternion",
+    )
+
+    args_schema: Type[MoveObjectFromToToolInput] = MoveObjectFromToToolInput
+
+    def _run(
+        self,
+        x: float,
+        y: float,
+        z: float,
+        x1: float,
+        y1: float,
+        z1: float,
+    ) -> str:
+        # NOTE: create_client could be refactored into self.connector.service_call
+        self.connector.service_call
+        client = self.connector.node.create_client(
+            ManipulatorMoveTo,
+            "/manipulator_move_to",
+        )
+        pose_stamped = PoseStamped()
+        pose_stamped.header.frame_id = self.manipulator_frame
+        pose_stamped.pose = Pose(
+            position=Point(x=x, y=y, z=z),
+            orientation=self.quaternion,
+        )
+
+        pose_stamped1 = PoseStamped()
+        pose_stamped1.header.frame_id = self.manipulator_frame
+        pose_stamped1.pose = Pose(
+            position=Point(x=x1, y=y1, z=z1),
+            orientation=self.quaternion,
+        )
+
+        pose_stamped.pose.position.x += self.calibration_x
+        pose_stamped.pose.position.y += self.calibration_y
+        pose_stamped.pose.position.z += self.calibration_z
+
+        pose_stamped.pose.position.z = np.max(
+            [pose_stamped.pose.position.z, self.min_z]
+        )
+
+        pose_stamped1.pose.position.x += self.calibration_x
+        pose_stamped1.pose.position.y += self.calibration_y
+        pose_stamped1.pose.position.z += self.calibration_z
+
+        pose_stamped1.pose.position.z = np.max(
+            [pose_stamped1.pose.position.z, self.min_z]
+        )
+
+        request = ManipulatorMoveTo.Request()
+        request.target_pose = pose_stamped
+
+        request.initial_gripper_state = True  # open
+        request.final_gripper_state = False  # closed
+
+        future = client.call_async(request)
+        self.connector.node.get_logger().debug(
+            f"Calling ManipulatorMoveTo service with request: x={request.target_pose.pose.position.x:.2f}, y={request.target_pose.pose.position.y:.2f}, z={request.target_pose.pose.position.z:.2f}"
+        )
+        response = get_future_result(future, timeout_sec=5.0)
+
+        if response is None:
+            return f"Service call failed for point ({x:.2f}, {y:.2f}, {z:.2f})."
+
+        if response.success:
+            self.connector.logger.info(
+                f"End effector successfully positioned at coordinates ({x:.2f}, {y:.2f}, {z:.2f})."
+            )
+        else:
+            self.connector.logger.error(
+                f"Failed to position end effector at coordinates ({x:.2f}, {y:.2f}, {z:.2f})."
+            )
+            return "Failed to position end effector at coordinates ({x:.2f}, {y:.2f}, {z:.2f})."
+
+        request = ManipulatorMoveTo.Request()
+        request.target_pose = pose_stamped1
+
+        request.initial_gripper_state = False  # closed
+        request.final_gripper_state = True  # open
+
+        future = client.call_async(request)
+        self.connector.node.get_logger().debug(
+            f"Calling ManipulatorMoveTo service with request: x={request.target_pose.pose.position.x:.2f}, y={request.target_pose.pose.position.y:.2f}, z={request.target_pose.pose.position.z:.2f}"
+        )
+
+        response = get_future_result(future, timeout_sec=20.0)
+
+        if response is None:
+            return f"Service call failed for point ({x:.2f}, {y:.2f}, {z:.2f})."
+
+        if response.success:
+            return f"End effector successfully positioned at coordinates ({x:.2f}, {y:.2f}, {z:.2f}). Note: The status of object interaction (grab/drop) is not confirmed by this movement."
+        else:
+            return f"Failed to position end effector at coordinates ({x:.2f}, {y:.2f}, {z:.2f})."
+
+
 class GetObjectPositionsToolInput(BaseModel):
     object_name: str = Field(
         ..., description="The name of the object to get the positions of"
@@ -182,3 +308,56 @@ def _run(self, object_name: str):
             return f"No {object_name}s detected."
         else:
             return f"Centroids of detected {object_name}s in {self.target_frame} frame: [{', '.join(map(self.format_pose, mani_frame_poses))}]. Sizes of the detected objects are unknown."
+
+
+class ResetArmToolInput(BaseModel):
+    pass
+
+
+class ResetArmTool(BaseROS2Tool):
+    name: str = "reset_arm"
+    description: str = "Reset the arm to the initial position. Use when the arm is stuck or when arm obstructs the objects."
+
+    args_schema: Type[ResetArmToolInput] = ResetArmToolInput
+
+    # constant quaternion
+    quaternion: Quaternion = Field(
+        default=Quaternion(x=0.9238795325112867, y=-0.3826834323650898, z=0.0, w=0.0),
+        description="Constant quaternion",
+    )
+    manipulator_frame: str = Field(..., description="Manipulator frame")
+
+    def _run(self):
+        client = self.connector.node.create_client(
+            ManipulatorMoveTo,
+            "/manipulator_move_to",
+        )
+
+        x = 0.31
+        y = 0.0
+        z = 0.59
+
+        request = ManipulatorMoveTo.Request()
+        request.target_pose = PoseStamped()
+        request.target_pose.header.frame_id = self.manipulator_frame
+        request.target_pose.pose = Pose(
+            position=Point(x=x, y=y, z=z),
+            orientation=self.quaternion,
+        )
+
+        request.initial_gripper_state = True  # open
+        request.final_gripper_state = False  # closed
+
+        future = client.call_async(request)
+        self.connector.node.get_logger().debug(
+            f"Calling ManipulatorMoveTo service with request: x={request.target_pose.pose.position.x:.2f}, y={request.target_pose.pose.position.y:.2f}, z={request.target_pose.pose.position.z:.2f}"
+        )
+        response = get_future_result(future, timeout_sec=5.0)
+
+        if response is None:
+            return "Failed to reset the arm."
+
+        if response.success:
+            return "Arm successfully reset."
+        else:
+            return "Failed to reset the arm."

src/rai_extensions/rai_open_set_vision/rai_open_set_vision/agents/base_vision_agent.py

@@ -36,7 +36,7 @@ def __init__(
         os.makedirs(self._weights_path, exist_ok=True)
         self._init_weight_path()
         self.weight_path = self._weights_path
-        self.ros2_connector = ROS2Connector(ros2_name)
+        self.ros2_connector = ROS2Connector(ros2_name, executor_type="single_threaded")
 
     def _init_weight_path(self):
         try: