Merge pull request #281 from bdaiinstitute/lis-spot/add-lis-spot-test…

…-env

add LIS spot test env

predicators/envs/spot_env.py

@@ -3027,3 +3027,55 @@ def _generate_goal_description(self) -> GoalDescription:
     def _get_dry_task(self, train_or_test: str,
                       task_idx: int) -> EnvironmentTask:
         raise NotImplementedError("Dry task generation not implemented.")
+
+
+###############################################################################
+#                             LIS Test Block Floor                            #
+###############################################################################
+
+
+class LISSpotBlockFloorEnv(SpotRearrangementEnv):
+    """An extremely basic environment where a block needs to be picked up and
+    is specifically used for testing in the LIS Spot room.
+
+    Very simple and mostly just for testing.
+    """
+
+    def __init__(self, use_gui: bool = True) -> None:
+        super().__init__(use_gui)
+
+        op_to_name = {o.name: o for o in _create_operators()}
+        op_names_to_keep = {
+            "MoveToReachObject",
+            "MoveToHandViewObject",
+            "PickObjectFromTop",
+            "PlaceObjectOnTop",
+        }
+        self._strips_operators = {op_to_name[o] for o in op_names_to_keep}
+
+    @classmethod
+    def get_name(cls) -> str:
+        return "lis_spot_block_floor_env"
+
+    @property
+    def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
+
+        detection_id_to_obj: Dict[ObjectDetectionID, Object] = {}
+
+        red_block = Object("red_block", _movable_object_type)
+        red_block_detection = LanguageObjectDetectionID(
+            "red block/orange block/yellow block")
+        detection_id_to_obj[red_block_detection] = red_block
+
+        for obj, pose in get_known_immovable_objects().items():
+            detection_id = KnownStaticObjectDetectionID(obj.name, pose)
+            detection_id_to_obj[detection_id] = obj
+
+        return detection_id_to_obj
+
+    def _generate_goal_description(self) -> GoalDescription:
+        return "pick up the red block"
+
+    def _get_dry_task(self, train_or_test: str,
+                      task_idx: int) -> EnvironmentTask:
+        raise NotImplementedError("Dry task generation not implemented.")

predicators/ground_truth_models/spot_env/nsrts.py

@@ -285,14 +285,10 @@ class SpotEnvsGroundTruthNSRTFactory(GroundTruthNSRTFactory):
     @classmethod
     def get_env_names(cls) -> Set[str]:
         return {
-            "spot_cube_env",
-            "spot_soda_floor_env",
-            "spot_soda_table_env",
-            "spot_soda_bucket_env",
-            "spot_soda_chair_env",
-            "spot_main_sweep_env",
-            "spot_ball_and_cup_sticky_table_env",
-            "spot_brush_shelf_env",
+            "spot_cube_env", "spot_soda_floor_env", "spot_soda_table_env",
+            "spot_soda_bucket_env", "spot_soda_chair_env",
+            "spot_main_sweep_env", "spot_ball_and_cup_sticky_table_env",
+            "spot_brush_shelf_env", "lis_spot_block_floor_env"
         }
 
     @staticmethod

predicators/ground_truth_models/spot_env/options.py

@@ -276,11 +276,13 @@ def _move_to_target_policy(name: str, distance_param_idx: int,
                            target_obj_idx: int, do_gaze: bool, state: State,
                            memory: Dict, objects: Sequence[Object],
                            params: Array) -> Action:
-
     del memory  # not used
 
     robot, localizer, _ = get_robot()
-    sim_robot = get_simulated_robot()
+    if not CFG.bilevel_plan_without_sim:
+        sim_robot = get_simulated_robot()
+    else:
+        sim_robot = None
 
     distance = params[distance_param_idx]
     yaw = params[yaw_param_idx]
@@ -298,16 +300,28 @@ def _move_to_target_policy(name: str, distance_param_idx: int,
                                     target_pose.z + target_height / 2)
     fn = navigate_to_relative_pose_and_gaze
     fn_args = (robot, rel_pose, localizer, gaze_target)
