major update: add an aggregated perception monitor to decide when to call perception pipelines

Author: lf-zhao

predicators/envs/spot_env.py

@@ -33,6 +33,8 @@
     Predicate, Action, GoalDescription, EnvironmentTask, STRIPSOperator, \
     VLMPredicate, VLMGroundAtom, Observation, Type, SpotActionExtraInfo, \
     LiftedAtom, Variable
+from predicators.perception.perception_monitor import PerceptionDecision, \
+    PerceptionMonitor
 from predicators.spot_utils.perception.object_detection import \
     AprilTagObjectDetectionID, KnownStaticObjectDetectionID, \
     LanguageObjectDetectionID, ObjectDetectionID, detect_objects_from_language, \
@@ -75,6 +77,11 @@
 _obj_name_to_sim_obj: Dict[str, pbrspot.body.Body] = {}
 
 
+def _is_hand_view_action(action_name: str) -> bool:
+    """Detect whether the operator causes a hand-view observation."""
+    return "HandView" in action_name
+
+
 @dataclass(frozen=True)
 class _SpotObservation:
     """An observation for a SpotEnv."""
@@ -213,7 +220,8 @@ def get_robot(
     lease_client.take()
     lease_keepalive = LeaseKeepAlive(lease_client,
                                      must_acquire=True,
-                                     return_at_exit=True)
+                                     return_at_exit=True,
+                                     warnings=False)
     localizer = None
     assert path.exists()
     if use_localizer:
@@ -331,6 +339,7 @@ def __init__(self, use_gui: bool = True) -> None:
         self._cached_realworld_observation: Optional[_SpotObservation] = None
         self._last_vlm_pointing: Optional[
             vlm_pointing.VLMPointingResult] = None
+        self._perception_monitor = PerceptionMonitor()
 
     def _initialize_pybullet(self) -> None:
         # First, check if we have any connections to pybullet already,
@@ -515,7 +524,7 @@ def _maybe_trigger_vlm_pointing(self, action_name: str,
                                     action_objs: Sequence[Object],
                                     observation: _SpotObservation) -> None:
         """Optionally compute a hand-camera grasp point via Gemini pointing."""
-        if not CFG.spot_use_object_pointing:
+        if not CFG.spot_use_vlm_pointing:
             self._last_vlm_pointing = None
             return
 
@@ -777,6 +786,11 @@ def reset(self, train_or_test: str, task_idx: int) -> Observation:
             while True:
                 try:
                     self._lease_client.take()
+                    if CFG.spot_enable_manual_reset_teleop and \
+                            not CFG.spot_run_dry:
+                        logging.info(
+                            "[SpotEnv] Manual reset enabled; skipping automatic"
+                            " stow/go-home motions before initial observation.")
                     self._current_task = self._actively_construct_env_task()
                     break
                 except RetryableRpcError as e:
@@ -790,6 +804,8 @@ def reset(self, train_or_test: str, task_idx: int) -> Observation:
             self._cached_realworld_observation = self._current_observation
         else:
             self._cached_realworld_observation = None
+        self._perception_monitor.reset(
+            initial_scan_done=self._cached_realworld_observation is not None)
         self._current_task_goal_reached = False
         self._last_action = None
 
@@ -900,96 +916,104 @@ def step(self, action: Action) -> Observation:
                     logging.warning("WARNING: the following retryable error "
                                     f"was encountered. Trying again.\n{e}")
 
-            observe_only_refresh = CFG.spot_perception_refresh_observe_only
-            force_skip_perception = observe_only_refresh and not is_info_action
-            cached_available = self._cached_realworld_observation is not None
-            skip_perception = False
+            decision = self._perception_monitor.decide(
+                has_cached_observation=self._cached_realworld_observation
+                is not None,
+                is_information_gathering=is_info_action,
+                is_hand_view_action=_is_hand_view_action(action_name))
+            logging.debug("[SpotEnv] Perception decision: %s",
+                          decision.reason)
 
