WIP; need to get integration test running

predicators/envs/spot_env.py

@@ -9,6 +9,7 @@
 from pathlib import Path
 from typing import Callable, Dict, Iterator, List, Optional, Sequence, Set, \
     Tuple
+import scipy
 
 import matplotlib
 import numpy as np
@@ -177,6 +178,18 @@ def __init__(self, use_gui: bool = True) -> None:
         # Create constant objects.
         self._spot_object = Object("robot", _robot_type)
 
+        # DEBUGGING!
+        metadata = load_spot_metadata()
+        allowed_regions = metadata.get("allowed-regions", {})
+        convex_hulls = []
+        for region_pts in allowed_regions.values():
+            dealunay_hull = scipy.spatial.Delaunay(np.array(region_pts))
+            convex_hulls.append(dealunay_hull)
+        # We can simply test if a point is inside the hull or not with
+        # hull.find_simplex(np.array(point)) >= 0
+        # Next step: put this in an integration test!
+        import ipdb; ipdb.set_trace()
+
     @property
     def strips_operators(self) -> Set[STRIPSOperator]:
         """Expose the STRIPSOperators for use by oracles."""

predicators/ground_truth_models/spot/nsrts.py

@@ -1,18 +1,34 @@
 """Ground-truth NSRTs for the PDDLEnv."""
 
-from typing import Dict, Sequence, Set
+from typing import Dict, Sequence, Set, Tuple
 
 import numpy as np
 
 from predicators.envs import get_or_create_env
-from predicators.envs.spot_env import SpotRearrangementEnv
+from predicators.envs.spot_env import SpotRearrangementEnv, _object_to_top_down_geom
 from predicators.ground_truth_models import GroundTruthNSRTFactory
 from predicators.spot_utils.utils import get_spot_home_pose
 from predicators.structs import NSRT, Array, GroundAtom, NSRTSampler, Object, \
     ParameterizedOption, Predicate, State, Type
 from predicators.utils import null_sampler
 
 
+def _sample_collision_free_robot_point(
+        state: State, rng: np.random.Generator, target_obj: Object,
+        min_distance_from_obj: float) -> Tuple[float, float]:
+    """Uses known object geometries to find collision-free locations for the
+    robot to navigate to.
+
+    NOTE: this currently is not aware of the bounds of the map; it's only
+    aware of the objects in it.
+    """
+    # TODO: use the "sample_move_offset_from_target" function from
+    # utils.py. This might actually preclude the need for this current
+    # function.
+
+    pass
+
+
 def _move_to_body_view_object_sampler(state: State, goal: Set[GroundAtom],
                                       rng: np.random.Generator,
                                       objs: Sequence[Object]) -> Array:
