started env

Author: wmcclinton

predicators/envs/spot_env.py

@@ -3180,6 +3180,76 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
     def _generate_goal_description(self) -> GoalDescription:
         return "open the drawer"
 
+    def _get_dry_task(self, train_or_test: str,
+                      task_idx: int) -> EnvironmentTask:
+        raise NotImplementedError("Dry task generation not implemented.")
+    
+class LISSpotCollectEnv(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",
+            "PickObjectToDrag",
+            "DragToOpenObject",
+            "DragToCloseObject",
+            "PickObjectFromTop",
+            "PlaceObjectOnTop",
+            "DropObjectInside"
+        }
+        self._strips_operators = {op_to_name[o] for o in op_names_to_keep}
+
+    @classmethod
+    def get_name(cls) -> str:
+        return "lis_spot_collect_misplaced_items_env"
+
+    @property
+    def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
+
+        detection_id_to_obj: Dict[ObjectDetectionID, Object] = {}
+
+        blue_block = Object("blue_block", _movable_object_type)
+        blue_block_detection = LanguageObjectDetectionID(
+            "blue block/blue-ish block/blue-green block")
+        detection_id_to_obj[blue_block_detection] = blue_block
+
+        green_cup = Object("yellow_cup", _movable_object_type)
+        green_cup_detection = LanguageObjectDetectionID(
+            "yellow cup/yellow cylinder")
+        detection_id_to_obj[green_cup_detection] = green_cup
+
+        toy_plane = Object("toy_plane", _movable_object_type)
+        toy_plane_detection = LanguageObjectDetectionID(
+            "toy plane")
+        detection_id_to_obj[toy_plane_detection] = toy_plane
+
+        cardboard_box = Object("cardboard_box", _container_type)
+        cardboard_box_detection = LanguageObjectDetectionID(
+            "cardboard box/brown box")
+        detection_id_to_obj[cardboard_box_detection] = cardboard_box
+
+        green_handle = Object("green_handle", _movable_object_type)
+        green_handle_detection = LanguageObjectDetectionID(
+            "green duct tape/green handle/green object")
+        detection_id_to_obj[green_handle_detection] = green_handle
+
+        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 "collect misplaced items"
+
     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

@@ -304,7 +304,8 @@ def get_env_names(cls) -> Set[str]:
             "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", "lis_spot_block_drawer_env"
+            "spot_brush_shelf_env", "lis_spot_block_floor_env", "lis_spot_block_drawer_env",
+            "lis_spot_collect_misplaced_items_env"
         }
 
     @staticmethod

predicators/ground_truth_models/spot_env/options.py

@@ -1034,6 +1034,7 @@ def get_env_names(cls) -> Set[str]:
             "spot_brush_shelf_env",
             "lis_spot_block_floor_env",
             "lis_spot_block_drawer_env",
+            "lis_spot_collect_misplaced_items_env"
         }
 
     @classmethod

predicators/perception/spot_perceiver.py

