add a wiping skill

exec_plan.py

@@ -23,6 +23,8 @@
     move_hand_to_relative_pose,
     open_gripper,
 )
+from skills.wipe import wipe_multiple_strokes
+from skills.wipe_online import wipe_online as run_wipe_online
 from skills.spot_navigation import navigate_to_absolute_pose
 from spot_utils.perception.spot_cameras import capture_images
 from spot_utils.spot_localization import SpotLocalizer
@@ -54,6 +56,15 @@
 SPOT_ROOM_POSE: Dict[str, float] = dict()
 
 
+def np_pose_to_SE3(X_RobEE: NDArray) -> math_helpers.SE3Pose:
+    return math_helpers.SE3Pose(
+        X_RobEE[0],
+        X_RobEE[1],
+        X_RobEE[2],
+        rot=math_helpers.Quat(X_RobEE[6], X_RobEE[3], X_RobEE[4], X_RobEE[5]),
+    )
+
+
 def init(hostname: str, map_name: str, endpoint_url: Optional[str]) -> None:
     """Initialize the robot and the localizer."""
     global LOCALIZER
@@ -121,30 +132,48 @@ def grasp(text_prompt: Optional[str]) -> None:
 def grasp_at_pose(X_RobEE: NDArray) -> None:
     """Grasp an object at a specified pose relative to the robot."""
     open_gripper(ROBOT)
-    pose = math_helpers.SE3Pose(
-        x=X_RobEE[0],
-        y=X_RobEE[1],
-        z=X_RobEE[2],
-        rot=math_helpers.Quat(X_RobEE[6], X_RobEE[3], X_RobEE[4], X_RobEE[5]),
-    )
+    pose = np_pose_to_SE3(X_RobEE)
     move_hand_to_relative_pose(ROBOT, pose.mult(grasp_offset))
     close_gripper(ROBOT)
     move_hand_to_relative_pose(ROBOT, DEFAULT_HAND_LOOK_FLOOR_POSE)
 
 
 def place_at_pose(X_RobEE: NDArray) -> None:
     """Place an object at a specified pose relative to the robot."""
-    pose = math_helpers.SE3Pose(
-        x=X_RobEE[0],
-        y=X_RobEE[1],
-        z=X_RobEE[2],
-        rot=math_helpers.Quat(X_RobEE[6], X_RobEE[3], X_RobEE[4], X_RobEE[5]),
-    )
+    pose = np_pose_to_SE3(X_RobEE)
     move_hand_to_relative_pose(ROBOT, pose.mult(grasp_offset))
     open_gripper(ROBOT)
     move_hand_to_relative_pose(ROBOT, DEFAULT_HAND_LOOK_FLOOR_POSE)
 
 
+def vertical_wipe(
+    X_RobEE_start: NDArray, stroke_dx: float, y_delta: float, num_strokes: int
+) -> None:
+    """Wipes a surface at a given pose with known height and width.."""
+    start_pose = np_pose_to_SE3(X_RobEE_start)
+    wipe_multiple_strokes(
+        ROBOT,
+        start_pose,
+        start_pose,
+        stroke_dx=stroke_dx,
+        stroke_dy=0,
+        delta_x_y_between_strokes=(0, y_delta),
+        num_strokes=num_strokes,
+        duration_per_stroke=3.0,
+        num_attempts_per_stroke=1,
+    )
+
+
+def wipe_at(*args, **kwargs) -> None:
+    """Run the online wipe skill using defaults from the skill module."""
+    run_wipe_online(
+        ROBOT,
+        None,
+        None,
+        LOCALIZER,
+    )
+
+
 if __name__ == "__main__":
     # running this script standalone initializes a bosdyn robot and localizer.
     # It then executes the list of commands provided in the plan file
@@ -178,7 +207,7 @@ def place_at_pose(X_RobEE: NDArray) -> None:
             SPOT_ROOM_POSE = metadata["spot-room-pose"]
         else:
             print("spot-room-pose not found in metadata.yaml, using default val")
-            SPOT_ROOM_POSE = {"x": 0.0, "y": 0.0, "z": 0.0}
+            SPOT_ROOM_POSE = {"x": 0.0, "y": 0.0, "angle": 0.0}
     with open(args.plan, "r") as plan_file:
         exec(plan_file.read())
     print("done")

skills/wipe.py

