yayay! mapping works!

Author: NishanthJKumar

predicators/approaches/bilevel_planning_approach.py

@@ -74,8 +74,7 @@ def _solve(self, task: Task, timeout: int) -> Callable[[State], Action]:
 
             for act in nsrt_plan:
                 print(act)
-            import ipdb
-            ipdb.set_trace()
+            import ipdb; ipdb.set_trace()
 
         # Run full bilevel planning.
         else:

predicators/envs/spot_env.py

@@ -39,7 +39,8 @@
 from predicators.spot_utils.skills.spot_navigation import go_home, \
     navigate_to_absolute_pose
 from predicators.spot_utils.skills.spot_stow_arm import stow_arm
-from predicators.spot_utils.spot_localization import SpotLocalizer
+from predicators.spot_utils.spot_localization import OdomLocalizer, \
+    SpotLocalizer
 from predicators.spot_utils.utils import _base_object_type, _broom_type, \
     _container_type, _cup_type, _dustpan_type, _immovable_object_type, \
     _juicer_type, _movable_object_type, _robot_type, _table_type, \
@@ -176,7 +177,6 @@ def get_robot(
         return None, None, None
     setup_logging(False)
     hostname = CFG.spot_robot_ip
-    path = get_graph_nav_dir()
     sdk = create_standard_sdk("PredicatorsClient-")
     robot = sdk.create_robot(hostname)
     authenticate(robot)
@@ -187,9 +187,14 @@ def get_robot(
                                      must_acquire=True,
                                      return_at_exit=True)
     localizer = None
-    assert path.exists()
     if use_localizer:
-        localizer = SpotLocalizer(robot, path, lease_client, lease_keepalive)
+        if CFG.spot_mapless_mode:
+            localizer = OdomLocalizer(robot)
+        else:
+            path = get_graph_nav_dir()
+            assert path.exists()
+            localizer = SpotLocalizer(robot, path, lease_client,
+                                      lease_keepalive)
     return robot, localizer, lease_client
 
 
@@ -1178,25 +1183,28 @@ def _actively_construct_initial_object_views(
         assert self._localizer is not None
         self._localizer.localize()
 
-        # Capture images from the current position and run detection
-        # without moving the robot.
-        # rgbds = capture_images(self._robot, self._localizer)
-        # detection_ids = set(self._detection_id_to_obj.keys())
-        # detections, artifacts = detect_objects(
-        #     detection_ids, rgbds, self._allowed_regions)
-
-        # if CFG.spot_render_perception_outputs:
-        #     outdir = Path(CFG.spot_perception_outdir)
-        #     time_str = time.strftime("%Y%m%d-%H%M%S")
-        #     detections_outfile = outdir / f"detections_{time_str}.png"
-        #     no_detections_outfile = outdir / f"no_detections_{time_str}.png"
-        #     visualize_all_artifacts(artifacts, detections_outfile,
-        #                             no_detections_outfile)
-
-        # Detection is currently disabled (commented out above), so default
-        # to empty so the fallback to known poses below still works.
-        detections: Dict[ObjectDetectionID, math_helpers.SE3Pose] = {}
-        artifacts: Dict[str, Any] = {}
+        if CFG.spot_mapless_mode:
+            # In mapless mode, we must actually run object detection since
+            # there are no metadata poses to fall back on. Use the init
+            # search procedure which spins the robot to find objects.
+            detection_ids = set(self._detection_id_to_obj.keys())
+            # Filter out KnownStaticObjectDetectionIDs (e.g. floor) since
+            # those don't need searching.
+            search_ids = {
+                d for d in detection_ids
+                if not isinstance(d, KnownStaticObjectDetectionID)
+            }
+            detections, artifacts = self._run_init_search_for_objects(
+                search_ids)
+            # Also include known static objects (e.g. floor).
+            for d in detection_ids:
+                if isinstance(d, KnownStaticObjectDetectionID):
+                    detections[d] = d.pose
+        else:
+            # Detection is currently disabled for map-based mode; fall back
+            # to known poses from metadata below.
+            detections = {}
+            artifacts = {}
 
         obj_to_se3_pose = {
             self._detection_id_to_obj[det_id]: val
@@ -4422,49 +4430,50 @@ def get_name(cls) -> str:
     @property
     def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
         detection_id_to_obj: Dict[ObjectDetectionID, Object] = {}
-        # detection_id_to_obj[LanguageObjectDetectionID(
-        #     "bottle/clear_cup/clear_trashcan")] = Object(
-        #         "clear_plastic_container", _trash_can_type)
-        # detection_id_to_obj[LanguageObjectDetectionID(
-        #     "apple/red_ball")] = Object("apple", _movable_object_type)
-        # detection_id_to_obj[LanguageObjectDetectionID(
-        #     "soda_can")] = Object(
-        #         "soda_can", _movable_object_type)
-        # detection_id_to_obj[LanguageObjectDetectionID(
-        #     "fluffy_toy/flower_arrangement")] = Object(
-        #         "fluffy_green_toy_eraser", _movable_object_type)
-        # detection_id_to_obj[LanguageObjectDetectionID(
-        #     "blue_block")] = Object("blue_block", _movable_object_type)
-
-        # detection_id_to_obj[LanguageObjectDetectionID(
-        #     "cardboard_box")] = Object("cardboard_box_bin", _trash_can_type)
-        # detection_id_to_obj[LanguageObjectDetectionID(
-        # "blue_coffee_cup")] = Object("blue_coffee_cup",
-        #                              _movable_object_type)
-        for obj, pose in get_known_movable_objects().items():
-            if obj.name == "pink_furry_eraser":
-                eraser_obj = Object("pink_furry_eraser",
-                                    _movable_object_type)
-                detection_id = LanguageObjectDetectionID("toy/flower_arrangement")
-                detection_id_to_obj[detection_id] = eraser_obj
-            elif obj.name == "clear_plastic_container":
-                container_obj = Object("clear_plastic_container",
-                                       _trash_can_type)
-                detection_id = LanguageObjectDetectionID("bottle/clear_cup/clear_trashcan")
-                detection_id_to_obj[detection_id] = container_obj
-            else:
-                detection_id = LanguageObjectDetectionID(obj.name)
-                detection_id_to_obj[detection_id] = obj
-        for obj, pose in get_known_immovable_objects().items():
-            stat_detection_id = KnownStaticObjectDetectionID(obj.name, pose)
-            if obj.name == "short_round_coffee_table":
-                table_obj = Object("short_round_coffee_table", _table_type)
-                detection_id_to_obj[stat_detection_id] = table_obj
-            elif obj.name == "childs_play_table":
-                table_obj = Object("childs_play_table", _table_type)
-                detection_id_to_obj[stat_detection_id] = table_obj
-            else:
+
+        if CFG.spot_mapless_mode:
+            # In mapless mode, hardcode the objects to detect via
+            # language rather than relying on metadata.
+            detection_id_to_obj[LanguageObjectDetectionID(
+                "green_and_blue_furry_eraser")] = Object(
+                    "green_and_blue_furry_eraser", _movable_object_type)
+            detection_id_to_obj[LanguageObjectDetectionID(
+                "childs_play_table")] = Object(
+                    "childs_play_table", _table_type)
+            # Floor is always a known static object.
+            for obj, pose in get_known_immovable_objects().items():
+                stat_detection_id = KnownStaticObjectDetectionID(
+                    obj.name, pose)
                 detection_id_to_obj[stat_detection_id] = obj
+        else:
+            for obj, pose in get_known_movable_objects().items():
+                if obj.name == "pink_furry_eraser":
+                    eraser_obj = Object("pink_furry_eraser",
+                                        _movable_object_type)
+                    detection_id = LanguageObjectDetectionID(
+                        "toy/flower_arrangement")
+                    detection_id_to_obj[detection_id] = eraser_obj
+                elif obj.name == "clear_plastic_container":
+                    container_obj = Object("clear_plastic_container",
+                                           _trash_can_type)
+                    detection_id = LanguageObjectDetectionID(
+                        "bottle/clear_cup/clear_trashcan")
+                    detection_id_to_obj[detection_id] = container_obj
+                else:
+                    detection_id = LanguageObjectDetectionID(obj.name)
+                    detection_id_to_obj[detection_id] = obj
+            for obj, pose in get_known_immovable_objects().items():
+                stat_detection_id = KnownStaticObjectDetectionID(
+                    obj.name, pose)
+                if obj.name == "short_round_coffee_table":
+                    table_obj = Object("short_round_coffee_table",
+                                       _table_type)
+                    detection_id_to_obj[stat_detection_id] = table_obj
+                elif obj.name == "childs_play_table":
+                    table_obj = Object("childs_play_table", _table_type)
+                    detection_id_to_obj[stat_detection_id] = table_obj
+                else:
+                    detection_id_to_obj[stat_detection_id] = obj
 
         return detection_id_to_obj
 

predicators/ground_truth_models/spot_env/nsrts.py

@@ -284,7 +284,27 @@ def _move_and_wipe_table_sampler(state: State, goal: Set[GroundAtom],
                                  rng: np.random.Generator,
                                  objs: Sequence[Object]) -> Array:
     target_obj = objs[1]
-    move_sample_params = load_spot_metadata()["wipe_location"][target_obj.name]
+    if CFG.spot_mapless_mode:
+        # In mapless mode, derive the wipe approach pose from the
+        # detected table pose and robot pose in the state.
+        robot_obj = objs[0]
+        table_x = state.get(target_obj, "x")
+        table_y = state.get(target_obj, "y")
+        robot_x = state.get(robot_obj, "x")
+        robot_y = state.get(robot_obj, "y")
+        # Angle from table to robot (i.e., the direction to stand back).
+        angle_table_to_robot = np.arctan2(robot_y - table_y,
+                                          robot_x - table_x)
+        # Stand ~0.85m from the table center, along that direction.
+        standoff = 0.85
+        stand_x = table_x + standoff * np.cos(angle_table_to_robot)
+        stand_y = table_y + standoff * np.sin(angle_table_to_robot)
+        # Face toward the table (opposite direction).
+        facing_angle = angle_table_to_robot + np.pi
+        move_sample_params = [stand_x, stand_y, facing_angle]
+    else:
+        move_sample_params = load_spot_metadata()["wipe_location"][
+            target_obj.name]
     # Hardcoded params; probably need to change in the future.
     rel_dx = 0.0
     # # Params for child play table:

predicators/settings.py

@@ -184,6 +184,7 @@ class GlobalSettings:
     spot_perception_outdir = "spot_perception_outputs"
     spot_render_perception_outputs = True
     spot_graph_nav_map = "floor8-sweeping"
+    spot_mapless_mode = False
     spot_grasp_stow_volume_threshold = 0.074
     spot_run_dry = False
     spot_use_perfect_samplers = False  # for debugging

predicators/spot_utils/perception/object_detection.py

@@ -244,7 +244,7 @@ def _get_detection_score(img_detections: Dict[str, SegmentedBoundingBox],
                 continue
             # If the detected pose is outside the allowed bounds, skip.
             pose_xy = np.array([pose.x, pose.y])
-            if allowed_regions is not None:
+            if allowed_regions is not None and len(allowed_regions) > 0:
                 in_allowed_region = False
                 for region in allowed_regions:
                     if region.find_simplex(pose_xy).item() >= 0:

predicators/spot_utils/spot_localization.py

@@ -172,6 +172,39 @@ def localize(self,
         return None
 
 
+class OdomLocalizer:
+    """Uses the odom frame as the world frame. No GraphNav map required.
+
+    Drop-in replacement for SpotLocalizer for short sessions in small
+    areas where odometry drift is acceptable.
+    """
+
+    def __init__(self, robot: Robot) -> None:
+        self._robot = robot
+        self._robot.time_sync.wait_for_sync()
+        self._robot_pose = math_helpers.SE3Pose(0, 0, 0, math_helpers.Quat())
+        self.localize()
+
+    def get_last_robot_pose(self) -> math_helpers.SE3Pose:
+        """Get the last estimated robot pose in the odom frame."""
+        return self._robot_pose
+
+    def get_world_tform_odom(self) -> math_helpers.SE3Pose:
+        """Identity transform since world frame IS the odom frame."""
+        return math_helpers.SE3Pose(0, 0, 0, math_helpers.Quat())
+
+    def localize(self,
+                 num_retries: int = 10,
+                 retry_wait_time: float = 1.0) -> None:
+        """Read odom_tform_body from robot state."""
+        robot_state = get_robot_state(self._robot)
+        odom_tform_body = get_odom_tform_body(
+            robot_state.kinematic_state.transforms_snapshot)
+        self._robot_pose = math_helpers.SE3Pose.from_proto(
+            odom_tform_body.to_proto())
+        return None
+
+
 if __name__ == "__main__":
     # Run this file alone to test manually.
     # Make sure to pass in --spot_robot_ip.

predicators/spot_utils/utils.py

@@ -195,8 +195,59 @@ def get_graph_nav_dir() -> Path:
     return upload_dir / CFG.spot_graph_nav_map
 
 
+def _get_mapless_static_object_features(env_name: str) -> Dict:
+    """Return static object features for mapless mode, per environment."""
+    # Floor is always present.
+    feats: Dict = {
+        "floor": {
+            "shape": 1,
+            "height": 0.0001,
+            "length": 10000000,
+            "width": 10000000,
+            "flat_top_surface": 1,
+        },
+    }
+    if env_name == "spot_vlm_table_wiping_invented_predicates_env":
+        feats["childs_play_table"] = {
+            "shape": 2,
+            "height": 0.1,
+            "length": 0.35,
+            "width": 0.62,
+            "flat_top_surface": 1,
+        }
+        feats["green_and_blue_furry_eraser"] = {
+            "shape": 1,
+            "height": 0.1,
+            "length": 0.1,
+            "width": 0.1,
+            "placeable": 1,
+            "is_sweeper": 0,
+        }
+    return feats
+
+
 def load_spot_metadata() -> Dict:
-    """Load from the YAML config."""
+    """Load from the YAML config.
+
+    In mapless mode, returns sensible defaults so that no map directory
+    or metadata.yaml file is required.
+    """
+    from predicators.settings import CFG  # pylint: disable=import-outside-toplevel
+    if CFG.spot_mapless_mode:
+        return {
+            "allowed-regions": {},
+            "known-immovable-objects": {
+                # The floor always exists with a sensible default pose.
+                # z=-0.5 is roughly where the floor sits relative to the
+                # robot's body frame origin.
+                "floor": {"x": 0.0, "y": 0.0, "z": -0.5},
+            },
+            "known-movable-objects": {},
+            "static-object-features": _get_mapless_static_object_features(
+                CFG.env),
+            "spot-home-pose": {"x": 0.0, "y": 0.0, "angle": 0.0},
+            "april-tag-offsets": {},
+        }
     config_filepath = get_graph_nav_dir() / "metadata.yaml"
     with open(config_filepath, "r", encoding="utf-8") as f:
         config = yaml.safe_load(f)