@@ -66,6 +82,8 @@ def _move_to_reach_object_sampler(state: State, goal: Set[GroundAtom],
     if len(objs) == 2 and objs[1].name == "cup":
         approach_angle = home_pose.angle - (np.pi / 2)
 
+    collision_geoms = [_object_to_top_down_geom(obj) for obj in set(state)]
+
     # NOTE: closer than move_to_view. Important for placing.
     return np.array([0.8, approach_angle])
 

predicators/spot_utils/README.md

@@ -1,5 +1,42 @@
 # Spot Utils
 
+## Mapping
+> Last Updated: 11/14/2023
+
+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. Populate this with details such as a `spot-home-pose`, etc.
+See `predicators/spot_utils/graph_nav_maps/floor8-v2/metadata.yaml` for an example and
+explanation(s) of the various fields in the metadata file.
+5. Set --spot_graph_nav_map to your new env name.
+
+### Obtaining points for the `allowed-regions` in the metadata
+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.
+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.
+
 ## Perception
 
 > Last updated: 07/17/2023

predicators/spot_utils/graph_nav_maps/floor8-v2/metadata.yaml

@@ -16,6 +16,22 @@ april-tag-offsets:
     x: 0.25
     y: -0.1
     z: -0.2
+# Allowed regions. Each region is defined by a set of points
+# that form the boundary of the region. We will check
+# whether a pose is within the region by checking whether the
+# robot will be within the convex hull of these boundary
+# points.
+allowed-regions:
+  main-room:
+    - [3.6, -1.3]
+    - [0.16, -1.3]
+    - [0.069, 1.8]
+    - [3.7, 2.3]
+  hallway:
+    - [-2.1, 1.9]
+    - [-1.8, -3.7]
+    - [-0.3, 2.1]
+    - [-0.49, -3.6]
 # Known immovable objects. Assuming default rotations.
 known-immovable-objects:
   floor:

predicators/spot_utils/integration_tests.py

@@ -3,6 +3,7 @@
 Run with --spot_robot_ip and any other flags.
 """
 from typing import Optional
+import scipy
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -34,7 +35,7 @@
     DEFAULT_HAND_LOOK_FLOOR_POSE, get_graph_nav_dir, \
     get_relative_se2_from_se3, get_spot_home_pose, \
     sample_move_offset_from_target, spot_pose_to_geom2d, verify_estop
-
+from predicators.envs.spot_env import load_spot_metadata
 
 def test_find_move_pick_place(
     robot: Robot,
@@ -181,8 +182,6 @@ def test_move_with_sampling() -> None:
     """Test for moving to a surface with a sampled rotation and distance,
     taking into account potential collisions with walls and other surfaces."""
 
-    # Approximate values for the set up on the fourth floor.
-    room_bounds = (0.4, -1.0, 4.0, 2.0)  # min x, min y, max x, max y
     surface_radius = 0.2
 
     num_samples = 10
@@ -208,6 +207,12 @@ def test_move_with_sampling() -> None:
                                      return_at_exit=True)
     assert path.exists()
     localizer = SpotLocalizer(robot, path, lease_client, lease_keepalive)
+    metadata = load_spot_metadata()
+    allowed_regions = metadata.get("allowed-regions", {})
+    convex_hulls = []
+    for region_pts in allowed_regions.values():
+        dealunay_hull = scipy.spatial.Delaunay(np.array(region_pts))
+        convex_hulls.append(dealunay_hull)
 
     # Run test with april tag cube.
     surface1 = AprilTagObjectDetectionID(408)
@@ -240,11 +245,11 @@ def test_move_with_sampling() -> None:
             collision_geoms,
             rng,
             max_distance=max_distance,
-            room_bounds=room_bounds,
+            allowed_regions=convex_hulls,
         )
         # Visualize everything.
-        figsize = (1.1 * (room_bounds[2] - room_bounds[0]),
-                   1.1 * (room_bounds[3] - room_bounds[1]))
+        # figsize = (1.1 * (room_bounds[2] - room_bounds[0]),
+        #            1.1 * (room_bounds[3] - room_bounds[1]))
         _, ax = plt.subplots(1, 1, figsize=figsize)
         robot_geom.plot(ax, facecolor="lightgreen", edgecolor="black")
         # Draw the origin of the robot, which should be the back right leg.
@@ -271,11 +276,11 @@ def test_move_with_sampling() -> None:
                            color="gold",
                            zorder=3)
         # Draw the walls.
-        min_x, min_y, max_x, max_y = room_bounds
-        ax.plot((min_x, min_x), (min_y, max_y), linestyle="--", color="gray")
-        ax.plot((max_x, max_x), (min_y, max_y), linestyle="--", color="gray")
-        ax.plot((min_x, max_x), (min_y, min_y), linestyle="--", color="gray")
-        ax.plot((min_x, max_x), (max_y, max_y), linestyle="--", color="gray")
+        # min_x, min_y, max_x, max_y = room_bounds
+        # ax.plot((min_x, min_x), (min_y, max_y), linestyle="--", color="gray")
+        # ax.plot((max_x, max_x), (min_y, max_y), linestyle="--", color="gray")
+        # ax.plot((min_x, max_x), (min_y, min_y), linestyle="--", color="gray")
+        # ax.plot((min_x, max_x), (max_y, max_y), linestyle="--", color="gray")
         plt.savefig(f"sampling_integration_test_{i}.png")
 
         # Execute the move.
@@ -429,6 +434,6 @@ def test_repeated_brush_bucket_dump_pick_place(
 
 
 if __name__ == "__main__":
-    test_all_find_move_pick_place()
+    # test_all_find_move_pick_place()
     test_move_with_sampling()
-    test_repeated_brush_bucket_dump_pick_place()
+    # test_repeated_brush_bucket_dump_pick_place()

predicators/spot_utils/spot_localization.py

@@ -4,16 +4,8 @@
 between runs. That way, absolute positions of objects that are saved in task
 files remain accurate, even when starting the robot from a different location.
 
-Before using this interface:
-
-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. Set --spot_graph_nav_map to your new env name.
+Before using this interface, follow instructions from the 'Mapping' section
+of the README file under the spot_utils folder.
 """
 
 import logging

predicators/spot_utils/utils.py

@@ -14,6 +14,7 @@
 from bosdyn.client.robot_state import RobotStateClient
 from bosdyn.client.sdk import Robot
 from numpy.typing import NDArray
+import scipy
 
 from predicators.settings import CFG
 from predicators.utils import Rectangle, _Geom2D
@@ -143,14 +144,13 @@ def sample_move_offset_from_target(
         collision_geoms: Collection[_Geom2D],
         rng: np.random.Generator,
         max_distance: float,
-        room_bounds: Tuple[float, float, float, float],
+        allowed_regions: Collection[scipy.spatial.Delaunay],
         max_samples: int = 100) -> Tuple[float, float, Rectangle]:
     """Sampler for navigating to a target object.
 
     Returns a distance and an angle in radians. Also returns the next
     robot geom for visualization and debugging convenience.
     """
-    min_x, min_y, max_x, max_y = room_bounds
     for _ in range(max_samples):
         distance = rng.uniform(0.0, max_distance)
         angle = rng.uniform(-np.pi, np.pi)
@@ -165,8 +165,11 @@ def sample_move_offset_from_target(
         # Check for out-of-bounds.
         oob = False
         for cx, cy in cand_geom.vertices:
-            if cx < min_x or cy < min_y or cx > max_x or cy > max_y:
-                oob = True
+            for region in allowed_regions:
+                if region.find_simplex(np.array([cx, cy])) < 0:
+                    oob = True
+                    break
+            if oob:
                 break
         if oob:
             continue