@@ -0,0 +1,157 @@
+"""Interface for spot sweeping skill."""
+
+import argparse
+import time
+from typing import Tuple
+
+import numpy as np
+from bosdyn.client import math_helpers
+from bosdyn.client.sdk import Robot
+
+from skills.spot_hand_move import (
+    close_gripper,
+    move_hand_to_relative_pose,
+    move_hand_to_relative_pose_with_velocity,
+    open_gripper,
+)
+
+
+def wipe_one_stroke(
+    robot: Robot,
+    wipe_start_pose: math_helpers.SE3Pose,
+    move_dx: float,
+    move_dy: float,
+    duration: float,
+) -> None:
+    """Wipe a table surface in the xy plane.
+
+    The robot starts at a start pose, and then moves forward and back by
+    dx and dy.
+    """
+    # Move first in the yz direction (perpendicular to robot body) to avoid
+    # knocking the target object over.
+    move_hand_to_relative_pose(robot, wipe_start_pose)
+    first_move_pose = math_helpers.SE3Pose(
+        x=wipe_start_pose.x + move_dx,  # sensible default
+        y=wipe_start_pose.y + move_dy,
+        z=wipe_start_pose.z,
+        rot=wipe_start_pose.rot,
+    )
+    move_hand_to_relative_pose_with_velocity(
+        robot, wipe_start_pose, first_move_pose, duration
+    )
+    time.sleep(0.1)
+    # Move back to the start pose.
+    move_hand_to_relative_pose_with_velocity(
+        robot, first_move_pose, wipe_start_pose, duration
+    )
+
+
+def wipe_multiple_strokes(
+    robot: Robot,
+    wipe_start_pose: math_helpers.SE3Pose,
+    end_look_pose: math_helpers.SE3Pose,
+    stroke_dx: float,
+    stroke_dy: float,
+    delta_x_y_between_strokes: Tuple[float, float],
+    num_strokes: int,
+    duration_per_stroke: float,
+    num_attempts_per_stroke: int,
+) -> None:
+    """Wipe a table surface in the xy plane.
+
+    The robot starts at a start pose, and then moves forward and back by
+    dx and dy.
+    """
+    curr_stroke_start_pose = wipe_start_pose
+    for i in range(num_strokes):
+        for i in range(num_attempts_per_stroke):
+            move_hand_to_relative_pose(robot, curr_stroke_start_pose)
+            first_move_pose = math_helpers.SE3Pose(
+                x=curr_stroke_start_pose.x + stroke_dx,
+                y=curr_stroke_start_pose.y + stroke_dy,
+                z=curr_stroke_start_pose.z,
+                rot=curr_stroke_start_pose.rot,
+            )
+            move_hand_to_relative_pose_with_velocity(
+                robot, curr_stroke_start_pose, first_move_pose, duration_per_stroke
+            )
+            time.sleep(0.1)
+            # Move back to the start pose.
+            move_hand_to_relative_pose_with_velocity(
+                robot, first_move_pose, curr_stroke_start_pose, duration_per_stroke
+            )
+        # Move to the next stroke position.
+        curr_stroke_start_pose = math_helpers.SE3Pose(
+            x=curr_stroke_start_pose.x + delta_x_y_between_strokes[0],
+            y=curr_stroke_start_pose.y + delta_x_y_between_strokes[1],
+            z=curr_stroke_start_pose.z,
+            rot=curr_stroke_start_pose.rot,
+        )
+    # Move to the end look pose.
+    move_hand_to_relative_pose(robot, end_look_pose)
+
+
+if __name__ == "__main__":
+    # Run this file alone to test manually.
+    # Make sure to pass in --spot_robot_ip.
+
+    # the robot opens its gripper and pauses
+    # until a brush is put in the gripper, with the bristles facing down and
+    # forward. The robot should then brush to the right.
+
+    # pylint: disable=ungrouped-imports
+    from bosdyn.client import create_standard_sdk
+    from bosdyn.client.lease import LeaseClient
+    from bosdyn.client.util import authenticate
+
+    from spot_utils.utils import verify_estop
+
+    def _run_manual_test() -> None:
+        # Put inside a function to avoid variable scoping issues.
+        parser = argparse.ArgumentParser(description="Parse the robot's hostname.")
+        parser.add_argument(
+            "--hostname",
+            type=str,
+            required=True,
+            help="The robot's hostname/ip-address (e.g. 192.168.80.3)",
+        )
+        args = parser.parse_args()
+
+        # Get constants.
+        hostname = args.hostname
+
+        sdk = create_standard_sdk("WipeSkillTestClient")
+        robot = sdk.create_robot(hostname)
+        authenticate(robot)
+        verify_estop(robot)
+        lease_client = robot.ensure_client(LeaseClient.default_service_name)
+        lease_client.take()
+        robot.time_sync.wait_for_sync()
+
+        # Move the hand to the side.
+        hand_side_pose = math_helpers.SE3Pose(
+            x=0.80, y=0.0, z=0.25, rot=math_helpers.Quat.from_yaw(-np.pi / 2)
+        )
+        move_hand_to_relative_pose(robot, hand_side_pose)
+        # Ask for the eraser.
+        open_gripper(robot)
+        msg = "Put the brush in the robot's gripper, then press enter"
+        input(msg)
+        close_gripper(robot)
+
+        # NOTE: these parameters hardcoded for a particular child_play_table
+        # object njk is experimenting with. Please swap out depending on the
+        # actual object you have
+        start_pose = math_helpers.SE3Pose(
+            x=0.85, y=-0.2, z=-0.08, rot=math_helpers.Quat.from_pitch(np.pi / 2)
+        )
+        end_pose = math_helpers.SE3Pose(
+            x=0.65, y=0.0, z=0.4, rot=math_helpers.Quat.from_pitch(np.pi / 2.5)
+        )
+        # Execute the sweep.
+        wipe_multiple_strokes(
+            robot, start_pose, end_pose, 0.0, 0.4, (0.05, 0.0), 5, 1.0, 1
+        )
+
+    _run_manual_test()