-            if force_skip_perception:
-                if cached_available:
-                    skip_perception = True
-                else:
-                    logging.debug(
-                        "[SpotEnv] Observe-only perception requested but no "
-                        "cached observation is available; running perception "
-                        "once to seed cache.")
-            elif CFG.spot_skip_perception_for_manipulation and \
-                    not is_info_action and cached_available:
-                skip_perception = True
-
-            if skip_perception:
+            if decision.refresh_full:
+                while True:
+                    try:
+                        next_obs = self._build_realworld_observation(
+                            next_nonpercept, curr_obs=obs)
+                        self._perception_monitor.mark_refresh_complete()
+                        self._cached_realworld_observation = next_obs
+                        break
+                    except RetryableRpcError as e:
+                        logging.warning(
+                            "WARNING: the following retryable error "
+                            f"was encountered. Trying again.\n{e}")
+            elif decision.reuse_cache and \
+                    self._cached_realworld_observation is not None:
                 next_obs = self._reuse_cached_observation(next_nonpercept)
-                self._cached_realworld_observation = next_obs
             else:
-                # Get the new observation. Again, automatically retry if needed.
+                logging.debug(
+                    "[SpotEnv] No cached observation available; running "
+                    "perception to seed cache.")
                 while True:
                     try:
                         next_obs = self._build_realworld_observation(
                             next_nonpercept, curr_obs=obs)
+                        self._perception_monitor.mark_refresh_complete()
+                        self._cached_realworld_observation = next_obs
                         break
                     except RetryableRpcError as e:
                         logging.warning(
                             "WARNING: the following retryable error "
                             f"was encountered. Trying again.\n{e}")
 
+            if decision.trigger_pointing:
                 self._maybe_trigger_vlm_pointing(action_name, action_objs,
                                                  next_obs)
 
