From eb529493cab60abb4331b4ce67dbd1c870e97e38 Mon Sep 17 00:00:00 2001 From: NishanthJKumar Date: Tue, 14 Nov 2023 17:33:47 -0500 Subject: [PATCH] WIP; need to get integration test running --- predicators/envs/spot_env.py | 13 +++++++ predicators/ground_truth_models/spot/nsrts.py | 22 ++++++++++- predicators/spot_utils/README.md | 37 +++++++++++++++++++ .../graph_nav_maps/floor8-v2/metadata.yaml | 16 ++++++++ predicators/spot_utils/integration_tests.py | 31 +++++++++------- predicators/spot_utils/spot_localization.py | 12 +----- predicators/spot_utils/utils.py | 11 ++++-- 7 files changed, 113 insertions(+), 29 deletions(-) diff --git a/predicators/envs/spot_env.py b/predicators/envs/spot_env.py index 3b28506619..695710b83d 100644 --- a/predicators/envs/spot_env.py +++ b/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.""" diff --git a/predicators/ground_truth_models/spot/nsrts.py b/predicators/ground_truth_models/spot/nsrts.py index aeed5cdefa..e9f7019a49 100644 --- a/predicators/ground_truth_models/spot/nsrts.py +++ b/predicators/ground_truth_models/spot/nsrts.py @@ -1,11 +1,11 @@ """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, \ @@ -13,6 +13,22 @@ 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]) diff --git a/predicators/spot_utils/README.md b/predicators/spot_utils/README.md index 42da5dc105..c5b04a60c6 100644 --- a/predicators/spot_utils/README.md +++ b/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 / +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("") +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 diff --git a/predicators/spot_utils/graph_nav_maps/floor8-v2/metadata.yaml b/predicators/spot_utils/graph_nav_maps/floor8-v2/metadata.yaml index 06e756700e..e8597df4f1 100644 --- a/predicators/spot_utils/graph_nav_maps/floor8-v2/metadata.yaml +++ b/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: diff --git a/predicators/spot_utils/integration_tests.py b/predicators/spot_utils/integration_tests.py index 74e7bae634..26ee505bd9 100644 --- a/predicators/spot_utils/integration_tests.py +++ b/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() diff --git a/predicators/spot_utils/spot_localization.py b/predicators/spot_utils/spot_localization.py index e3ff7ccf51..298d43782c 100644 --- a/predicators/spot_utils/spot_localization.py +++ b/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 / -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 diff --git a/predicators/spot_utils/utils.py b/predicators/spot_utils/utils.py index a0b0b18b6d..635acc8ba6 100644 --- a/predicators/spot_utils/utils.py +++ b/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