@@ -479,6 +479,19 @@ def _create_goal(self, state: State,
             handle = Object("green_handle", _movable_object_type)
             NotOpen = pred_name_to_pred["NotOpen"]
             return {GroundAtom(NotOpen, [handle])}
+        if goal_description == "collect misplaced items":
+            robot = Object("robot", _robot_type)
+            handle = Object("green_handle", _movable_object_type)
+            blue_block = Object("blue_block", _movable_object_type)
+            yellow_cup = Object("yellow_cup", _movable_object_type)
+            toy_plane = Object("toy_plane", _movable_object_type)
+            cardboard_box = Object("cardboard_box", _container_type)
+            Inside = pred_name_to_pred["Inside"]
+            return {
+                GroundAtom(Inside, [blue_block, cardboard_box]),
+                GroundAtom(Inside, [yellow_cup, cardboard_box]),
+                GroundAtom(Inside, [toy_plane, cardboard_box]),
+            }
         if goal_description == "setup sweeping":
             robot = Object("robot", _robot_type)
             brush = Object("brush", _movable_object_type)

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

@@ -60,4 +60,38 @@ static-object-features:
     length: 0.1
     width: 0.1
     placeable: 0
-    is_sweeper: 0
+    is_sweeper: 0
+  yellow_cup:
+    shape: 2
+    height: 0.1
+    length: 0.1
+    width: 0.1
+    placeable: 1
+    is_sweeper: 0
+  toy_plane:
+    shape: 2
+    height: 0.1
+    length: 0.1
+    width: 0.1
+    placeable: 1
+    is_sweeper: 0
+  cardboard_box:
+    shape: 1
+    height: 0.05
+    width: 0.3
+    length: 0.4
+    placeable: 1
+    is_sweeper: 0
+    # Important for TopAbove that the bucket appears to always be on the ground
+    z: -0.15
+    # Rotation can't be detected.
+    qw: 1
+    qx: 0
+    qy: 0
+    qz: 0
+# This should be calculable, but I'm lazy. These values represent the SE2 pose
+# (in the table frame) for the robot before placing the container.
+prepare_container_relative_xy:
+  dx: -0.8
+  dy: -0.6
+  angle: 0.0

run_predicates.py

@@ -0,0 +1,74 @@
+import numpy as np
+import json
+from predicators.envs.spot_env import LISSpotCollectEnv
+from predicators.perception.spot_perceiver import SpotPerceiver
+from predicators.ground_truth_models import get_gt_nsrts, get_gt_options
+from predicators.structs import Object, State, GroundAtom
+from predicators.spot_utils.utils import _container_type, _movable_object_type, _robot_type
+from predicators import utils
+
+utils.reset_config({
+    "env":
+    "lis_spot_collect_misplaced_items_env",
+    "approach":
+    "spot_wrapper[oracle]",
+    "num_train_tasks":
+    0,
+    "num_test_tasks":
+    1,
+    "seed":
+    0,
+    "spot_run_dry":
+    True,
+    "spot_robot_ip":
+    None,
+    "spot_graph_nav_map":
+    "b45-621",
+    "bilevel_plan_without_sim":
+    True,
+    "perceiver":
+    "spot_perceiver",
+    "spot_use_perfect_samplers":
+    True,
+})
+
+rng = np.random.default_rng(123)
+env = LISSpotCollectEnv()
+perceiver = SpotPerceiver()
+nsrts = get_gt_nsrts(env.get_name(), env.predicates,
+                        get_gt_options(env.get_name()))
+
+pred_name_to_pred = {p.name: p for p in env.predicates}
+
+robot = Object("robot", _robot_type)
+handle = Object("green_handle", _movable_object_type)
+blue_block = Object("blue_block", _movable_object_type)
+yellow_cup = Object("yellow_cup", _movable_object_type)
+toy_plane = Object("toy_plane", _movable_object_type)
+cardboard_box = Object("cardboard_box", _container_type)
+Inside = pred_name_to_pred["Inside"]
+
+# state from json
+state = State({}, simulator_state=None)
+
+# get all grounded atoms
+all_grounded_atoms = None
+
+json_file = "/Users/shashlik/Desktop/last.json"
+with open(json_file, "r", encoding="utf-8") as f:
+    json_dict = json.load(f)
+object_name_to_object = env._parse_object_name_to_object_from_json(
+    json_dict)
+init_dict = env._parse_init_state_dict_from_json(
+    json_dict, object_name_to_object)
+for i, (obj, init_val) in enumerate(sorted(init_dict.items())):
+            init_val["object_id"] = i
+state = utils.create_state_from_dict(init_dict)
+
+a = GroundAtom(Inside, [blue_block, cardboard_box])
+print(a, a.holds(state))
+
+import ipdb; ipdb.set_trace()
+
+for a in all_grounded_atoms:
+    a.holds(state)