Changes to get feijoa demo working with 621-outside map

README.md

@@ -13,3 +13,40 @@ Currently, the main script in the repo is `spot_localization.py`. Here is an exa
 python spot_utils/spot_localization.py --hostname 192.168.80.3 --map_name b45-621
 ```
 You can 'hijack' the robot via the tablet when prompted, move it around, and then have it print out its pose! We can then command it to navigate to any saved pose via other utilities (which are forthcoming).
+
+## Mapping
+> Last Updated: 05/17/2025
+
+Our code is currently designed to operate given a saved map of a particular
+environment. The map is required to define a coordinate system that persists
+between robot runs. Moreover, each map contains associated metadata information
+that we use for visualization, collision checking, and a variety of other
+functions.
+
+To create a new map of a new environment:
+1. Print out and tape april tags around the environment. The tags are [here](https://support.bostondynamics.com/s/article/About-Fiducials)
+2. Run the interactive script from the spot SDK to create a map, while walking
+   the spot around the environment. The script is [here](https://github.com/boston-dynamics/spot-sdk/blob/master/python/examples/graph_nav_command_line/recording_command_line.py)   
+3. Save the map files to spot_utils / graph_nav_maps / <your new env name>
+4. Create a file named `metadata.yaml` if one doesn't already exist within the folder
+associated with a map. See below for more details.
+5. Set --spot_graph_nav_map to your new env name.
+
+
+**Converting a map to a pointcloud**
+
+A challenging thing for the metadata is to define the points that yield `allowed-regions`.
+The following workflow is one way to make this relatively easy.
+
+1. Run [this script](https://github.com/boston-dynamics/spot-sdk/tree/master/python/examples/graph_nav_extract_point_cloud) on the pre-made map to yield an output `.ply` pointcloud file.
+[Optional]
+2. Install the [Open3D package](http://www.open3d.org/docs/release/getting_started.html) with `pip install open3d`.
+3. Open up a python interpreter in your terminal, and run the following commands:
+```
+import open3d as o3d
+
+pcd = o3d.io.read_point_cloud("<path to your pointcloud file>")
+o3d.visualization.draw_geometries_with_editing([pcd])
+```
+4. Within the window, do SHIFT + left click on a point to print out its 3D coords to the terminal. Copy the first two (x, y) of all the
+boundary points into the yaml. Note that you can do SHIFT + right click to unselect a point.

exec_plan.py

@@ -23,7 +23,9 @@
     move_hand_to_relative_pose,
     open_gripper,
 )
-from skills.spot_navigation import navigate_to_absolute_pose
+from skills.spot_navigation import (
+    navigate_to_absolute_pose_precise,
+)
 from spot_utils.perception.spot_cameras import capture_images
 from spot_utils.spot_localization import SpotLocalizer
 from spot_utils.utils import (
@@ -88,7 +90,9 @@ def move_to(x_abs: float, y_abs: float, yaw_abs: float) -> None:
     desired_pose = math_helpers.SE2Pose(x=x_abs, y=y_abs, angle=yaw_abs)
     desired_pose_spot = map_to_spot(desired_pose)
     if ROBOT is not None and LOCALIZER is not None:
-        navigate_to_absolute_pose(ROBOT, LOCALIZER, desired_pose_spot)
+        navigate_to_absolute_pose_precise(
+            ROBOT, LOCALIZER, desired_pose_spot, tolerance=0.015
+        )
 
 
 def gaze(direction: str) -> None:
@@ -178,7 +182,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, "z": 0.0, "angle": 0.0}
     with open(args.plan, "r") as plan_file:
         exec(plan_file.read())
     print("done")

skills/spot_navigation.py

@@ -18,7 +18,12 @@
 from bosdyn.client.sdk import Robot
 
 from spot_utils.spot_localization import SpotLocalizer
-from spot_utils.utils import get_graph_nav_dir, get_robot_state, verify_estop
+from spot_utils.utils import (
+    get_graph_nav_dir,
+    get_robot_state,
+    get_se2_distance,
+    verify_estop,
+)
 
 
 def navigate_to_relative_pose(
@@ -106,6 +111,57 @@ def navigate_to_absolute_pose(
     )
 
 
+def navigate_to_absolute_pose_precise(
+    robot: Robot,
+    localizer: SpotLocalizer,
+    target_pose: math_helpers.SE2Pose,
+    max_xytheta_vel: Tuple[float, float, float] = (2.0, 2.0, 1.0),
+    min_xytheta_vel: Tuple[float, float, float] = (-2.0, -2.0, -1.0),
+    timeout: float = 20.0,
+    tolerance: float = 0.05,
+    max_num_tries: int = 5,
+) -> None:
+    """Move to the specific target absolute pose; potentially trying multiple
+    times.
+    """
+    # First, localize the robot.
+    localizer.localize()
+    robot_pose = localizer.get_last_robot_pose()
+    robot_se2 = robot_pose.get_closest_se2_transform()
+    curr_dist_to_target = get_se2_distance(robot_se2, target_pose)
+    curr_tries = 0
+    prev_dist_to_target = float("inf")
+    while curr_dist_to_target > tolerance and max_num_tries > curr_tries:
+        prev_dist_to_target = curr_dist_to_target
+        localizer.localize()
+        navigate_to_absolute_pose(
+            robot, localizer, target_pose, max_xytheta_vel, min_xytheta_vel, timeout
+        )
+        # Re-localize the robot.
+        localizer.localize()
+        robot_pose = localizer.get_last_robot_pose()
+        robot_se2 = robot_pose.get_closest_se2_transform()
+        curr_dist_to_target = get_se2_distance(robot_se2, target_pose)
+        print(curr_dist_to_target)
+        # If the robot is not moving, we should move randomly backwards
+        # to avoid getting stuck.
+        if abs(curr_dist_to_target - prev_dist_to_target) < 0.01:
+            # Move backwards.
+            rel_pose = math_helpers.SE2Pose(x=-0.25, y=0, angle=0)
+            navigate_to_relative_pose(
+                robot, rel_pose, max_xytheta_vel, min_xytheta_vel, timeout
+            )
+        curr_tries += 1
+    if curr_dist_to_target > tolerance:
+        navigate_to_absolute_pose(
+            robot, localizer, target_pose, max_xytheta_vel, min_xytheta_vel, timeout
+        )
+        print(
+            f"Failed to reach the target pose after {curr_tries} tries. "
+            f"Current distance to target: {curr_dist_to_target}"
+        )
+
+
 if __name__ == "__main__":
     # Run this file alone to test manually.
     # Make sure to pass in --spot_robot_ip.

spot_utils/graph_nav_maps/outside-621/edge_snapshots/edge_snapshot_id_ane-syphon-WgF8TQA5by29Onmt7bAc7w==

@@ -0,0 +1,2 @@
+
+4edge_snapshot_id_ane-syphon-WgF8TQA5by29Onmt7bAc7w==

spot_utils/graph_nav_maps/outside-621/edge_snapshots/edge_snapshot_id_carven-angler-6dUv3NnhHWdPOOU36y+eEw==

@@ -0,0 +1,2 @@
+
+7edge_snapshot_id_carven-angler-6dUv3NnhHWdPOOU36y+eEw==

spot_utils/utils.py

@@ -168,3 +168,10 @@ def get_robot_gripper_open_percentage(robot: Robot) -> float:
     """Get the current state of how open the gripper is."""
     robot_state = get_robot_state(robot)
     return float(robot_state.manipulator_state.gripper_open_percentage)
+
+
+def get_se2_distance(pose1: math_helpers.SE2Pose, pose2: math_helpers.SE2Pose) -> float:
+    """Get the Euclidean distance between two SE2Poses, ignoring rotation."""
+    dx = pose2.x - pose1.x
+    dy = pose2.y - pose1.y
+    return (dx * dx + dy * dy) ** 0.5