Harden DualVLN navigation recovery

internnav/habitat_extensions/vln/recovery_controller.py

@@ -0,0 +1,180 @@
+"""Minimal stateful recovery policy for queued S1 actions."""
+
+from collections import Counter
+from dataclasses import dataclass
+from typing import Optional
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class RecoveryDecision:
+    cancel_remaining: bool
+    replan_system2: bool
+    reason: Optional[str]
+    gps_displacement: Optional[float]
+    retry_count: int
+    goal_retry_count: int
+    turn_count: int
+
+
+class RecoveryController:
+    def __init__(
+        self,
+        min_forward_displacement=0.03,
+        system2_retry_limit=3,
+        max_consecutive_turns=24,
+        direct_turn_escape_clearance=0.55,
+    ):
+        self.min_forward_displacement = float(min_forward_displacement)
+        self.system2_retry_limit = int(system2_retry_limit)
+        self.max_consecutive_turns = int(max_consecutive_turns)
+        self.direct_turn_escape_clearance = float(direct_turn_escape_clearance)
+        self.failed_directions = Counter()
+        self.failed_route_directions = Counter()
+        self.retry_count = 0
+        self.goal_retry_count = 0
+        self.consecutive_failures = 0
+        self.current_route_direction = None
+        self.turn_direction = None
+        self.consecutive_turns = 0
+        self.pending_turn_stall = False
+
+    def reset(self):
+        self.failed_directions.clear()
+        self.failed_route_directions.clear()
+        self.retry_count = 0
+        self.current_route_direction = None
+        self.turn_direction = None
+        self.consecutive_turns = 0
+        self.pending_turn_stall = False
+        self.start_new_goal()
+
+    def start_new_goal(self, preserve_failures=False):
+        if not preserve_failures:
+            self.goal_retry_count = 0
+            self.consecutive_failures = 0
+        self.current_route_direction = None
+        self.turn_direction = None
+        self.consecutive_turns = 0
+
+    def set_route_direction(self, direction):
+        direction = str(direction).lower() if direction is not None else None
+        self.current_route_direction = direction if direction in ("left", "straight", "right") else None
+
+    def failed_route_context(self):
+        if not self.failed_route_directions:
+            return None
+        ordered = sorted(self.failed_route_directions.items(), key=lambda item: (-item[1], item[0]))
+        attempts = ", ".join(f"{direction} x{count}" for direction, count in ordered)
+        return (
+            f"Failed route sectors from this episode: {attempts}. Prefer a less-failed visible route, "
+            "but returning or reversing remains allowed when the instruction requires correction."
+        )
+
+    def exploratory_turn(self):
+        """Return LEFT/RIGHT action code using the less-failed route sector."""
+        left = self.failed_route_directions.get("left", 0)
+        right = self.failed_route_directions.get("right", 0)
+        if self.current_route_direction == "left":
+            return 3
+        if self.current_route_direction == "right":
+            return 2
+        return 2 if left <= right else 3
+
+    def consume_turn_stall(self):
+        """Return a recent full-circle stall once, then clear the signal."""
+        pending = self.pending_turn_stall
+        self.pending_turn_stall = False
+        return pending
+
+    def filter_direct_actions(self, actions, centre_clearance=None, centre_blocked=False):
+        """Replace a repeated full-circle S2 turn with one bounded escape action."""
+        sequence = [int(action) for action in actions]
+        if not sequence or sequence[0] not in (2, 3) or any(
+            action != sequence[0] for action in sequence
+        ):
+            return sequence, None
+        failed_turn = sequence[0]
+        if self.failed_directions.get(failed_turn, 0) <= 0:
+            return sequence, None
+
+        clearance = None
+        if centre_clearance is not None:
+            try:
+                value = float(centre_clearance)
+                clearance = value if np.isfinite(value) else None
+            except (TypeError, ValueError):
+                clearance = None
+        if (
+            not bool(centre_blocked)
+            and clearance is not None
+            and clearance >= self.direct_turn_escape_clearance
+        ):
+            replacement = [1]
+            reason = "depth_gated_forward_after_repeated_direct_turn"
+        else:
+            replacement = [3 if failed_turn == 2 else 2]
+            reason = "opposite_probe_after_repeated_direct_turn"
+
+        # Consume one failure marker. After the bounded escape the changed pose
+        # receives a fresh normal-turn budget instead of being permanently banned.
+        self.failed_directions[failed_turn] -= 1
+        if self.failed_directions[failed_turn] <= 0:
+            del self.failed_directions[failed_turn]
+        return replacement, reason
+
+    def update(self, action, gps_before, gps_after, collision=False, is_s1=True):
+        action = int(action)
+        before = np.asarray(gps_before, dtype=np.float32).reshape(-1)
+        after = np.asarray(gps_after, dtype=np.float32).reshape(-1)
+        displacement = None
+        if before.size >= 2 and after.size >= 2 and np.all(np.isfinite(before[:2])) and np.all(np.isfinite(after[:2])):
+            displacement = float(np.linalg.norm(after[:2] - before[:2]))
+
+        if action in (2, 3) and self.max_consecutive_turns > 0:
+            if action == self.turn_direction:
+                self.consecutive_turns += 1
+            else:
+                self.turn_direction = action
+                self.consecutive_turns = 1
+        else:
+            self.turn_direction = None
+            self.consecutive_turns = 0
+        observed_turn_count = self.consecutive_turns
+
+        reason = None
+        if is_s1 and collision:
+            reason = "collision"
+        elif is_s1 and action == 1 and displacement is not None and displacement < self.min_forward_displacement:
+            reason = "low_gps_displacement"
+        elif self.max_consecutive_turns > 0 and observed_turn_count >= self.max_consecutive_turns:
+            reason = "repeated_turns"
+
+        if reason is not None:
+            self.retry_count += 1
+            self.goal_retry_count += 1
+            self.consecutive_failures += 1
+            self.failed_directions[action] += 1
+            if self.current_route_direction is not None:
+                self.failed_route_directions[self.current_route_direction] += 1
+        elif is_s1 and action == 1:
+            self.goal_retry_count = 0
+            self.consecutive_failures = 0
+
+        replan_system2 = reason == "repeated_turns" or (
+            reason is not None and self.goal_retry_count >= self.system2_retry_limit
+        )
+        if reason == "repeated_turns":
+            self.pending_turn_stall = True
+            self.turn_direction = None
+            self.consecutive_turns = 0
+        return RecoveryDecision(
+            cancel_remaining=reason is not None,
+            replan_system2=replan_system2,
+            reason=reason,
+            gps_displacement=displacement,
+            retry_count=self.retry_count,
+            goal_retry_count=self.goal_retry_count,
+            turn_count=observed_turn_count,
+        )