-    sim_fn: Callable = simulated_navigate_to_relative_pose_and_gaze
-    sim_fn_args: Tuple = (sim_robot,
-                          robot_pose.get_closest_se2_transform() * rel_pose,
-                          gaze_target)
+
+    if not CFG.bilevel_plan_without_sim:
+        sim_fn: Callable = simulated_navigate_to_relative_pose_and_gaze
+        sim_fn_args: Tuple = (sim_robot,
+                              robot_pose.get_closest_se2_transform() *
+                              rel_pose, gaze_target)
+    else:
+        sim_fn = lambda _: None
+        sim_fn_args = ()
+
     if not do_gaze:
         fn = navigate_to_relative_pose  # type: ignore
         fn_args = (robot, rel_pose)  # type: ignore
-        sim_fn = simulated_navigate_to_relative_pose
-        sim_fn_args = (sim_robot,
-                       robot_pose.get_closest_se2_transform() * rel_pose)
+
+        if not CFG.bilevel_plan_without_sim:
+            sim_fn = simulated_navigate_to_relative_pose
+            sim_fn_args = (sim_robot,
+                           robot_pose.get_closest_se2_transform() * rel_pose)
+        else:
+            sim_fn = lambda _: None
+            sim_fn_args = ()
+
     action_extra_info = SpotActionExtraInfo(name, objects, fn, fn_args, sim_fn,
                                             sim_fn_args)
     return utils.create_spot_env_action(action_extra_info)