-                # Very hacky optimization to force viewing/reaching to work.
-                # NOTE: enter the check if "move to" "object"; make sure op names in such pattern are always needed to check
-                if CFG.spot_enable_interactive_checks and \
-                        "MoveTo" in action_name and "Object" in action_name:
+            # Very hacky optimization to force viewing/reaching to work.
+            # NOTE: enter the check if "move to" "object"; make sure op names in such pattern are always needed to check
+            if CFG.spot_enable_interactive_checks and \
+                    "MoveTo" in action_name and "Object" in action_name:
+                logging.warning(
+                    f"Entering object detection check with action_name: {action_name}")
+                _, target_obj = action_objs
+                # Retry if each of the types of moving failed in their own way.
+                if action_name == "MoveToHandViewObject" or "Hand" in action_name:
+                    need_retry = target_obj not in \
+                        next_obs.objects_in_hand_view
+                elif action_name == "MoveToBodyViewObject":
+                    need_retry = target_obj not in \
+                        next_obs.objects_in_any_view_except_back
+                elif action_name == "MoveToReachObject":
+                    obj_pose = self._last_known_object_poses[target_obj]
+                    obj_position = math_helpers.Vec3(x=obj_pose.x,
+                                                     y=obj_pose.y,
+                                                     z=obj_pose.z)
+                    need_retry = not _obj_reachable_from_spot_pose(
+                        next_obs.robot_pos, obj_position)
+                else:
+                    need_retry = False
+                    logging.info("WARNING: object detection check not "
+                                 "implemented for this action: %s",
+                                 action_name)
+                if need_retry:
                     logging.warning(
-                        f"Entering object detection check with action_name: {action_name}")
-                    _, target_obj = action_objs
-                    # Retry if each of the types of moving failed in their own way.
-                    if action_name == "MoveToHandViewObject" or "Hand" in action_name:
-                        need_retry = target_obj not in \
-                            next_obs.objects_in_hand_view
-                    elif action_name == "MoveToBodyViewObject":
-                        need_retry = target_obj not in \
-                            next_obs.objects_in_any_view_except_back
-                    elif action_name == "MoveToReachObject":
-                        obj_pose = self._last_known_object_poses[target_obj]
-                        obj_position = math_helpers.Vec3(x=obj_pose.x,
-                                                         y=obj_pose.y,
-                                                         z=obj_pose.z)
-                        need_retry = not _obj_reachable_from_spot_pose(
-                            next_obs.robot_pos, obj_position)
-                    else:
-                        need_retry = False
-                        logging.info("WARNING: object detection check not "
-                                     "implemented for this action: %s",
-                                     action_name)
-                    if need_retry:
-                        logging.warning(
-                            "WARNING: retrying %s because %s was not "
-                            "seen/reached.", action_name, target_obj)
-                        prompt = (
-                            "Hit 'c' to have the robot do a random movement "
-                            "or take control and move the robot accordingly. "
-                            "Hit the 'Enter' key when you're done!\n")
-                        user_pref = input(prompt)
-                        assert self._lease_client is not None
-                        self._lease_client.take()
-                        angle = 0.0
-                        if user_pref == "c":
-                            # Do a small random movement to get a new view.
-                            assert isinstance(action_fn_args[1],
-                                              math_helpers.SE2Pose)
-                            angle = self._noise_rng.uniform(-np.pi / 6,
-                                                            np.pi / 6)
-                        rel_pose = math_helpers.SE2Pose(0, 0, angle)
-                        assert isinstance(action_fn_args, tuple)
-                        new_action_args = action_fn_args[0:1] + (rel_pose, ) + \
-                            action_fn_args[2:]
-
-                        new_action = utils.create_spot_env_action(
-                            SpotActionExtraInfo(action_name, action_objs,
-                                                action_fn, new_action_args,
-                                                sim_action_fn,
-                                                sim_action_args))
-                        return self.step(new_action)
+                        "WARNING: retrying %s because %s was not "
+                        "seen/reached.", action_name, target_obj)
+                    prompt = (
+                        "Hit 'c' to have the robot do a random movement "
+                        "or take control and move the robot accordingly. "
+                        "Hit the 'Enter' key when you're done!\n")
+                    user_pref = input(prompt)
+                    assert self._lease_client is not None
+                    self._lease_client.take()
+                    angle = 0.0
+                    if user_pref == "c":
+                        # Do a small random movement to get a new view.
+                        assert isinstance(action_fn_args[1],
+                                          math_helpers.SE2Pose)
+                        angle = self._noise_rng.uniform(-np.pi / 6,
+                                                        np.pi / 6)
+                    rel_pose = math_helpers.SE2Pose(0, 0, angle)
+                    assert isinstance(action_fn_args, tuple)
+                    new_action_args = action_fn_args[0:1] + (rel_pose, ) + \
+                        action_fn_args[2:]
+
+                    new_action = utils.create_spot_env_action(
+                        SpotActionExtraInfo(action_name, action_objs,
+                                            action_fn, new_action_args,
+                                            sim_action_fn,
+                                            sim_action_args))
+                    return self.step(new_action)
             self._allow_incidental_discovery = False
 
         self._current_observation = next_obs
@@ -1464,13 +1488,22 @@ def _actively_construct_initial_object_views(
                                                         bool or None]]:
         assert self._robot is not None
         assert self._localizer is not None
-        stow_arm(self._robot)
-        go_home(self._robot, self._localizer)
+        skip_motion = (CFG.spot_enable_manual_reset_teleop
+                       and not CFG.spot_run_dry)
+        if skip_motion:
+            logging.info(
+                "[SpotEnv] Manual reset enabled; skipping automatic stow/go_home"
+                " during initial observation. Ensure the robot is already in a"
+                " safe pose before continuing.")
+        else:
+            stow_arm(self._robot)
+            go_home(self._robot, self._localizer)
         self._localizer.localize()
         detection_ids = self._detection_id_to_obj.keys()
         detections, vlm_atom_dict = self._run_init_search_for_objects(
             set(detection_ids))