internnav/habitat_extensions/vln/trajectory_selector.py

@@ -0,0 +1,207 @@
+"""Lightweight candidate selection and risk estimates for S1 trajectories."""
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class TrajectorySelection:
+    selected_index: int
+    trajectory: np.ndarray
+    dispersion: float
+    chunk_size: int
+    bimodal: bool
+    cluster_sizes: tuple
+    clearance: float
+    smoothness: float
+    depth_risk: bool
+    route_direction: str
+    failed_route_penalty: float
+
+
+class TrajectorySelector:
+    """Select a real candidate using consensus, clustering, and cheap safety scores."""
+
+    def __init__(
+        self,
+        cluster_min_fraction=0.25,
+        cluster_lateral_gap=0.25,
+        depth_lookahead=1.25,
+        unsafe_clearance=0.12,
+        near_clearance=0.35,
+        horizontal_fov_deg=79.0,
+    ):
+        self.cluster_min_fraction = float(cluster_min_fraction)
+        self.cluster_lateral_gap = float(cluster_lateral_gap)
+        self.depth_lookahead = float(depth_lookahead)
+        self.unsafe_clearance = float(unsafe_clearance)
+        self.near_clearance = float(near_clearance)
+        self.horizontal_fov_rad = np.deg2rad(float(horizontal_fov_deg))
+
+    @staticmethod
+    def _as_numpy(value):
+        value = value.detach() if hasattr(value, "detach") else value
+        value = value.float().cpu().numpy() if hasattr(value, "cpu") else np.asarray(value)
+        return np.asarray(value, dtype=np.float32)
+
+    @classmethod
+    def reconstruct(cls, dp_actions):
+        deltas = cls._as_numpy(dp_actions).copy()
+        if deltas.ndim != 3 or deltas.shape[-1] < 2 or deltas.shape[0] == 0:
+            raise ValueError("dp_actions must have shape [candidates, steps, >=2]")
+        deltas[:, :, :2] /= 4.0
+        positions = np.cumsum(deltas[:, :, :2], axis=1)
+        origin = np.zeros((positions.shape[0], 1, 2), dtype=positions.dtype)
+        return np.concatenate((origin, positions), axis=1)
+
+    def _two_medoid_clusters(self, pairwise, trajectories):
+        count = pairwise.shape[0]
+        if count < 4:
+            return False, (count, 0)
+
+        medoids = np.unravel_index(np.argmax(pairwise), pairwise.shape)
+        assignments = None
+        for _ in range(3):
+            assignments = np.argmin(pairwise[:, medoids], axis=1)
+            updated = []
+            for cluster in range(2):
+                members = np.flatnonzero(assignments == cluster)
+                if members.size == 0:
+                    updated.append(medoids[cluster])
+                else:
+                    costs = pairwise[np.ix_(members, members)].sum(axis=1)
+                    updated.append(int(members[np.argmin(costs)]))
+            medoids = tuple(updated)
+
+        assignments = np.argmin(pairwise[:, medoids], axis=1)
+        sizes = tuple(int(np.sum(assignments == cluster)) for cluster in range(2))
+        minimum_size = max(2, int(np.ceil(count * self.cluster_min_fraction)))
+        endpoints = trajectories[list(medoids), -1, 1]
+        lateral_gap = float(abs(endpoints[0] - endpoints[1]))
+        opposite_sides = bool(endpoints[0] * endpoints[1] < 0)
+        within = []
+        for cluster in range(2):
+            members = np.flatnonzero(assignments == cluster)
+            within.extend(pairwise[members, medoids[cluster]].tolist())
+        within_distance = float(np.mean(within)) if within else 0.0
+        separation = float(pairwise[medoids])
+        bimodal = (
+            min(sizes) >= minimum_size
+            and opposite_sides
+            and lateral_gap >= self.cluster_lateral_gap
+            and separation >= max(0.12, 1.5 * within_distance)
+        )
+        return bimodal, sizes
+
+    @staticmethod
+    def _smoothness(trajectories):
+        segments = np.diff(trajectories, axis=1)
+        headings = np.unwrap(np.arctan2(segments[:, :, 1], segments[:, :, 0]), axis=1)
+        turns = np.diff(headings, axis=1)
+        return np.mean(np.abs(turns), axis=1)
+
+    @staticmethod
+    def direction_label(trajectory):
+        """Classify a candidate by its endpoint bearing in the local frame."""
+        trajectory = np.asarray(trajectory, dtype=np.float32)
+        forward, lateral = trajectory[-1, :2]
+        bearing = float(np.arctan2(lateral, max(float(forward), 0.05)))
+        if bearing >= np.deg2rad(12.0):
+            return "left"
+        if bearing <= -np.deg2rad(12.0):
+            return "right"
+        return "straight"
+
+    def _depth_clearance(self, trajectories, depth):
+        if depth is None:
+            return np.full(trajectories.shape[0], np.inf, dtype=np.float32)
+        depth = self._as_numpy(depth).squeeze()
+        if depth.ndim != 2:
+            raise ValueError("depth must be a 2-D image in metres")
+
+        height, width = depth.shape
+        row_start, row_stop = int(height * 0.30), max(int(height * 0.82), 1)
+        depth_crop = depth[row_start:row_stop]
+        valid_mask = np.isfinite(depth_crop) & (depth_crop > 0.05)
+        valid_columns = np.any(valid_mask, axis=0)
+        depth_profile = np.full(width, np.inf, dtype=np.float32)
+        if np.any(valid_columns):
+            valid_values = np.where(valid_mask[:, valid_columns], depth_crop[:, valid_columns], np.inf)
+            sorted_values = np.sort(valid_values, axis=0)
+            valid_counts = np.sum(valid_mask[:, valid_columns], axis=0)
+            percentile_indices = np.floor(0.15 * (valid_counts - 1)).astype(np.int32)
+            depth_profile[valid_columns] = sorted_values[
+                percentile_indices, np.arange(sorted_values.shape[1])
+            ]
+
+        # A seven-column minimum approximates the original narrow ray window
+        # conservatively, while avoiding one quantile call per trajectory point.
+        padded = np.pad(depth_profile, (3, 3), constant_values=np.inf)
+        ray_profile = np.min(np.lib.stride_tricks.sliding_window_view(padded, 7), axis=1)
+
+        forward = trajectories[:, 1:, 0]
+        lateral = trajectories[:, 1:, 1]
+        radial = np.hypot(forward, lateral)
+        bearing = np.arctan2(lateral, forward)
+        visible = (
+            (forward > 0.05)
+            & (radial <= self.depth_lookahead)
+            & (np.abs(bearing) < self.horizontal_fov_rad / 2)
+        )
+        columns = np.rint((0.5 - bearing / self.horizontal_fov_rad) * (width - 1)).astype(np.int32)
+        columns = np.clip(columns, 0, width - 1)
+        point_clearance = np.where(visible, ray_profile[columns] - forward, np.inf)
+        return np.min(point_clearance, axis=1).astype(np.float32)
+
+    def select(self, dp_actions, depth=None, recent_failure=False, failed_route_directions=None):
+        trajectories = self.reconstruct(dp_actions)
+        flattened = trajectories.reshape(trajectories.shape[0], -1)
+        pairwise = np.sqrt(np.mean((flattened[:, None] - flattened[None, :]) ** 2, axis=2))
+        centrality = pairwise.mean(axis=1)
+        medoid_index = int(np.argmin(centrality))
+        dispersion = float(np.sqrt(np.mean((trajectories - trajectories[medoid_index]) ** 2)))
+        bimodal, cluster_sizes = self._two_medoid_clusters(pairwise, trajectories)
+        smoothness = self._smoothness(trajectories)
+        clearance = self._depth_clearance(trajectories, depth)
+        directions = [self.direction_label(trajectory) for trajectory in trajectories]
+        failed_route_directions = failed_route_directions or {}
+        route_penalties = np.asarray(
+            [min(float(failed_route_directions.get(direction, 0)), 3.0) for direction in directions],
+            dtype=np.float32,
+        )
+
+        selected_index = medoid_index
+        if depth is not None or np.any(route_penalties > 0):
+            centrality_scale = max(float(np.median(centrality)), 1e-6)
+            smoothness_scale = max(float(np.median(smoothness)), 1e-6)
+            clearance_penalty = np.maximum(self.near_clearance - clearance, 0.0) / self.near_clearance
+            scores = (
+                centrality / centrality_scale
+                + 0.20 * smoothness / smoothness_scale
+                + 4.0 * clearance_penalty
+                + 1.25 * route_penalties
+            )
+            selected_index = int(np.argmin(scores))
+
+        selected_clearance = float(clearance[selected_index])
+        depth_risk = selected_clearance < self.unsafe_clearance
+        if recent_failure or bimodal or depth_risk:
+            chunk_size = 1
+        elif selected_clearance < self.near_clearance:
+            chunk_size = 2
+        else:
+            chunk_size = 4
+        return TrajectorySelection(
+            selected_index=selected_index,
+            trajectory=trajectories[selected_index],
+            dispersion=dispersion,
+            chunk_size=chunk_size,
+            bimodal=bimodal,
+            cluster_sizes=cluster_sizes,
+            clearance=selected_clearance,
+            smoothness=float(smoothness[selected_index]),
+            depth_risk=depth_risk,
+            route_direction=directions[selected_index],
+            failed_route_penalty=float(route_penalties[selected_index]),
+        )