@@ -323,11 +337,18 @@ def _grasp_policy(name: str,
     del memory  # not used
 
     robot, _, _ = get_robot()
-    sim_robot = get_simulated_robot()
+    if not CFG.bilevel_plan_without_sim:
+        sim_robot = get_simulated_robot()
+    else:
+        sim_robot = None
+
     assert len(params) == 6
     pixel = (int(params[0]), int(params[1]))
     target_obj = objects[target_obj_idx]
-    sim_target_obj = get_simulated_object(target_obj)
+    if not CFG.bilevel_plan_without_sim:
+        sim_target_obj = get_simulated_object(target_obj)
+    else:
+        sim_target_obj = None
 
     # Special case: if we're running dry, the image won't be used.
     if CFG.spot_run_dry:
@@ -343,11 +364,12 @@ def _grasp_policy(name: str,
         grasp_rot = math_helpers.Quat(params[2], params[3], params[4],
                                       params[5])
     # If the target object is reasonably large, don't try to stow!
-    target_obj_volume = state.get(target_obj, "height") * \
-        state.get(target_obj, "length") * state.get(target_obj, "width")
+    target_obj_volume = (state.get(target_obj, "height") *
+                         state.get(target_obj, "length") *
+                         state.get(target_obj, "width"))
 
     do_stow = not do_dump and \
-        target_obj_volume < CFG.spot_grasp_stow_volume_threshold
+              target_obj_volume < CFG.spot_grasp_stow_volume_threshold
     fn = _grasp_at_pixel_and_maybe_stow_or_dump
     sim_fn = None  # NOTE: cannot simulate using this option, so this
     # shouldn't be called anyways...
@@ -374,7 +396,6 @@ def _sweep_objects_into_container_policy(name: str, robot_obj_idx: int,
                                          memory: Dict,
                                          objects: Sequence[Object],
                                          params: Array) -> Action:
-
     del memory  # not used
 
     robot, _, _ = get_robot()
@@ -545,13 +566,23 @@ def _sim_safe_pick_object_from_top_policy(state: State, memory: Dict,
     name = "SimSafePickObjectFromTop"
     target_obj_idx = 1
     robot, _, _ = get_robot()
-    sim_robot = get_simulated_robot()
+    if not CFG.bilevel_plan_without_sim:
+        sim_robot = get_simulated_robot()
+    else:
+        sim_robot = None
+
     fn = _sim_safe_grasp_at_pixel_and_maybe_stow_or_dump
     fn_args = (robot, objects[target_obj_idx], _options_rng, 10.0, True, True,
                False)
-    sim_fn = simulated_grasp_at_pixel
-    sim_target_obj = get_simulated_object(objects[target_obj_idx])
-    sim_fn_args = (sim_robot, sim_target_obj)
+
+    if not CFG.bilevel_plan_without_sim:
+        sim_fn: Callable = simulated_grasp_at_pixel
+        sim_target_obj = get_simulated_object(objects[target_obj_idx])
+        sim_fn_args: Tuple = (sim_robot, sim_target_obj)
+    else:
+        sim_fn = lambda _: None
+        sim_fn_args = ()
+
     action_extra_info = SpotActionExtraInfo(name, objects, fn, fn_args, sim_fn,
                                             sim_fn_args)
     return utils.create_spot_env_action(action_extra_info)
@@ -793,7 +824,6 @@ def _sweep_into_container_policy(state: State, memory: Dict,
 def _sweep_two_objects_into_container_policy(state: State, memory: Dict,
                                              objects: Sequence[Object],
                                              params: Array) -> Action:
-
     name = "SweepTwoObjectsIntoContainer"
     robot_obj_idx = 0
     target_obj_idxs = {2, 3}
@@ -965,6 +995,7 @@ def get_env_names(cls) -> Set[str]:
             "spot_main_sweep_env",
             "spot_ball_and_cup_sticky_table_env",
             "spot_brush_shelf_env",
+            "lis_spot_block_floor_env",
         }
 
     @classmethod

predicators/perception/spot_perceiver.py

@@ -459,6 +459,11 @@ def _create_goal(self, state: State,
             return {
                 GroundAtom(Holding, [robot, brush]),
             }
+        if goal_description == "pick up the red block":
+            robot = Object("robot", _robot_type)
+            block = Object("red_block", _movable_object_type)
+            Holding = pred_name_to_pred["Holding"]
+            return {GroundAtom(Holding, [robot, block])}
         if goal_description == "setup sweeping":
             robot = Object("robot", _robot_type)
             brush = Object("brush", _movable_object_type)

predicators/spot_utils/README.md

@@ -1,5 +1,41 @@
 # Spot Utils
 
+## How to run your own Spot environment
+
+> Last Updated: 04/11/2024
+
+**Steps:**
+- Set up the codebase, perception pipeline, and Spot
+  - You need to have access to a GPU server for the perception pipeline (e.g., the Detic-SAM pipeline)
+  - You need to connect to Spot (through WiFi or ethernet cable). The Spot at LIS uses its own WiFi AP mode and is on IP `192.168.80.3`.
+  - To connect to both Spot and GPU server, our current solution is to use WiFi for Spot and ethernet cable for the GPU server.
+- Create a new map of the environment: See the `Mapping` section.
+  - Prepare the metadata file: See the `Prepare Metadata` section.
+- Implement your task
+- Start actual run. Examples:
+```
+# template
+python predicators/main.py --spot_robot_ip <spot_ip> --spot_graph_nav_map <map_name> --env <env_name>
+
+# an example to run LIS Spot
+python predicators/main.py --spot_robot_ip 192.168.80.3 --spot_graph_nav_map b45-621 --env lis_spot_block_floor_env --approach spot_wrapper[oracle] --bilevel_plan_without_sim True --seed 0
+```
+
+### Implement Your Task
+
+To create a simple task before you can run, you only need to:
+
+- Create a new environment in `envs/spot_envs.py`
+  - In `spot_env.py`, subclass the `SpotRearrangementEnv` and define the necessary methods needed to override.
+  - The simplest possible example is `SpotSodaFloorEnv` (and maybe you can directly use this!).
+  - Doing this involves selecting some operators that you'll need.
+- Add ground truth model
+  - Add environment name into `SpotEnvsGroundTruthNSRTFactory`  in `ground_truth_models/spot_env/nsrt.py`
+  - Add environment name into `SpotEnvsGroundTruthOptionFactory` in `ground_truth_models/spot_env/options.py`
+- If you want, define a new `goal_description` string. Then, go to the _`create_goal` function of `spot_perceiver.py` and follow the example to convert a goal description string into an actual set of atoms needed to implement the goal.
+
+
+
 ## Mapping
 > Last Updated: 11/14/2023
 
@@ -15,17 +51,31 @@ To create a new map of a new environment:
    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.
+associated with a map. See below for more details.
 5. Set --spot_graph_nav_map to your new env name.
 
-### Obtaining points for the `allowed-regions` in the metadata
+
+### Prepare Metadata
+
+The metadata file is a yaml file that contains information about the map and is used by the codebase to make decisions about the environment. 
+See `predicators/spot_utils/graph_nav_maps/floor8-v2/metadata.yaml` or `predicators/spot_utils/graph_nav_maps/floor8-sweeping/metadata.yaml` for an example and
+explanation(s) of the various fields in the metadata file.
+
+**Specifying the following required fields**
+
+- `spot-home-pose`: a place in the room from which most of the room is visible and the robot can execute its object finding procedure.
+- `allowed-regions`: these are (x,y) points that define 4 corners of a region that the robot will be allowed to be in. This is to prevent it from trying to navigate into a wall, or outside a door. In the case of 621, you should basically just put in the 4 corners of the room. _See below for a note._
+- `known-immovable-objects`. These are the x, y, z positions of objects that the robot cannot manipulate (e.g. the floor). You'll probably want to add the floor and or any big tables in the room
+- `static-object-features`. These are some hand-defined features for various objects that you might want to use (e.g., the object shape, width, length, height, etc.).
+
+
+
+**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.
+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:
 ```

predicators/spot_utils/graph_nav_maps/b45-621/edge_snapshots/edge_snapshot_id_afoul-eel-VMlsWuUtsMDVqYYD2cUO2A==

@@ -0,0 +1,2 @@
+
+3edge_snapshot_id_afoul-eel-VMlsWuUtsMDVqYYD2cUO2A==

predicators/spot_utils/graph_nav_maps/b45-621/edge_snapshots/edge_snapshot_id_molar-fly-HtkxrGlYKQxmU0CprZ9pkw==

@@ -0,0 +1,2 @@
+
+3edge_snapshot_id_molar-fly-HtkxrGlYKQxmU0CprZ9pkw==

predicators/spot_utils/graph_nav_maps/b45-621/edge_snapshots/edge_snapshot_id_posh-pike-R3QBi9scZbjiZUBOpJI0.A==

@@ -0,0 +1,2 @@
+
+3edge_snapshot_id_posh-pike-R3QBi9scZbjiZUBOpJI0.A==

predicators/spot_utils/graph_nav_maps/b45-621/metadata.yaml

@@ -0,0 +1,49 @@
+# Additional info associated with the map.
+---
+# NOTE: close to the center of the room
+#spot-home-pose:
+#  x: 2.85
+#  y: 0.0
+#  angle: -1.45769
+# NOTE: a place closer to the corner that the cable can reach - easier for debug
+spot-home-pose:
+  x: 3.5
+  y: 0.45
+  angle: 0.
+april-tag-offsets: []
+
+# 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:
+  spot-room:
+    - [0.25, 1.9]
+    - [1.2, -2.5]
+    - [5.7, -1.5]
+    - [5.0, 2.7]
+
+# Known immovable objects. Assuming default rotations.
+# TODO note: later we can add static objects like tables, chairs, etc. here
+known-immovable-objects:
+  floor:
+    x: 1.4
+    y: 0.5
+    z: -0.5
+
+# Static object features, including the shapes and sizes of known objects.
+static-object-features:
+  floor:
+    shape: 1
+    height: 0.0001
+    length: 10000000  # effectively infinite
+    width: 10000000
+    flat_top_surface: 1
+  red_block:
+    shape: 2
+    height: 0.1
+    length: 0.1
+    width: 0.1
+    placeable: 1
+    is_sweeper: 0

predicators/spot_utils/perception/object_detection.py

@@ -274,7 +274,7 @@ def _query_detic_sam(
     # Create buffer dictionary to send to server.
     buf_dict = {}
     for camera_name, rgbd in rgbds.items():
-        pil_rotated_img = PIL.Image.fromarray(rgbd.rotated_rgb)
+        pil_rotated_img = PIL.Image.fromarray(rgbd.rotated_rgb)  # type: ignore
         buf_dict[camera_name] = _image_to_bytes(pil_rotated_img)
 
     # Extract all the classes that we want to detect.