-        stow_arm(self._robot)
+        if not skip_motion:
+            stow_arm(self._robot)
         obj_to_se3_pose = {
             self._detection_id_to_obj[det_id]: val
             for (det_id, val) in detections.items()
@@ -1485,12 +1518,18 @@ def _run_init_search_for_objects(
         """Have the hand look down from high up at first."""
         assert self._robot is not None
         assert self._localizer is not None
-        hand_pose = math_helpers.SE3Pose(x=0.80,
-                                         y=0.0,
-                                         z=0.75,
-                                         rot=math_helpers.Quat.from_pitch(
-                                             np.pi / 3))
-        move_hand_to_relative_pose(self._robot, hand_pose)
+        skip_motion = (CFG.spot_enable_manual_reset_teleop
+                       and not CFG.spot_run_dry)
+        if skip_motion:
+            logging.info("[SpotEnv] Manual reset enabled; keeping operator-set"
+                         " hand pose for initial scan.")
+        else:
+            hand_pose = math_helpers.SE3Pose(x=0.80,
+                                             y=0.0,
+                                             z=0.75,
+                                             rot=math_helpers.Quat.from_pitch(
+                                                 np.pi / 3))
+            move_hand_to_relative_pose(self._robot, hand_pose)
         # Input VLM predicates (to filter task-relevant ones) and objects
         # Obtain detections and additionally VLM ground atoms
         detections, artifacts, vlm_atom_dict = init_search_for_objects(
@@ -5007,7 +5046,7 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
         # NOTE: cup is container type
         objects_to_detect = [
             ("cardboard_box", "cardboard_box", _container_type),
-            ("cup", "orange_cup", _container_type),
+            ("cup", "green_cup", _container_type),
         ]
 
         # Add detection object prompt and save object identifier

predicators/perception/perception_monitor.py

@@ -0,0 +1,82 @@
+"""Monitor that decides when Spot should run perception updates."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Tuple
+
+from predicators.settings import CFG
+
+
+@dataclass(frozen=True)
+class PerceptionDecision:
+    """Single-step perception request."""
+    refresh_full: bool
+    reuse_cache: bool
+    trigger_pointing: bool
+    reason: str = ""
+
+    @property
+    def requires_observation(self) -> bool:
+        return self.refresh_full
+
+
+class PerceptionMonitor:
+    """Small helper that mirrors the execution monitor for perception cadence."""
+
+    def __init__(self) -> None:
+        self._initial_scan_done = True
+
+    def reset(self, *, initial_scan_done: bool = True) -> None:
+        self._initial_scan_done = initial_scan_done
+
+    def mark_refresh_complete(self) -> None:
+        self._initial_scan_done = True
+
+    def decide(self, *, has_cached_observation: bool,
+               is_information_gathering: bool,
+               is_hand_view_action: bool) -> PerceptionDecision:
+        """Compute the perception request for this env step."""
+        trigger_pointing = is_hand_view_action and CFG.spot_use_vlm_pointing
+
+        if not self._initial_scan_done:
+            return PerceptionDecision(refresh_full=True,
+                                      reuse_cache=False,
+                                      trigger_pointing=trigger_pointing,
+                                      reason="initial perception scan")
+
+        if is_hand_view_action:
+            # Always refresh when we rotate the hand camera to observe a target.
+            return PerceptionDecision(refresh_full=True,
+                                      reuse_cache=False,
+                                      trigger_pointing=trigger_pointing,
+                                      reason="hand-view observation")
+
+        if is_information_gathering:
+            return PerceptionDecision(refresh_full=True,
+                                      reuse_cache=False,
+                                      trigger_pointing=trigger_pointing,
+                                      reason="information-gathering operator")
+
+        if CFG.spot_perception_refresh_observe_only:
+            if has_cached_observation:
+                return PerceptionDecision(refresh_full=False,
+                                          reuse_cache=True,
+                                          trigger_pointing=trigger_pointing,
+                                          reason="observe-only reuse")
+            # No cache available; fall back to a full refresh.
+            return PerceptionDecision(refresh_full=True,
+                                      reuse_cache=False,
+                                      trigger_pointing=trigger_pointing,
+                                      reason="observe-only but cache empty")
+
+        if CFG.spot_skip_perception_for_manipulation and has_cached_observation:
+            return PerceptionDecision(refresh_full=False,
+                                      reuse_cache=True,
+                                      trigger_pointing=trigger_pointing,
+                                      reason="manipulation skip w/ cache")
+
+        return PerceptionDecision(refresh_full=True,
+                                  reuse_cache=False,
+                                  trigger_pointing=trigger_pointing,
+                                  reason="default refresh")