From e6616456fb72f40310b6c535a001ff173be62dc8 Mon Sep 17 00:00:00 2001 From: Pepijn Date: Mon, 20 Jul 2026 18:27:07 +0200 Subject: [PATCH] navigation: port value maps, features, skills, agent + dog-nav CLI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Copied from the dyna360 research stack into lerobot.navigation: - value_map.py: DynaMem §3.4 exploration scoring (V_T recency + V_S query-similarity, combined value with distance discount). - features.py: SiglipFeatureExtractor (MaskCLIP dense patches, lazy transformers) + FeatureExtractor protocol + BasisVectorFeatureExtractor stand-in for model-free dry-run/tests. - skills.py: SpatialSkills locate/goto/explore over the voxel memory + base controller + text encoder. - agent.py: DeterministicAgent (locate→goto / explore→relocate policy) + HardcodedTaskParser (regex NL→Task). - sim.py: self-contained synthetic scenes (kitchen) + basis-vector text encoder, replacing the dyna360 eval harness for dry-run. - dog_cli.py: `dog-nav` interactive REPL — idle→explore, prompt→locate+ goto (explore-to-find on miss), preemptible, Ctrl-C e-stop, --dry-run. Silenced benign fp16 matmul warnings in query/similarity via np.errstate. Pure numpy + optional lazy torch/transformers; 67 new tests (116 total in tests/navigation/), all model/hardware-free. Deliverable runs: `python -m lerobot.navigation.dog_cli --dry-run`. Co-Authored-By: Claude Opus 4.8 --- src/lerobot/navigation/__init__.py | 39 ++++ src/lerobot/navigation/agent.py | 262 +++++++++++++++++++++++ src/lerobot/navigation/dog_cli.py | 194 +++++++++++++++++ src/lerobot/navigation/features.py | 231 ++++++++++++++++++++ src/lerobot/navigation/sim.py | 207 ++++++++++++++++++ src/lerobot/navigation/skills.py | 321 ++++++++++++++++++++++++++++ src/lerobot/navigation/value_map.py | 221 +++++++++++++++++++ src/lerobot/navigation/voxel_map.py | 5 +- tests/navigation/test_agent.py | 248 +++++++++++++++++++++ tests/navigation/test_dog_cli.py | 106 +++++++++ tests/navigation/test_skills.py | 268 +++++++++++++++++++++++ tests/navigation/test_value_map.py | 211 ++++++++++++++++++ 12 files changed, 2312 insertions(+), 1 deletion(-) create mode 100644 src/lerobot/navigation/agent.py create mode 100644 src/lerobot/navigation/dog_cli.py create mode 100644 src/lerobot/navigation/features.py create mode 100644 src/lerobot/navigation/sim.py create mode 100644 src/lerobot/navigation/skills.py create mode 100644 src/lerobot/navigation/value_map.py create mode 100644 tests/navigation/test_agent.py create mode 100644 tests/navigation/test_dog_cli.py create mode 100644 tests/navigation/test_skills.py create mode 100644 tests/navigation/test_value_map.py diff --git a/src/lerobot/navigation/__init__.py b/src/lerobot/navigation/__init__.py index 0db735b22..35827bb12 100644 --- a/src/lerobot/navigation/__init__.py +++ b/src/lerobot/navigation/__init__.py @@ -23,6 +23,14 @@ dyna360 research stack; the physical robot layer lives in ``lerobot.robots`` (e.g. ``unitree_go2``). """ +from .agent import ( + AgentConfig, + AgentResult, + DeterministicAgent, + HardcodedTaskParser, + Task, + TaskParser, +) from .base_controller import ( BaseController, RobotBaseController, @@ -31,6 +39,11 @@ from .base_controller import ( odometry_to_world_pose, world_velocity_to_body, ) +from .features import ( + BasisVectorFeatureExtractor, + FeatureExtractor, + SiglipFeatureExtractor, +) from .occupancy import ( NAVIGABLE, OBSTACLE, @@ -40,24 +53,50 @@ from .occupancy import ( find_frontier_cells, project_voxel_map_to_grid, ) +from .skills import ( + ExploreResult, + GotoResult, + LocateResult, + SkillsConfig, + SpatialSkills, +) +from .value_map import ValueMapConfig, ValueMaps, compute_value_maps, pick_best_frontier_cell from .voxel_map import CarveResult, QueryResult, VoxelMap, VoxelSnapshot __all__ = [ "NAVIGABLE", "OBSTACLE", "UNOBSERVED", + "AgentConfig", + "AgentResult", "BaseController", + "BasisVectorFeatureExtractor", "CarveResult", + "DeterministicAgent", + "ExploreResult", + "FeatureExtractor", + "GotoResult", + "HardcodedTaskParser", + "LocateResult", "OccupancyGrid", "QueryResult", "RobotBaseController", "SafeBaseController", + "SiglipFeatureExtractor", + "SkillsConfig", + "SpatialSkills", "StubBaseController", + "Task", + "TaskParser", + "ValueMapConfig", + "ValueMaps", "VoxelMap", "VoxelSnapshot", "astar", + "compute_value_maps", "find_frontier_cells", "odometry_to_world_pose", + "pick_best_frontier_cell", "project_voxel_map_to_grid", "world_velocity_to_body", ] diff --git a/src/lerobot/navigation/agent.py b/src/lerobot/navigation/agent.py new file mode 100644 index 000000000..5f7d8a81b --- /dev/null +++ b/src/lerobot/navigation/agent.py @@ -0,0 +1,262 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Deterministic agent wrapper + language-parser interface. + +Ported from the dyna360 research stack. The high-level agent is a thin +deterministic wrapper, not LLM-driven: explore-vs-go control lives here +in plain Python. A language model (when wired up) only parses a +natural-language command into a typed :class:`Task`; the deterministic +wrapper then executes it. Swapping the parser (regex vs a real LLM) must +not change the spatial behaviour. +""" + +from __future__ import annotations + +import logging +import re +from dataclasses import dataclass, field +from typing import TYPE_CHECKING, Protocol, runtime_checkable + +if TYPE_CHECKING: + from lerobot.navigation.skills import SpatialSkills + +LOG = logging.getLogger(__name__) + + +# ============== task data structures ====================================== # + + +@dataclass(frozen=True) +class Task: + """Parsed command, ready for the deterministic wrapper to execute. + + ``go to X`` yields ``Task(targets=['X'])``; ``go to X then Y`` yields + ``Task(targets=['X', 'Y'])``, executed sequentially. + """ + + targets: list[str] + raw: str = "" + + +@dataclass(frozen=True) +class TargetResult: + """Outcome of executing the policy for a single target.""" + + target: str + reached: bool + final_xyz: tuple[float, float, float] | None + n_explore_iters: int + confidence: float + reason: str + """'ok' | 'no_path' | 'budget_exhausted' | 'no_frontier' | 'parse_empty'.""" + + +@dataclass(frozen=True) +class AgentResult: + """Outcome of executing a full Task (one or more sequential targets).""" + + task: Task + target_results: list[TargetResult] = field(default_factory=list) + + @property + def fully_successful(self) -> bool: + return bool(self.target_results) and all(r.reached for r in self.target_results) + + +# ============== language parser ========================================== # + + +@runtime_checkable +class TaskParser(Protocol): + """Anything that turns a free-text command into a :class:`Task`.""" + + def parse(self, command: str) -> Task: ... + + +class HardcodedTaskParser: + """Regex-only parser — fast, dependency-free, good enough to validate + the deterministic policy without loading a language model. + + Handles ``go to (the) X`` / ``find (the) X`` → single target, ``go to + X then Y`` → multi-step, and falls back to "the whole command is the + target" if no pattern matches. + """ + + _SINGLE_PATTERNS = ( + re.compile( + r"^\s*(?:go to|navigate to|find|locate|look for)\s+(?:the\s+)?(.+?)\s*$", + re.IGNORECASE, + ), + ) + _SPLIT_PATTERN = re.compile(r"\s+(?:then|and then)\s+|\s*,\s*", re.IGNORECASE) + + def parse(self, command: str) -> Task: + raw = command.strip() + if not raw: + return Task(targets=[], raw=raw) + + parts = self._SPLIT_PATTERN.split(raw) + targets: list[str] = [] + for part in parts: + t = self._extract_target(part) + if t: + targets.append(t) + return Task(targets=targets, raw=raw) + + def _extract_target(self, text: str) -> str: + text = text.strip().rstrip(".?!") + for p in self._SINGLE_PATTERNS: + m = p.match(text) + if m: + return m.group(1).strip() + prefix = re.match(r"^\s*(?:the\s+)?(.+)$", text, re.IGNORECASE) + if prefix: + return prefix.group(1).strip() + return text + + +# ============== deterministic agent ====================================== # + + +@dataclass(frozen=True) +class AgentConfig: + """Agent policy knobs.""" + + max_explore_iters: int = 5 + """How many ``explore → relocate`` loops before giving up on a target.""" + + explore_step_uses_goto: bool = True + """Drive to the explore frontier via closed-loop ``goto``. False + teleports instead (fast offline eval).""" + + +class DeterministicAgent: + """Executes a :class:`Task` via a fixed policy. + + For each target: locate; if found, goto and done; else explore(query), + goto the frontier, and relocate — up to ``max_explore_iters``, then give + up. The control flow is plain Python; no LLM in the loop. + """ + + def __init__(self, skills: SpatialSkills, cfg: AgentConfig | None = None) -> None: + self.skills = skills + self.cfg = cfg or AgentConfig() + + def execute(self, task: Task) -> AgentResult: + out: list[TargetResult] = [] + for target in task.targets: + out.append(self._execute_target(target)) + if not out[-1].reached: + # Don't auto-skip after a failed multi-step leg; bail so the + # caller sees the failure clearly. + break + return AgentResult(task=task, target_results=out) + + def execute_command(self, command: str, parser: TaskParser) -> AgentResult: + """Parse a free-text command, then execute.""" + task = parser.parse(command) + if not task.targets: + return AgentResult( + task=task, + target_results=[ + TargetResult( + target="", + reached=False, + final_xyz=None, + n_explore_iters=0, + confidence=-1.0, + reason="parse_empty", + ) + ], + ) + return self.execute(task) + + # ----- single-target inner loop ---------------------------------------- + + def _execute_target(self, target: str) -> TargetResult: + last_conf = -1.0 + for it in range(self.cfg.max_explore_iters + 1): + loc = self.skills.locate(target) + last_conf = loc.confidence + if loc.found and loc.xyz is not None: + LOG.info( + "agent: locate(%r) found at %s (conf %.3f); goto", + target, + loc.xyz, + loc.confidence, + ) + gr = self.skills.goto(loc.xyz) + return TargetResult( + target=target, + reached=gr.reached, + final_xyz=gr.final_xyz, + n_explore_iters=it, + confidence=loc.confidence, + reason="ok" if gr.reached else gr.reason, + ) + + if it >= self.cfg.max_explore_iters: + LOG.info( + "agent: locate(%r) NOT_FOUND (conf %.3f) and explore budget exhausted", + target, + loc.confidence, + ) + return TargetResult( + target=target, + reached=False, + final_xyz=None, + n_explore_iters=it, + confidence=loc.confidence, + reason="budget_exhausted", + ) + + # NOT_FOUND → explore once, then loop and re-locate. + LOG.info( + "agent: locate(%r) NOT_FOUND (conf %.3f) → explore iter %d", + target, + loc.confidence, + it + 1, + ) + ex = self.skills.explore(query=target) + if not ex.found_frontier or ex.target_xyz is None: + return TargetResult( + target=target, + reached=False, + final_xyz=None, + n_explore_iters=it, + confidence=loc.confidence, + reason="no_frontier", + ) + if self.cfg.explore_step_uses_goto: + self.skills.goto(ex.target_xyz) + else: + # Teleport for offline-eval speed. + self.skills.base.move(0.0, 0.0, dt=0.0) + pose = self.skills.base.pose() + pose[0, 3] = ex.target_xyz[0] + pose[2, 3] = ex.target_xyz[2] + if hasattr(self.skills.base, "_pose"): + self.skills.base._pose = pose # noqa: SLF001 + + return TargetResult( + target=target, + reached=False, + final_xyz=None, + n_explore_iters=self.cfg.max_explore_iters, + confidence=last_conf, + reason="budget_exhausted", + ) diff --git a/src/lerobot/navigation/dog_cli.py b/src/lerobot/navigation/dog_cli.py new file mode 100644 index 000000000..89c71eecd --- /dev/null +++ b/src/lerobot/navigation/dog_cli.py @@ -0,0 +1,194 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""``dog-nav`` — interactive spatial-memory navigation REPL. + +Behaviour: + - **No prompt** (idle) → the base explores autonomously: value-map + frontier selection, A* on the live occupancy map, obstacle-gated + motion. The map grows/refreshes as it goes. + - **Typed prompt** (e.g. ``find the couch``) → query the map; if a + confident match exists, navigate to it; otherwise explore until it is + found (or the budget is exhausted), then resume idle exploring. + +A new prompt preempts the current goal. Ctrl-C latches an e-stop and +exits. ``--dry-run`` runs the whole loop against a synthetic scene with no +robot, camera, or models — the default until the live geometry pipeline +(LingBot-Map) is wired. + +Run: ``python -m lerobot.navigation.dog_cli --dry-run`` and type object +names; empty line ⇒ one exploration step; ``quit`` ⇒ exit. +""" + +from __future__ import annotations + +import argparse +import logging +import select +import sys + +from lerobot.navigation.agent import ( + AgentConfig, + AgentResult, + DeterministicAgent, + HardcodedTaskParser, +) +from lerobot.navigation.skills import ExploreResult, SkillsConfig, SpatialSkills + +LOG = logging.getLogger("dog-nav") + + +class DogController: + """The behaviour loop over a :class:`SpatialSkills` toolset. + + Construct with a ready ``SpatialSkills`` (real robot or synthetic + scene). :meth:`handle_prompt` runs a full locate/goto/explore task; + :meth:`idle_tick` runs one autonomous exploration step. Both are + plain calls, so the REPL and the tests share the same code. + """ + + def __init__( + self, + skills: SpatialSkills, + agent: DeterministicAgent | None = None, + parser: HardcodedTaskParser | None = None, + ) -> None: + self.skills = skills + self.agent = agent or DeterministicAgent(skills) + self.parser = parser or HardcodedTaskParser() + + def handle_prompt(self, text: str) -> AgentResult: + """Query the map and navigate to the target (exploring if needed).""" + LOG.info("prompt: %r", text) + result = self.agent.execute_command(text, self.parser) + for tr in result.target_results: + if tr.reached: + LOG.info(" reached %r at %s (conf %.3f)", tr.target, tr.final_xyz, tr.confidence) + else: + LOG.info(" did not reach %r: %s (conf %.3f)", tr.target, tr.reason, tr.confidence) + return result + + def idle_tick(self) -> ExploreResult: + """One autonomous exploration step: pick a frontier and drive to it.""" + ex = self.skills.explore(query=None) + if ex.found_frontier and ex.target_xyz is not None: + LOG.info("idle: exploring toward %s (value %.3f)", ex.target_xyz, ex.value) + self.skills.goto(ex.target_xyz) + else: + LOG.debug("idle: no frontier to explore (%s)", ex.reason) + return ex + + def stop(self) -> None: + self.skills.base.stop() + + +def _build_dry_run() -> DogController: + """Wire the controller against the synthetic kitchen scene.""" + from lerobot.navigation.base_controller import StubBaseController + from lerobot.navigation.sim import kitchen_scene + + scene = kitchen_scene() + base = StubBaseController() + siglip = scene.feature_extractor() + skills = SpatialSkills( + scene.voxel_map, + base, + siglip, + SkillsConfig( + cell_size=0.2, + obstacle_inflate_cells=0, + goto_threshold=1.0, + goto_max_steps=300, + locate_threshold=0.5, + ), + ) + agent = DeterministicAgent(skills, AgentConfig(max_explore_iters=4)) + objs = ", ".join(o.name for o in scene.objects) + LOG.info("dry-run kitchen scene ready — try one of: %s", objs) + return DogController(skills, agent) + + +def _stdin_line_ready(timeout_s: float) -> bool: + """True when a full line is available on stdin within ``timeout_s``. + + Uses ``select`` so idle ticks keep running while we wait for input. + Falls back to blocking reads where ``select`` on stdin isn't supported + (e.g. some Windows terminals). + """ + try: + ready, _, _ = select.select([sys.stdin], [], [], timeout_s) + return bool(ready) + except (OSError, ValueError): + return True + + +def run_repl(controller: DogController, idle_period_s: float = 0.5) -> int: + """Interactive loop: explore while idle, run a task on each typed line.""" + print("dog-nav ready. Type an object to find it, empty line to explore, 'quit' to exit.") + try: + while True: + if _stdin_line_ready(idle_period_s): + line = sys.stdin.readline() + if not line: # EOF + break + text = line.strip() + if text.lower() in {"quit", "exit"}: + break + if text: + controller.handle_prompt(text) # a new prompt preempts idle + else: + controller.idle_tick() + else: + controller.idle_tick() + except KeyboardInterrupt: + LOG.warning("interrupted — stopping base") + finally: + controller.stop() + return 0 + + +def main(argv: list[str] | None = None) -> int: + ap = argparse.ArgumentParser(prog="dog-nav", description=__doc__) + ap.add_argument( + "--dry-run", + action="store_true", + help="Run against a synthetic scene (no robot/camera/models). " + "Currently the only supported mode until the live geometry pipeline lands.", + ) + ap.add_argument("--command", default=None, help="Run a single command non-interactively, then exit.") + ap.add_argument("--log-level", default="INFO", choices=["DEBUG", "INFO", "WARNING"]) + args = ap.parse_args(argv) + + logging.basicConfig( + level=getattr(logging, args.log_level), format="%(levelname)-7s %(name)s: %(message)s" + ) + + if not args.dry_run: + raise SystemExit( + "Live mode needs the geometry pipeline (LingBot-Map + segment map), which is not " + "wired yet. Run with --dry-run for now." + ) + + controller = _build_dry_run() + if args.command is not None: + result = controller.handle_prompt(args.command) + controller.stop() + return 0 if result.fully_successful else 1 + return run_repl(controller) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/lerobot/navigation/features.py b/src/lerobot/navigation/features.py new file mode 100644 index 000000000..1bd30bc16 --- /dev/null +++ b/src/lerobot/navigation/features.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""SigLIP2 dense patch features (MaskCLIP-style) + text query encoding. + +Ported from the dyna360 research stack. Default checkpoint +``google/siglip2-so400m-patch16-384``. For per-patch dense matching +against text, raw ``last_hidden_state`` is the wrong space: SigLIP2's +image-text matching lives in the MAP (Multihead Attention Pooling) head +output. We use the MaskCLIP recipe — apply the MAP head's value +projection + output projection + LayerNorm + MLP residual to each patch +token, skipping the attention reduction — so each patch lands in +(approximately) the shared text/vision space. Outputs are L2-normalized +fp16. + +For dry-run and tests, :class:`BasisVectorFeatureExtractor` provides a +deterministic name→vector stand-in with the same interface, no models +required. +""" + +from __future__ import annotations + +import logging +from contextlib import nullcontext +from typing import Any, Protocol, runtime_checkable + +import numpy as np + +LOG = logging.getLogger(__name__) + +DEFAULT_CHECKPOINT = "google/siglip2-so400m-patch16-384" + + +@runtime_checkable +class FeatureExtractor(Protocol): + """What the navigation stack needs from a vision-language encoder. + + ``encode_text`` is required (used by ``locate``/``explore`` queries); + ``feature_dim`` reports the embedding size. Dense image encoding + (``encode_views``) is only needed by the live mapping pipeline. + """ + + @property + def feature_dim(self) -> int: ... + + def encode_text(self, text: str) -> np.ndarray: ... + + +def _select_autocast(device: str) -> tuple[Any, str]: + """Pick an autocast context + label for the given device.""" + import torch + + if device != "cuda": + return nullcontext(), "no-autocast" + if not torch.cuda.is_available(): + raise RuntimeError("device='cuda' requested but torch.cuda.is_available() is False") + cap = torch.cuda.get_device_capability()[0] + dtype = torch.bfloat16 if cap >= 8 else torch.float16 + return torch.amp.autocast("cuda", dtype=dtype), f"cuda/{str(dtype).split('.')[-1]}" + + +class SiglipFeatureExtractor: + """Lazy-loaded SigLIP2 wrapper for dense patch features + text query.""" + + def __init__( + self, + checkpoint: str = DEFAULT_CHECKPOINT, + device: str = "cuda", + max_batch: int = 8, + ) -> None: + self.checkpoint = checkpoint + self.device = device + self.max_batch = int(max_batch) + self._model: Any | None = None + self._processor: Any | None = None + self._patch_grid: tuple[int, int] | None = None + self._feature_dim: int | None = None + + @property + def feature_dim(self) -> int: + if self._feature_dim is None: + raise RuntimeError("SigLIP2 not loaded yet; call encode_views first") + return self._feature_dim + + @property + def patch_grid(self) -> tuple[int, int]: + if self._patch_grid is None: + raise RuntimeError("SigLIP2 not loaded yet; call encode_views first") + return self._patch_grid + + def _ensure_loaded(self) -> None: + if self._model is not None: + return + from transformers import AutoModel, AutoProcessor + + LOG.info("loading SigLIP2 (%s) on %s ...", self.checkpoint, self.device) + self._processor = AutoProcessor.from_pretrained(self.checkpoint) + self._model = AutoModel.from_pretrained(self.checkpoint).to(self.device).eval() + LOG.info("SigLIP2 loaded") + + def _maskclip_project(self, patches): + """Push raw patch tokens through the MAP head with the attention + reduction removed — value-projects + post-processes each patch so it + lives in the shared text/vision space. ``patches``: (B, P, D).""" + import torch + + assert self._model is not None + head = self._model.vision_model.head + mha = head.attention # nn.MultiheadAttention + embed_dim = patches.shape[-1] + + # in_proj_weight is concatenated [Q | K | V], (3*D, D). Slice out V. + v_weight = mha.in_proj_weight[2 * embed_dim : 3 * embed_dim] + v_bias = mha.in_proj_bias[2 * embed_dim : 3 * embed_dim] if mha.in_proj_bias is not None else None + v = torch.nn.functional.linear(patches, v_weight, v_bias) # (B, P, D) + v = mha.out_proj(v) + + residual = v + v = head.layernorm(v) + v = residual + head.mlp(v) + return v + + def encode_views(self, views_rgb_uint8: np.ndarray) -> np.ndarray: + """Encode ``(N, H, W, 3)`` RGB uint8 views to ``(N, Hp, Wp, D)`` fp16 + dense patch features in the shared text/vision space, L2-normalized.""" + import torch + + if views_rgb_uint8.ndim != 4 or views_rgb_uint8.shape[-1] != 3: # noqa: N806 + raise ValueError(f"expected (N, H, W, 3), got {views_rgb_uint8.shape}") + if views_rgb_uint8.dtype != np.uint8: + raise ValueError(f"expected uint8, got {views_rgb_uint8.dtype}") + self._ensure_loaded() + assert self._model is not None and self._processor is not None + + autocast_ctx, autocast_label = _select_autocast(self.device) + LOG.info( + "SigLIP2 forward (MaskCLIP-projected patches): N=%d (batched up to %d), %s", + views_rgb_uint8.shape[0], + self.max_batch, + autocast_label, + ) + + out_list: list[np.ndarray] = [] + for s in range(0, views_rgb_uint8.shape[0], self.max_batch): + e = s + self.max_batch + chunk = [views_rgb_uint8[i] for i in range(s, min(e, views_rgb_uint8.shape[0]))] + inputs = self._processor(images=chunk, return_tensors="pt").to(self.device) + with torch.no_grad(), autocast_ctx: + vision = self._model.vision_model(**inputs) + patches = vision.last_hidden_state # (B, P, D) + patches = self._maskclip_project(patches) # (B, P, D) shared-space + patches = torch.nn.functional.normalize(patches.float(), dim=-1) + out_list.append(patches.to(torch.float16).cpu().numpy()) + + feats = np.concatenate(out_list, axis=0) # (N, P, D) + n, p, d = feats.shape + side = int(round(p**0.5)) + if side * side != p: + raise RuntimeError( + f"SigLIP2 returned a non-square patch grid (P={p}); non-square inputs aren't supported yet" + ) + self._patch_grid = (side, side) + self._feature_dim = d + return feats.reshape(n, side, side, d) + + def encode_text(self, text: str) -> np.ndarray: + """Encode a text query to a single (D,) fp16 unit vector. + + SigLIP2 uses last-token ([EOS]) pooling for text. We extract it + explicitly because ``get_text_features`` behaves differently across + ``transformers`` versions. + """ + import torch + + self._ensure_loaded() + assert self._model is not None and self._processor is not None + autocast_ctx, _ = _select_autocast(self.device) + inputs = self._processor(text=[text], return_tensors="pt", padding="max_length").to(self.device) + with torch.no_grad(), autocast_ctx: + text_outputs = self._model.text_model(**inputs) + + pooled = getattr(text_outputs, "pooler_output", None) + if pooled is not None and pooled.dim() == 2: + feat = pooled[0] + else: + feat = text_outputs.last_hidden_state[0, -1] + + feat = feat.float() + feat = torch.nn.functional.normalize(feat, dim=-1) + return feat.to(torch.float16).cpu().numpy() + + +class BasisVectorFeatureExtractor: + """Deterministic name→vector stand-in for :class:`SiglipFeatureExtractor`. + + Maps known names to their stored feature vectors; unknown queries get a + deterministic per-text pseudo-random unit vector (same string → same + vector), so a locate threshold reliably rejects absent objects. Used by + the synthetic-scene dry-run and by tests — no models required. + """ + + def __init__(self, name_to_vec: dict[str, np.ndarray], feature_dim: int) -> None: + self.name_to_vec = name_to_vec + self._feature_dim = int(feature_dim) + + @property + def feature_dim(self) -> int: + return self._feature_dim + + def encode_text(self, text: str) -> np.ndarray: + v = self.name_to_vec.get(text) + if v is None: + seed = abs(hash(text)) % (2**32) + rng = np.random.default_rng(seed) + v = rng.normal(size=self._feature_dim).astype(np.float32) + v = v.astype(np.float32) + v = v / max(float(np.linalg.norm(v)), 1e-6) + return v diff --git a/src/lerobot/navigation/sim.py b/src/lerobot/navigation/sim.py new file mode 100644 index 000000000..daffbe0d9 --- /dev/null +++ b/src/lerobot/navigation/sim.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Synthetic scenes for hardware-free dry-runs and tests. + +Ported from the dyna360 eval harness. A :class:`SyntheticScene` is a +deterministic hand-crafted :class:`~lerobot.navigation.voxel_map.VoxelMap` +— a navigable floor plus labelled objects each carrying a unit feature +vector — paired with a +:class:`~lerobot.navigation.features.BasisVectorFeatureExtractor` whose +text encodings live in the same space. This lets ``dog_cli --dry-run`` +(and the tests) exercise the full locate/goto/explore stack with no +models, camera, or robot. +""" + +from __future__ import annotations + +import logging +from dataclasses import dataclass + +import numpy as np + +from lerobot.navigation.features import BasisVectorFeatureExtractor +from lerobot.navigation.voxel_map import VoxelMap + +LOG = logging.getLogger(__name__) + + +@dataclass(frozen=True) +class SyntheticObject: + """One labelled object. ``feature_vec`` lives in the same space as the + text embeddings fed to ``VoxelMap.query`` (one-hot basis vectors, so a + query hits the right cluster cleanly).""" + + name: str + xyz: tuple[float, float, float] + half_extent_m: float + feature_vec: np.ndarray + + +@dataclass(frozen=True) +class SyntheticScene: + """A ground-truth scene: voxel map + object metadata.""" + + voxel_map: VoxelMap + objects: list[SyntheticObject] + floor_extent_m: float + voxel_size: float + feature_dim: int + + def name_to_xyz(self) -> dict[str, tuple[float, float, float]]: + return {o.name: o.xyz for o in self.objects} + + def object(self, name: str) -> SyntheticObject | None: + for o in self.objects: + if o.name == name: + return o + return None + + def feature_extractor(self) -> BasisVectorFeatureExtractor: + """A text encoder whose vectors match this scene's object features.""" + table = {o.name: o.feature_vec for o in self.objects} + return BasisVectorFeatureExtractor(table, self.feature_dim) + + +@dataclass(frozen=True) +class SceneSpec: + """Declarative recipe used by :func:`build_scene`.""" + + objects: list[SyntheticObject] + floor_extent_m: float = 6.0 + voxel_size: float = 0.1 + feature_dim: int = 8 + ground_y: float = 1.0 + object_density_per_dim: int = 5 + wall_xz_range: tuple[float, float, float, float] | None = None + """Optional axis-aligned wall ``(x_min, z_min, x_max, z_max)`` of + obstacle voxels at robot height — to test ``goto`` against a block.""" + feature_noise: float = 0.0 + rng_seed: int = 0 + + +def basis_vec(dim: int, idx: int) -> np.ndarray: + """A unit basis vector of length ``dim`` with a 1 at ``idx``.""" + v = np.zeros(dim, dtype=np.float32) + v[idx] = 1.0 + return v + + +def build_scene(spec: SceneSpec) -> SyntheticScene: + """Construct a deterministic :class:`SyntheticScene` from a spec.""" + rng = np.random.default_rng(spec.rng_seed) + vm = VoxelMap(voxel_size=spec.voxel_size) + + # ----- floor (NAVIGABLE) ----- + half = spec.voxel_size / 2.0 + floor_pts: list[tuple[float, float, float]] = [] + for x in np.arange(-spec.floor_extent_m + half, spec.floor_extent_m + half, spec.voxel_size): + for z in np.arange(-spec.floor_extent_m + half, spec.floor_extent_m + half, spec.voxel_size): + floor_pts.append((float(x), spec.ground_y, float(z))) + arr = np.asarray(floor_pts, dtype=np.float64).reshape(-1, 1, 3) + rgb = np.full((len(floor_pts), 1, 3), 180, dtype=np.uint8) + conf = np.ones((len(floor_pts), 1), dtype=np.float32) + if spec.feature_dim >= 1: + floor_vec = np.zeros(spec.feature_dim, dtype=np.float16) + floor_vec[-1] = 1.0 + floor_feat = np.tile(floor_vec, (len(floor_pts), 1, 1)) + vm.add(arr, rgb, conf, frame=0, t=0.0, feat_map=floor_feat) + else: + vm.add(arr, rgb, conf, frame=0, t=0.0) + + # ----- objects ----- + for i, obj in enumerate(spec.objects, start=1): + d = obj.half_extent_m + n = spec.object_density_per_dim + coords = np.linspace(-d + half, d - half, n) + pts = np.array( + [ + (float(obj.xyz[0] + dx), float(obj.xyz[1] + dy), float(obj.xyz[2] + dz)) + for dx in coords + for dy in coords + for dz in coords + ], + dtype=np.float64, + ).reshape(-1, 1, 3) + rgb_o = np.full((pts.shape[0], 1, 3), 100 + (i * 30) % 156, dtype=np.uint8) + conf_o = np.ones((pts.shape[0], 1), dtype=np.float32) + + if obj.feature_vec.shape != (spec.feature_dim,): + raise ValueError( + f"object {obj.name!r} feature_vec has shape {obj.feature_vec.shape}, " + f"expected ({spec.feature_dim},) to match SceneSpec.feature_dim" + ) + base = obj.feature_vec.astype(np.float32).reshape(1, 1, -1) + feats = np.tile(base, (pts.shape[0], 1, 1)) + if spec.feature_noise > 0: + noise = rng.normal(scale=spec.feature_noise, size=feats.shape).astype(np.float32) + feats = feats + noise + norms = np.linalg.norm(feats, axis=-1, keepdims=True) + feats = feats / np.maximum(norms, 1e-6) + vm.add(pts, rgb_o, conf_o, frame=i, t=float(i), feat_map=feats.astype(np.float16)) + + # ----- optional wall (OBSTACLE) ----- + if spec.wall_xz_range is not None: + wx0, wz0, wx1, wz1 = spec.wall_xz_range + wall_pts = [ + (float(x), float(y), float(z)) + for x in np.arange(wx0 + half, wx1, spec.voxel_size) + for z in np.arange(wz0 + half, wz1, spec.voxel_size) + for y in np.arange(spec.ground_y - 1.0, spec.ground_y - 0.1, spec.voxel_size) + ] + if wall_pts: + pts = np.asarray(wall_pts, dtype=np.float64).reshape(-1, 1, 3) + rgb_w = np.full((len(wall_pts), 1, 3), 80, dtype=np.uint8) + conf_w = np.ones((len(wall_pts), 1), dtype=np.float32) + vm.add(pts, rgb_w, conf_w, frame=99, t=99.0) + + LOG.info( + "built scene: %d voxels, %d objects, floor extent %.1f m, D=%d", + len(vm), + len(spec.objects), + spec.floor_extent_m, + spec.feature_dim, + ) + return SyntheticScene( + voxel_map=vm, + objects=list(spec.objects), + floor_extent_m=spec.floor_extent_m, + voxel_size=spec.voxel_size, + feature_dim=spec.feature_dim, + ) + + +_KITCHEN_DIM = 64 # Feature dim sized so the random-direction noise floor +# (≈1/sqrt(D) ≈ 0.125) sits well below a sane locate threshold, so an absent +# object reliably ABSTAINS instead of hitting a known basis vector. + + +def kitchen_scene(wall: tuple[float, float, float, float] | None = None) -> SyntheticScene: + """A 6×6 m floor with four labelled objects at distinctive corners.""" + spec = SceneSpec( + objects=[ + SyntheticObject("couch", (3.0, 0.5, 2.0), 0.3, basis_vec(_KITCHEN_DIM, 0)), + SyntheticObject("chair", (-2.0, 0.5, -1.5), 0.2, basis_vec(_KITCHEN_DIM, 1)), + SyntheticObject("lamp", (2.5, 0.5, -2.0), 0.15, basis_vec(_KITCHEN_DIM, 2)), + SyntheticObject("plant", (-2.5, 0.5, 2.5), 0.25, basis_vec(_KITCHEN_DIM, 3)), + ], + floor_extent_m=6.0, + voxel_size=0.1, + feature_dim=_KITCHEN_DIM, + ground_y=1.0, + wall_xz_range=wall, + ) + return build_scene(spec) diff --git a/src/lerobot/navigation/skills.py b/src/lerobot/navigation/skills.py new file mode 100644 index 000000000..f1da7c4fb --- /dev/null +++ b/src/lerobot/navigation/skills.py @@ -0,0 +1,321 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""SpatialSkills tool layer. + +Ported from the dyna360 research stack. The agent calls these as a fixed +toolset: + + - :meth:`SpatialSkills.locate` — text → 3D position (or NOT_FOUND) + - :meth:`SpatialSkills.goto` — base navigation to a 3D target + - :meth:`SpatialSkills.explore` — pick a frontier to drive toward + +The skills compose a :class:`~lerobot.navigation.voxel_map.VoxelMap` +(geometry + semantic features) with a +:class:`~lerobot.navigation.base_controller.BaseController` (motion) and a +text encoder. Stateless-per-call: each call snapshots the world, does its +work, and hands control back. The agent decides what to call next. +""" + +from __future__ import annotations + +import logging +import math +from dataclasses import dataclass, field +from typing import TYPE_CHECKING + +import numpy as np + +from lerobot.navigation.occupancy import ( + OccupancyGrid, + astar, + find_frontier_cells, + project_voxel_map_to_grid, +) +from lerobot.navigation.value_map import ( + ValueMapConfig, + compute_value_maps, + pick_best_frontier_cell, +) + +if TYPE_CHECKING: + from lerobot.navigation.base_controller import BaseController + from lerobot.navigation.features import FeatureExtractor + from lerobot.navigation.voxel_map import VoxelMap + +LOG = logging.getLogger(__name__) + + +# ----- typed results returned to the agent ------------------------------- + + +@dataclass(frozen=True) +class LocateResult: + """Output of :meth:`SpatialSkills.locate`. + + ``found=False`` is load-bearing — the signal the agent uses to pick + :meth:`explore` over :meth:`goto`. Don't fabricate an ``xyz`` when + abstaining. + """ + + found: bool + xyz: tuple[float, float, float] | None + confidence: float # top cosine score; -1.0 if no features + n_voxels: int # how many voxels supported the cluster + text: str + + +@dataclass(frozen=True) +class GotoResult: + """Output of :meth:`SpatialSkills.goto`.""" + + reached: bool + final_xyz: tuple[float, float, float] + distance_to_target: float + n_steps: int + reason: str # "ok" | "no path" | "max steps" | "blocked" + path_xyz: list[tuple[float, float, float]] # for viz / debugging + + +@dataclass(frozen=True) +class ExploreResult: + """Output of :meth:`SpatialSkills.explore`.""" + + target_xyz: tuple[float, float, float] | None + found_frontier: bool + distance_to_target: float # 0.0 when no frontier + reason: str # "ok" | "no frontier" | ... + value: float = 0.0 + """Combined V_T + α·V_S value of the chosen frontier — useful for + debugging exploration bias and as a give-up signal for the agent.""" + + +# ----- configuration ------------------------------------------------------ + + +@dataclass(frozen=True) +class SkillsConfig: + """Knobs shared across the skills.""" + + # Occupancy projection + cell_size: float = 0.1 + ground_y: float | None = None # None ⇒ auto-estimate from voxels + obstacle_y_range: tuple[float, float] = (-2.0, -0.1) # m above ground (y-down) + obstacle_inflate_cells: int = 1 + + # locate() + locate_top_k: int = 128 + locate_threshold: float = 0.15 # min cosine for found=True + locate_outlier_quantile: float = 0.5 + locate_outlier_scale: float = 2.0 + + # goto() + goto_threshold: float = 0.3 + goto_step_size: float = 0.2 # m advanced per controller tick + goto_max_steps: int = 500 + goto_replan_every: int = 5 + goto_dt: float = 0.1 + + # explore() + explore_max_frontiers: int = 256 + value_cfg: ValueMapConfig = field(default_factory=ValueMapConfig) + """DynaMem-style V_T (recency) + V_S (similarity) knobs.""" + + +# ----- the skills layer --------------------------------------------------- + + +class SpatialSkills: + """Composes the voxel memory + base + text encoder into the agent toolset.""" + + def __init__( + self, + voxel_map: VoxelMap, + base: BaseController, + siglip: FeatureExtractor | None = None, + cfg: SkillsConfig | None = None, + ) -> None: + self.voxel_map = voxel_map + self.base = base + self.siglip = siglip + self.cfg = cfg or SkillsConfig() + + # ----- shared helper --------------------------------------------------- + + def occupancy(self) -> OccupancyGrid: + """Project the *current* voxel map into a 2D occupancy grid.""" + return project_voxel_map_to_grid( + self.voxel_map, + cell_size=self.cfg.cell_size, + ground_y=self.cfg.ground_y, + obstacle_y_range=self.cfg.obstacle_y_range, + inflate_cells=self.cfg.obstacle_inflate_cells, + ) + + # ----- locate(text) ---------------------------------------------------- + + def locate(self, text: str) -> LocateResult: + text = text.strip() + if not text: + return LocateResult(False, None, -1.0, 0, text) + if self.siglip is None: + return LocateResult(False, None, -1.0, 0, text) + if self.voxel_map.feature_dim is None: + return LocateResult(False, None, -1.0, 0, text) + + text_emb = self.siglip.encode_text(text) + qr = self.voxel_map.query(text_emb, top_k=self.cfg.locate_top_k) + if qr.score.size == 0: + return LocateResult(False, None, -1.0, 0, text) + top_score = float(qr.score.max()) + if top_score < self.cfg.locate_threshold: + LOG.info( + "locate(%r): top score %.3f < threshold %.3f → NOT_FOUND", + text, + top_score, + self.cfg.locate_threshold, + ) + return LocateResult(False, None, top_score, 0, text) + + # Score-weighted centroid, then outlier rejection (anchor against the + # cluster median distance so a couple of stray voxels in the top-k + # can't drag the centroid into empty space). + scores = qr.score.astype(np.float64) + weights = scores - scores.min() + 1e-6 + centroid = (qr.xyz * weights[:, None]).sum(axis=0) / weights.sum() + d = np.linalg.norm(qr.xyz - centroid, axis=1) + thresh = max( + self.cfg.cell_size * 4, + float(np.quantile(d, self.cfg.locate_outlier_quantile)) * self.cfg.locate_outlier_scale, + ) + inliers = d <= thresh + if inliers.sum() >= 3: + inlier_xyz = qr.xyz[inliers] + inlier_w = weights[inliers] + centroid = (inlier_xyz * inlier_w[:, None]).sum(axis=0) / inlier_w.sum() + return LocateResult( + True, + (float(centroid[0]), float(centroid[1]), float(centroid[2])), + top_score, + int(inliers.sum()), + text, + ) + + # ----- goto(xyz) ------------------------------------------------------- + + def goto( + self, + target_xyz: tuple[float, float, float], + *, + max_steps: int | None = None, + threshold: float | None = None, + ) -> GotoResult: + """Closed-loop nav: A* → step a few cells → replan → repeat. + + The replan cadence makes this a staleness governor — a moving + obstacle (or a previously-mapped one that got carved out) is picked + up at the next replan. + """ + max_steps = max_steps if max_steps is not None else self.cfg.goto_max_steps + threshold = threshold if threshold is not None else self.cfg.goto_threshold + + path_xyz_global: list[tuple[float, float, float]] = [] + n_steps = 0 + last_path: list[tuple[float, float]] = [] + + for step in range(max_steps): + pos = self.base.position() + d = math.hypot(pos[0] - target_xyz[0], pos[2] - target_xyz[2]) + if d <= threshold: + return GotoResult(True, pos, d, n_steps, "ok", path_xyz_global) + + if step % self.cfg.goto_replan_every == 0 or not last_path: + grid = self.occupancy() + last_path = ( + astar( + grid, + start_world=(pos[0], pos[2]), + goal_world=(target_xyz[0], target_xyz[2]), + ) + or [] + ) + if not last_path or len(last_path) < 2: + return GotoResult(False, pos, d, n_steps, "no path", path_xyz_global) + + # Head toward the next-but-one cell to smooth corners. + next_idx = min(2, len(last_path) - 1) + target_xz = last_path[next_idx] + dx = target_xz[0] - pos[0] + dz = target_xz[1] - pos[2] + n = math.hypot(dx, dz) + if n < 1e-6: + last_path.pop(0) + continue + vx = self.cfg.goto_step_size / max(self.cfg.goto_dt, 1e-6) * dx / n + vz = self.cfg.goto_step_size / max(self.cfg.goto_dt, 1e-6) * dz / n + self.base.move(vx=vx, vz=vz, dt=self.cfg.goto_dt) + pos = self.base.position() + path_xyz_global.append(pos) + n_steps += 1 + + # Pop waypoint when we've crossed it. + if math.hypot(target_xz[0] - pos[0], target_xz[1] - pos[2]) < self.cfg.cell_size: + last_path.pop(0) + if not last_path: + last_path = [] # force replan + + pos = self.base.position() + d = math.hypot(pos[0] - target_xyz[0], pos[2] - target_xyz[2]) + return GotoResult(False, pos, d, n_steps, "max steps", path_xyz_global) + + # ----- explore() ------------------------------------------------------- + + def explore(self, query: str | None = None) -> ExploreResult: + """Pick a frontier to drive toward via the DynaMem §3.4 value map. + + With no query this is pure recency (visit oldest-observed or + UNOBSERVED frontiers first); with a query + features it biases + toward semantic matches. + """ + grid = self.occupancy() + cells = find_frontier_cells(grid) + if cells.shape[0] == 0: + return ExploreResult(None, False, 0.0, "no frontier") + + # Subsample if huge so the loop stays fast even on big maps. + if cells.shape[0] > self.cfg.explore_max_frontiers: + idx = np.random.default_rng(0).choice( + cells.shape[0], self.cfg.explore_max_frontiers, replace=False + ) + cells = cells[idx] + + text_emb = None + if query is not None and self.siglip is not None and self.voxel_map.feature_dim is not None: + text_emb = self.siglip.encode_text(query) + + values = compute_value_maps(self.voxel_map, grid, text_emb=text_emb, cfg=self.cfg.value_cfg) + + pos = self.base.position() + _, (xt, zt), dist, score = pick_best_frontier_cell( + grid, cells, values, robot_position_xz=(pos[0], pos[2]), cfg=self.cfg.value_cfg + ) + return ExploreResult( + target_xyz=(xt, grid.ground_y, zt), + found_frontier=True, + distance_to_target=dist, + reason="ok", + value=score, + ) diff --git a/src/lerobot/navigation/value_map.py b/src/lerobot/navigation/value_map.py new file mode 100644 index 000000000..de44890b6 --- /dev/null +++ b/src/lerobot/navigation/value_map.py @@ -0,0 +1,221 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""DynaMem-style value maps for exploration. + +Ported from the dyna360 research stack. Two scalar fields over the same +occupancy grid as :mod:`occupancy`: + + - **V_T (time-recency)** — sigmoid of "how long ago was this cell last + observed?" Cells not seen in a while (or never) score high; freshly + observed cells score low. This biases exploration away from + just-covered territory. + - **V_S (query-similarity)** — sigmoid of the cosine between the cell's + aggregated feature and a text query. Only defined when a query is + given AND the voxel map carries features. + +Linear combination ``V = (1 − α)·V_T + α·V_S`` gates exploration. With no +query it is a pure recency-driven frontier walk; with a query it biases +toward regions semantically consistent with the target (DynaMem §3.4). +Maps are derived per-call from ``VoxelMap.snapshot`` so they inherit +carving for free. +""" + +# ruff: noqa: N806 — H, W, D are conventional array-dimension names +from __future__ import annotations + +import logging +import math +from dataclasses import dataclass + +import numpy as np + +from lerobot.navigation.occupancy import OccupancyGrid + +LOG = logging.getLogger(__name__) + + +@dataclass(frozen=True) +class ValueMapConfig: + """Knobs shared between recency and similarity value maps.""" + + recency_mid_s: float = 10.0 + """Age (s) at which V_T crosses 0.5 — older = more interesting.""" + + recency_scale_s: float = 8.0 + """How sharply V_T transitions around the mid age. Smaller = sharper.""" + + similarity_mid: float = 0.15 + """Cosine score at which V_S crosses 0.5.""" + + similarity_scale: float = 0.05 + """How sharply V_S transitions around the mid cosine.""" + + alpha_similarity: float = 0.6 + """Weight of V_S in the combined value when a query is given. + 0.0 = pure recency, 1.0 = pure similarity.""" + + unknown_value: float = 1.0 + """V_T for UNOBSERVED cells — they are maximally interesting.""" + + distance_discount_per_meter: float = 0.05 + """Multiplicative discount on far frontiers so the base does not + ping-pong across the map. 0 disables.""" + + +@dataclass(frozen=True) +class ValueMaps: + """The scalar fields, all shaped ``(H, W)`` like the occupancy grid.""" + + last_time: np.ndarray # float64 — −inf where UNOBSERVED + recency: np.ndarray # float32 V_T in [0, 1] + similarity: np.ndarray | None # float32 V_S in [0, 1], None when no query + combined: np.ndarray # float32 V — what explore() optimizes + + +# --------------------------------------------------------------------- # + + +def _eps_for_cell(cell_size: float) -> float: + """Same float32-drift epsilon as :mod:`occupancy` so the two + projections agree on which voxels land in which cells.""" + return cell_size * 1e-3 + + +def _project_voxels_to_cells(voxel_map, grid: OccupancyGrid, want_features: bool): + """Project every voxel into its XZ cell. + + Returns ``(last_time_per_cell, feat_per_cell)`` where last_time is + (H, W) float64 (−inf for empty cells) and feat_per_cell is + (H, W, D) float32 or None. + """ + snap = voxel_map.snapshot(include_features=want_features) + H, W = grid.shape + last_time = np.full((H, W), -math.inf, dtype=np.float64) + if snap.xyz.size == 0: + return last_time, None + + x = snap.xyz[:, 0].astype(np.float64) + z = snap.xyz[:, 2].astype(np.float64) + eps = _eps_for_cell(grid.cell_size) + ix = np.clip(np.floor((x - grid.origin_x) / grid.cell_size + eps).astype(np.int32), 0, W - 1) + iz = np.clip(np.floor((z - grid.origin_z) / grid.cell_size + eps).astype(np.int32), 0, H - 1) + + # Per-cell max last_time. `np.maximum.at` is the unbuffered ufunc version, + # which correctly handles duplicate (iz, ix) targets. + np.maximum.at(last_time, (iz, ix), snap.last_time.astype(np.float64)) + + feat_per_cell: np.ndarray | None = None + if want_features and snap.feat is not None and snap.feat.size > 0: + D = snap.feat.shape[1] + feat_sum = np.zeros((H, W, D), dtype=np.float32) + np.add.at(feat_sum, (iz, ix), snap.feat.astype(np.float32)) + counts = np.zeros((H, W), dtype=np.int32) + np.add.at(counts, (iz, ix), 1) + # Normalize per-cell — count is the number of CONTRIBUTING voxels. + denom = np.maximum(counts, 1).astype(np.float32)[..., None] + feat_per_cell = feat_sum / denom + + return last_time, feat_per_cell + + +def _recency_value(last_time_per_cell: np.ndarray, now_t: float, cfg: ValueMapConfig) -> np.ndarray: + """V_T per cell. Unobserved cells get ``cfg.unknown_value``.""" + out = np.full(last_time_per_cell.shape, cfg.unknown_value, dtype=np.float32) + observed = last_time_per_cell > -math.inf + if not observed.any(): + return out + age = (now_t - last_time_per_cell[observed]).astype(np.float32) + out[observed] = 1.0 / (1.0 + np.exp(-(age - cfg.recency_mid_s) / cfg.recency_scale_s)) + return out + + +def _similarity_value( + feat_per_cell: np.ndarray | None, + text_emb: np.ndarray | None, + cfg: ValueMapConfig, +) -> np.ndarray | None: + """V_S per cell. ``None`` when there are no features or no query.""" + if feat_per_cell is None or text_emb is None: + return None + text = text_emb.astype(np.float32) + text = text / max(float(np.linalg.norm(text)), 1e-6) + # Per-cell mean feat may not be unit-norm — renormalize so the dot product + # behaves like a cosine. Empty cells stay a 0 vector, so renorm clamps to 0. + norms = np.linalg.norm(feat_per_cell, axis=-1, keepdims=True) + feat_normed = feat_per_cell / np.maximum(norms, 1e-6) + with np.errstate(invalid="ignore", over="ignore", divide="ignore"): + cosine = np.nan_to_num((feat_normed @ text).astype(np.float32)) + sim = 1.0 / (1.0 + np.exp(-(cosine - cfg.similarity_mid) / cfg.similarity_scale)) + sim = np.where(norms.squeeze(-1) > 1e-6, sim, 0.0).astype(np.float32) + return sim + + +def compute_value_maps( + voxel_map, + grid: OccupancyGrid, + *, + text_emb: np.ndarray | None = None, + now_t: float | None = None, + cfg: ValueMapConfig | None = None, +) -> ValueMaps: + """Build the full value-map bundle for one ``explore`` call.""" + cfg = cfg or ValueMapConfig() + last_time, feat_per_cell = _project_voxels_to_cells(voxel_map, grid, want_features=(text_emb is not None)) + if now_t is None: + observed_mask = last_time > -math.inf + now_t = float(last_time[observed_mask].max()) if observed_mask.any() else 0.0 + + v_t = _recency_value(last_time, now_t, cfg) + v_s = _similarity_value(feat_per_cell, text_emb, cfg) + + if v_s is not None: + combined = ((1.0 - cfg.alpha_similarity) * v_t + cfg.alpha_similarity * v_s).astype(np.float32) + else: + combined = v_t + + return ValueMaps(last_time=last_time, recency=v_t, similarity=v_s, combined=combined) + + +def pick_best_frontier_cell( + grid: OccupancyGrid, + frontier_cells: np.ndarray, + values: ValueMaps, + robot_position_xz: tuple[float, float], + cfg: ValueMapConfig | None = None, +) -> tuple[int, tuple[float, float], float, float]: + """Score every frontier cell by ``values.combined`` (with a distance + discount) and return the winner. + + Returns ``(index_into_frontier_cells, (x, z), distance_m, score)``. + """ + if frontier_cells.shape[0] == 0: + raise ValueError("frontier_cells is empty") + cfg = cfg or ValueMapConfig() + + iz_f = frontier_cells[:, 0] + ix_f = frontier_cells[:, 1] + raw = values.combined[iz_f, ix_f] + + xs = grid.origin_x + (ix_f.astype(np.float64) + 0.5) * grid.cell_size + zs = grid.origin_z + (iz_f.astype(np.float64) + 0.5) * grid.cell_size + rx, rz = robot_position_xz + d = np.hypot(xs - rx, zs - rz) + discount = 1.0 / (1.0 + cfg.distance_discount_per_meter * d) + scored = raw * discount + + best = int(np.argmax(scored)) + return best, (float(xs[best]), float(zs[best])), float(d[best]), float(scored[best]) diff --git a/src/lerobot/navigation/voxel_map.py b/src/lerobot/navigation/voxel_map.py index e37fa1e1c..53e7a66a3 100644 --- a/src/lerobot/navigation/voxel_map.py +++ b/src/lerobot/navigation/voxel_map.py @@ -466,7 +466,10 @@ class VoxelMap: text_unit = text_embedding.astype(np.float32) text_unit = text_unit / max(float(np.linalg.norm(text_unit)), 1e-6) - scores = voxel_feat @ text_unit # (M,) + # fp16 feature storage can carry the odd inf/nan from a saturated + # running sum; the cosine stays well-defined, so don't warn on it. + with np.errstate(invalid="ignore", over="ignore", divide="ignore"): + scores = np.nan_to_num(voxel_feat @ text_unit) # (M,) k = min(int(top_k), len(scores)) # Partition-and-sort for the top-k. top_idx = np.argpartition(scores, -k)[-k:] diff --git a/tests/navigation/test_agent.py b/tests/navigation/test_agent.py new file mode 100644 index 000000000..67efa6bb1 --- /dev/null +++ b/tests/navigation/test_agent.py @@ -0,0 +1,248 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for the C1 deterministic agent + language parser.""" + +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np + +from lerobot.navigation.agent import ( + AgentConfig, + DeterministicAgent, + HardcodedTaskParser, + Task, +) +from lerobot.navigation.skills import ExploreResult, GotoResult, LocateResult + +# ----- fakes ------------------------------------------------------------- + + +@dataclass +class FakeSkills: + """Programmable :class:`SpatialSkills` stand-in. Each method consults a + pre-recorded script and bumps a call counter so tests can assert the + deterministic policy made the right sequence of calls.""" + + locate_script: list[LocateResult] = field(default_factory=list) + explore_script: list[ExploreResult] = field(default_factory=list) + goto_script: list[GotoResult] = field(default_factory=list) + + locate_calls: list[str] = field(default_factory=list) + goto_calls: list[tuple[float, float, float]] = field(default_factory=list) + explore_calls: list[str | None] = field(default_factory=list) + + def locate(self, text: str) -> LocateResult: + self.locate_calls.append(text) + if not self.locate_script: + return LocateResult(False, None, -1.0, 0, text) + return self.locate_script.pop(0) + + def explore(self, query: str | None = None) -> ExploreResult: + self.explore_calls.append(query) + if not self.explore_script: + return ExploreResult(None, False, 0.0, "no frontier") + return self.explore_script.pop(0) + + def goto(self, xyz: tuple[float, float, float], **_: object) -> GotoResult: + self.goto_calls.append(xyz) + if not self.goto_script: + return GotoResult(True, xyz, 0.0, 0, "ok", []) + return self.goto_script.pop(0) + + @property + def base(self): + # Minimal stub: agent's teleport branch isn't exercised by these tests. + class _Base: + def move(self, *a, **k): + pass + + def pose(self): + return np.eye(4) + + return _Base() + + +# ----- HardcodedTaskParser ------------------------------------------------ + + +def test_parser_simple_go_to(): + t = HardcodedTaskParser().parse("go to the mug") + assert t.targets == ["mug"] + + +def test_parser_strips_punctuation_and_articles(): + t = HardcodedTaskParser().parse("Find the red lamp.") + assert t.targets == ["red lamp"] + + +def test_parser_multi_step(): + t = HardcodedTaskParser().parse("go to the mug then the chair") + assert t.targets == ["mug", "chair"] + + +def test_parser_no_verb_treats_command_as_target(): + """``parser.parse('couch')`` should still produce a usable Task.""" + t = HardcodedTaskParser().parse("couch") + assert t.targets == ["couch"] + + +def test_parser_empty_string_returns_empty_task(): + t = HardcodedTaskParser().parse(" ") + assert t.targets == [] + + +def test_parser_split_by_comma(): + t = HardcodedTaskParser().parse("go to mug, chair") + assert t.targets == ["mug", "chair"] + + +# ----- DeterministicAgent policy ----------------------------------------- + + +def _ok_locate(xyz=(1.0, 0.0, 1.0), conf=0.9) -> LocateResult: + return LocateResult(True, xyz, conf, 10, "x") + + +def _miss_locate(conf=0.05) -> LocateResult: + return LocateResult(False, None, conf, 0, "x") + + +def _ok_goto(xyz=(1.0, 0.0, 1.0)) -> GotoResult: + return GotoResult(True, xyz, 0.0, 5, "ok", []) + + +def _failed_goto(xyz=(1.0, 0.0, 1.0)) -> GotoResult: + return GotoResult(False, (0.0, 0.0, 0.0), 1.4, 0, "no path", []) + + +def _explore_to(xyz=(2.0, 0.0, 2.0)) -> ExploreResult: + return ExploreResult(xyz, True, 2.8, "ok") + + +def test_agent_hit_then_goto(): + """Found on first call → no explore, single goto.""" + skills = FakeSkills( + locate_script=[_ok_locate()], + goto_script=[_ok_goto()], + ) + agent = DeterministicAgent(skills, AgentConfig(max_explore_iters=3)) + res = agent.execute(Task(targets=["mug"])) + assert res.fully_successful + assert skills.locate_calls == ["mug"] + assert skills.goto_calls == [(1.0, 0.0, 1.0)] + assert skills.explore_calls == [] + assert res.target_results[0].n_explore_iters == 0 + + +def test_agent_explore_then_relocate_then_goto(): + """First locate misses → explore → goto-to-frontier → re-locate finds → final goto.""" + skills = FakeSkills( + locate_script=[_miss_locate(), _ok_locate()], + explore_script=[_explore_to((3.0, 0.0, 0.0))], + goto_script=[_ok_goto((3.0, 0.0, 0.0)), _ok_goto((1.0, 0.0, 1.0))], + ) + agent = DeterministicAgent(skills, AgentConfig(max_explore_iters=3)) + res = agent.execute(Task(targets=["mug"])) + assert res.fully_successful + assert skills.locate_calls == ["mug", "mug"] + assert skills.explore_calls == ["mug"] + assert skills.goto_calls == [(3.0, 0.0, 0.0), (1.0, 0.0, 1.0)] + assert res.target_results[0].n_explore_iters == 1 + + +def test_agent_budget_exhaustion(): + """All N+1 locate calls miss → return budget_exhausted.""" + skills = FakeSkills( + locate_script=[_miss_locate() for _ in range(5)], + explore_script=[_explore_to() for _ in range(4)], + goto_script=[_ok_goto((2.0, 0.0, 2.0)) for _ in range(4)], + ) + agent = DeterministicAgent(skills, AgentConfig(max_explore_iters=3)) + res = agent.execute(Task(targets=["mug"])) + assert res.fully_successful is False + r = res.target_results[0] + assert r.reason == "budget_exhausted" + assert r.n_explore_iters == 3 + # 4 locate calls: initial + 3 retries. + assert len(skills.locate_calls) == 4 + assert len(skills.explore_calls) == 3 + + +def test_agent_no_frontier_short_circuits(): + """If explore can't find a frontier, give up immediately — no point looping.""" + skills = FakeSkills( + locate_script=[_miss_locate()], + explore_script=[ExploreResult(None, False, 0.0, "no frontier")], + ) + agent = DeterministicAgent(skills, AgentConfig(max_explore_iters=3)) + res = agent.execute(Task(targets=["mug"])) + r = res.target_results[0] + assert r.reached is False + assert r.reason == "no_frontier" + assert len(skills.locate_calls) == 1 + assert len(skills.explore_calls) == 1 + + +def test_agent_failed_goto_does_not_loop_back(): + """If locate finds the target but goto fails (e.g. no path), report the + failure cleanly rather than retrying.""" + skills = FakeSkills( + locate_script=[_ok_locate()], + goto_script=[_failed_goto()], + ) + agent = DeterministicAgent(skills) + res = agent.execute(Task(targets=["mug"])) + r = res.target_results[0] + assert r.reached is False + assert r.reason == "no path" + + +def test_agent_multi_target_bails_on_first_failure(): + """The spec says sequential targets stop at the first failure so the + caller sees the failure clearly.""" + skills = FakeSkills( + locate_script=[_miss_locate()], + explore_script=[ExploreResult(None, False, 0.0, "no frontier")], + ) + agent = DeterministicAgent(skills, AgentConfig(max_explore_iters=0)) + res = agent.execute(Task(targets=["mug", "chair"])) + assert len(res.target_results) == 1 # bailed before chair + assert res.target_results[0].target == "mug" + + +def test_agent_swap_parser_does_not_change_policy(): + """Acceptance from the spec: 'swapping Qwen for a hardcoded target string + yields the same spatial behaviour'. Same skills script, same scripted + locate/goto, regardless of how the command was parsed.""" + parser = HardcodedTaskParser() + for command in ("mug", "go to the mug", "find the mug"): + skills = FakeSkills(locate_script=[_ok_locate()], goto_script=[_ok_goto()]) + agent = DeterministicAgent(skills) + res = agent.execute_command(command, parser) + assert res.fully_successful + assert skills.goto_calls == [(1.0, 0.0, 1.0)] + + +def test_agent_empty_command_reports_parse_failure(): + skills = FakeSkills() + agent = DeterministicAgent(skills) + res = agent.execute_command("", HardcodedTaskParser()) + assert res.fully_successful is False + assert res.target_results[0].reason == "parse_empty" + assert skills.locate_calls == [] diff --git a/tests/navigation/test_dog_cli.py b/tests/navigation/test_dog_cli.py new file mode 100644 index 000000000..7b6226ada --- /dev/null +++ b/tests/navigation/test_dog_cli.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""End-to-end dry-run tests for the dog-nav REPL + synthetic scene. + +These exercise the whole navigation stack — sim scene → voxel map → +SigLIP stand-in → skills → agent → controller — with no robot, camera, +or models. +""" + +from __future__ import annotations + +import pytest + +from lerobot.navigation.dog_cli import DogController, _build_dry_run, main +from lerobot.navigation.sim import kitchen_scene + + +def test_kitchen_scene_builds_with_all_objects(): + scene = kitchen_scene() + assert {o.name for o in scene.objects} == {"couch", "chair", "lamp", "plant"} + assert len(scene.voxel_map) > 0 + assert scene.voxel_map.feature_dim == scene.feature_dim + + +def test_feature_extractor_matches_object_vectors(): + scene = kitchen_scene() + fx = scene.feature_extractor() + couch = scene.object("couch") + emb = fx.encode_text("couch") + # The couch query should align with the couch's stored basis vector. + import numpy as np + + assert float(np.dot(emb, couch.feature_vec / np.linalg.norm(couch.feature_vec))) > 0.9 + + +def test_controller_reaches_mapped_object(): + ctl = _build_dry_run() + result = ctl.handle_prompt("couch") + assert result.fully_successful + tr = result.target_results[0] + assert tr.reached + # Landed near the couch ground-truth (3.0, _, 2.0). + assert tr.final_xyz is not None + assert abs(tr.final_xyz[0] - 3.0) < 1.5 + assert abs(tr.final_xyz[2] - 2.0) < 1.5 + + +def test_controller_navigates_to_each_object(): + for name, (gx, gz) in { + "couch": (3.0, 2.0), + "chair": (-2.0, -1.5), + "plant": (-2.5, 2.5), + }.items(): + ctl = _build_dry_run() + result = ctl.handle_prompt(name) + assert result.fully_successful, f"failed to reach {name}" + fx = result.target_results[0].final_xyz + assert abs(fx[0] - gx) < 1.5 and abs(fx[2] - gz) < 1.5 + + +def test_controller_abstains_on_absent_object(): + ctl = _build_dry_run() + result = ctl.handle_prompt("banana") # not in the scene + assert not result.fully_successful + assert result.target_results[0].reason in {"budget_exhausted", "no_frontier"} + + +def test_idle_tick_explores_or_reports_no_frontier(): + ctl = _build_dry_run() + ex = ctl.idle_tick() + # A fully-observed synthetic floor may have no frontier; either way the + # call must be well-formed and not raise. + assert ex.reason in {"ok", "no frontier"} + + +def test_main_single_command_dry_run_returns_zero(): + assert main(["--dry-run", "--command", "couch", "--log-level", "WARNING"]) == 0 + + +def test_main_absent_object_returns_nonzero(): + assert main(["--dry-run", "--command", "banana", "--log-level", "WARNING"]) == 1 + + +def test_main_live_mode_refuses_until_pipeline_lands(): + with pytest.raises(SystemExit): + main(["--command", "couch"]) + + +def test_dogcontroller_stop_is_safe(): + ctl = _build_dry_run() + ctl.stop() + assert isinstance(ctl, DogController) diff --git a/tests/navigation/test_skills.py b/tests/navigation/test_skills.py new file mode 100644 index 000000000..a02411002 --- /dev/null +++ b/tests/navigation/test_skills.py @@ -0,0 +1,268 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for the unified ``SpatialSkills`` API (B1+B2+B3+B4).""" + +# ruff: noqa: N803, N806 — D: conventional feature-dimension name +from __future__ import annotations + +import math +from dataclasses import dataclass + +import numpy as np + +from lerobot.navigation.base_controller import StubBaseController +from lerobot.navigation.skills import SkillsConfig, SpatialSkills +from lerobot.navigation.voxel_map import VoxelMap + +# ----- fakes / fixtures --------------------------------------------------- + + +@dataclass +class FakeSiglip: + """Tiny stand-in for SiglipFeatureExtractor — text → fixed vector.""" + + text_to_vec: dict[str, np.ndarray] + feature_dim: int = 4 + + def encode_text(self, text: str) -> np.ndarray: + v = self.text_to_vec.get(text) + if v is None: + # Default: random-but-deterministic vector + rng = np.random.default_rng(abs(hash(text)) % (2**32)) + v = rng.normal(size=self.feature_dim).astype(np.float32) + v = v.astype(np.float32) + v = v / max(np.linalg.norm(v), 1e-6) + return v + + +def _vm_with_couch_and_chair(D: int = 4) -> VoxelMap: + """Two spatially-separated clusters with distinct unit feature vectors.""" + vm = VoxelMap(voxel_size=0.1) + rgb = np.full((1, 1, 3), 200, dtype=np.uint8) + conf = np.ones((1, 1), dtype=np.float32) + couch_vec = np.eye(D)[0].astype(np.float16).reshape(1, 1, D) + chair_vec = np.eye(D)[1].astype(np.float16).reshape(1, 1, D) + # Couch cluster around (5, 1, 3) + for x in (4.9, 5.0, 5.1): + for z in (2.9, 3.0, 3.1): + pts = np.array([[[x, 1.0, z]]], dtype=np.float32) + vm.add(pts, rgb, conf, frame=0, t=0.0, feat_map=couch_vec) + # Chair cluster around (-3, 1, 1) + for x in (-3.1, -3.0, -2.9): + for z in (0.9, 1.0, 1.1): + pts = np.array([[[x, 1.0, z]]], dtype=np.float32) + vm.add(pts, rgb, conf, frame=0, t=0.0, feat_map=chair_vec) + return vm + + +# ----- locate() ----------------------------------------------------------- + + +def test_locate_returns_centroid_for_matching_query(): + vm = _vm_with_couch_and_chair() + base = StubBaseController() + siglip = FakeSiglip(text_to_vec={"couch": np.array([1, 0, 0, 0], dtype=np.float32)}) + skills = SpatialSkills(vm, base, siglip, SkillsConfig(locate_threshold=0.3)) + result = skills.locate("couch") + assert result.found is True + assert result.xyz is not None + # Centroid should land near (5, 1, 3). + assert abs(result.xyz[0] - 5.0) < 0.2 + assert abs(result.xyz[2] - 3.0) < 0.2 + assert result.confidence > 0.5 + + +def test_locate_abstains_below_threshold(): + """Threshold tuned high enough that an unaligned query returns NOT_FOUND + rather than picking a "best of the bad" cluster.""" + vm = _vm_with_couch_and_chair() + base = StubBaseController() + # Query embedding orthogonal to both clusters' vectors. + siglip = FakeSiglip(text_to_vec={"banana": np.array([0, 0, 1, 0], dtype=np.float32)}) + skills = SpatialSkills( + vm, + base, + siglip, + SkillsConfig(locate_threshold=0.5), + ) + result = skills.locate("banana") + assert result.found is False + assert result.xyz is None + + +def test_locate_distinguishes_two_clusters(): + """red-cup / blue-cup style: two clusters present, the query should pick + the right one rather than averaging across both.""" + vm = _vm_with_couch_and_chair() + base = StubBaseController() + siglip = FakeSiglip( + text_to_vec={ + "couch": np.array([1, 0, 0, 0], dtype=np.float32), + "chair": np.array([0, 1, 0, 0], dtype=np.float32), + } + ) + skills = SpatialSkills(vm, base, siglip, SkillsConfig(locate_threshold=0.3)) + couch = skills.locate("couch") + chair = skills.locate("chair") + assert couch.found and chair.found + assert abs(couch.xyz[0] - 5.0) < 0.3 + assert abs(chair.xyz[0] - (-3.0)) < 0.3 + + +def test_locate_returns_not_found_without_siglip(): + vm = _vm_with_couch_and_chair() + skills = SpatialSkills(vm, StubBaseController(), siglip=None) + assert skills.locate("anything").found is False + + +def test_locate_returns_not_found_without_features(): + vm = VoxelMap() + rgb = np.full((1, 1, 3), 200, dtype=np.uint8) + vm.add(np.zeros((1, 1, 3), dtype=np.float32), rgb, np.ones((1, 1), dtype=np.float32), frame=0, t=0.0) + skills = SpatialSkills(vm, StubBaseController(), siglip=FakeSiglip({})) + assert skills.locate("anything").found is False + + +# ----- goto() ------------------------------------------------------------- + + +def _floor_vm(extent: float = 4.0, y_floor: float = 1.0, voxel_size: float = 0.1) -> VoxelMap: + """A clear floor of NAVIGABLE cells spanning [-extent, extent] in both x and z. + + Inputs are float64 to avoid float32 precision drift colliding adjacent + voxels at exact cell boundaries (Pi3X-shaped outputs are continuous and + don't hit this in practice; this fixture deliberately puts points AT + voxel boundaries so we'd quietly merge ~25% of them in float32).""" + vm = VoxelMap(voxel_size=voxel_size) + pts = [] + # Offset placement by half a voxel so each xz lands at a cell *centre*, + # robust to small float drift. + half = voxel_size / 2.0 + for x in np.arange(-extent + half, extent + half, voxel_size): + for z in np.arange(-extent + half, extent + half, voxel_size): + pts.append((float(x), y_floor, float(z))) + arr = np.asarray(pts, dtype=np.float64).reshape(-1, 1, 3) + rgb_arr = np.full((len(pts), 1, 3), 200, dtype=np.uint8) + conf_arr = np.ones((len(pts), 1), dtype=np.float32) + vm.add(arr, rgb_arr, conf_arr, frame=0, t=0.0) + return vm + + +def test_goto_reaches_static_goal(): + vm = _floor_vm() + base = StubBaseController() + skills = SpatialSkills( + vm, + base, + cfg=SkillsConfig( + cell_size=0.1, + obstacle_inflate_cells=0, + goto_threshold=0.3, + goto_max_steps=400, + goto_step_size=0.1, + ), + ) + result = skills.goto((2.0, 1.0, 2.0)) + assert result.reached, f"goto did not reach: {result}" + assert result.distance_to_target < 0.3 + # Should have logged the executed path. + assert len(result.path_xyz) > 0 + + +def test_goto_blocked_with_wall(): + """A floor with a wall of obstacle voxels splitting the navigable space. + The wall extends past the floor on both ends so there is no corner + detour — A* must report no-path.""" + vm = _floor_vm(extent=2.0) + # Vertical wall along x=0 at obstacle height, spanning more z than the + # floor so neither end of the wall has a navigable bypass cell. + wall_pts = [ + (0.0, float(y), float(z)) for y in np.arange(0.2, 0.9, 0.1) for z in np.arange(-3.0, 3.0, 0.1) + ] + arr = np.asarray(wall_pts, dtype=np.float64).reshape(-1, 1, 3) + rgb_arr = np.full((len(wall_pts), 1, 3), 200, dtype=np.uint8) + conf_arr = np.ones((len(wall_pts), 1), dtype=np.float32) + vm.add(arr, rgb_arr, conf_arr, frame=0, t=0.0) + + init = np.eye(4) + init[0, 3] = -1.0 + base = StubBaseController(initial_pose=init) + skills = SpatialSkills( + vm, + base, + cfg=SkillsConfig( + cell_size=0.1, + obstacle_inflate_cells=0, + goto_threshold=0.2, + goto_max_steps=200, + ), + ) + result = skills.goto((1.0, 1.0, 0.0)) + assert result.reached is False + assert result.reason == "no path" + + +def test_goto_stops_when_already_at_goal(): + vm = _floor_vm() + init = np.eye(4) + init[0, 3] = 0.5 + base = StubBaseController(initial_pose=init) + skills = SpatialSkills(vm, base, cfg=SkillsConfig(goto_threshold=0.5)) + result = skills.goto((0.5, 0.0, 0.0)) + assert result.reached and result.n_steps == 0 + + +# ----- explore() ---------------------------------------------------------- + + +def test_explore_returns_a_frontier_when_one_exists(): + # Build a small floor and let project_voxel_map_to_grid pad the bbox so + # there's UNOBSERVED space around it. + vm = _floor_vm(extent=1.0) + base = StubBaseController() + skills = SpatialSkills( + vm, + base, + cfg=SkillsConfig( + cell_size=0.1, + obstacle_inflate_cells=0, + ), + ) + result = skills.explore() + assert result.found_frontier + assert result.target_xyz is not None + + +def test_explore_reports_no_frontier_on_empty_voxelmap(): + vm = VoxelMap() + skills = SpatialSkills(vm, StubBaseController(), cfg=SkillsConfig()) + result = skills.explore() + assert result.found_frontier is False + assert result.target_xyz is None + + +def test_explore_target_distance_matches_pose(): + vm = _floor_vm(extent=1.0) + init = np.eye(4) + init[0, 3] = 0.3 + init[2, 3] = -0.4 + base = StubBaseController(initial_pose=init) + skills = SpatialSkills(vm, base, cfg=SkillsConfig(cell_size=0.1, obstacle_inflate_cells=0)) + result = skills.explore() + if result.target_xyz is not None: + d = math.hypot(result.target_xyz[0] - 0.3, result.target_xyz[2] - (-0.4)) + assert abs(d - result.distance_to_target) < 1e-3 diff --git a/tests/navigation/test_value_map.py b/tests/navigation/test_value_map.py new file mode 100644 index 000000000..62a51ea10 --- /dev/null +++ b/tests/navigation/test_value_map.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for the B2-full value-map exploration.""" + +# ruff: noqa: N803, N806 — D: conventional feature-dimension name +from __future__ import annotations + +import math + +import numpy as np + +from lerobot.navigation.occupancy import ( + NAVIGABLE, + UNOBSERVED, + OccupancyGrid, + project_voxel_map_to_grid, +) +from lerobot.navigation.value_map import ( + ValueMapConfig, + compute_value_maps, + pick_best_frontier_cell, +) +from lerobot.navigation.voxel_map import VoxelMap + + +def _vm_from(points, *, voxel_size=0.1, t0=0.0, dt=0.0, features=None): + vm = VoxelMap(voxel_size=voxel_size) + rgb = np.full((1, 1, 3), 200, dtype=np.uint8) + conf = np.ones((1, 1), dtype=np.float32) + for i, p in enumerate(points): + pt = np.array([[[p[0], p[1], p[2]]]], dtype=np.float64) + feat = None + if features is not None: + feat = features[i].reshape(1, 1, -1).astype(np.float16) + vm.add(pt, rgb, conf, frame=i, t=t0 + i * dt, feat_map=feat) + return vm + + +def _grid_around(vm: VoxelMap, cell_size: float = 0.5) -> OccupancyGrid: + return project_voxel_map_to_grid(vm, cell_size=cell_size, inflate_cells=0) + + +def test_recency_high_for_unobserved_cells(): + """Cells with no voxel projection should default to unknown_value.""" + vm = _vm_from([(0.0, 1.0, 0.0)]) + grid = _grid_around(vm) + cfg = ValueMapConfig(unknown_value=0.95) + vm_values = compute_value_maps(vm, grid, cfg=cfg) + # The voxel only fills one cell; the rest should be unknown. + unobs = grid.classes == UNOBSERVED + assert unobs.any() + np.testing.assert_allclose(vm_values.recency[unobs], 0.95, atol=1e-6) + + +def test_recency_drops_for_recent_observation(): + """A freshly-observed cell scores LOW on V_T (recency).""" + vm = _vm_from([(0.0, 1.0, 0.0)], t0=100.0) + grid = _grid_around(vm) + cfg = ValueMapConfig(recency_mid_s=10.0, recency_scale_s=3.0, unknown_value=1.0) + # now_t == t0 → age = 0 → sigmoid((0 - 10) / 3) ≈ 0.04 + values = compute_value_maps(vm, grid, now_t=100.0, cfg=cfg) + # Find the cell that received the voxel. + obs_mask = values.last_time > -math.inf + assert obs_mask.any() + assert values.recency[obs_mask].max() < 0.1 + + +def test_recency_grows_with_age(): + vm = _vm_from([(0.0, 1.0, 0.0)], t0=0.0) + grid = _grid_around(vm) + cfg = ValueMapConfig(recency_mid_s=10.0, recency_scale_s=3.0) + # 30 seconds later — V_T should be near 1. + values = compute_value_maps(vm, grid, now_t=30.0, cfg=cfg) + obs_mask = values.last_time > -math.inf + assert values.recency[obs_mask].max() > 0.9 + + +def test_similarity_high_for_matching_query(): + D = 8 + feat_couch = np.eye(D)[0] + vm = _vm_from( + [(0.0, 1.0, 0.0)], + features=[feat_couch], + ) + grid = _grid_around(vm) + text_emb = np.eye(D)[0] # same direction as couch + cfg = ValueMapConfig(similarity_mid=0.15, similarity_scale=0.05) + values = compute_value_maps(vm, grid, text_emb=text_emb, cfg=cfg) + assert values.similarity is not None + assert values.similarity.max() > 0.95 + + +def test_similarity_low_for_orthogonal_query(): + D = 8 + feat_couch = np.eye(D)[0] + vm = _vm_from([(0.0, 1.0, 0.0)], features=[feat_couch]) + grid = _grid_around(vm) + text_emb = np.eye(D)[3] # orthogonal + values = compute_value_maps(vm, grid, text_emb=text_emb) + assert values.similarity is not None + # Cells with content but no match: low V_S. + has_voxel = values.last_time > -math.inf + assert values.similarity[has_voxel].max() < 0.1 + + +def test_similarity_is_none_when_no_query(): + vm = _vm_from([(0.0, 1.0, 0.0)]) + grid = _grid_around(vm) + values = compute_value_maps(vm, grid) + assert values.similarity is None + np.testing.assert_array_equal(values.combined, values.recency) + + +def test_combined_balances_recency_and_similarity(): + D = 8 + # Two voxels with different features: one matches query, one doesn't. + feats = [np.eye(D)[0], np.eye(D)[3]] + vm = _vm_from( + [(0.0, 1.0, 0.0), (2.0, 1.0, 0.0)], + features=feats, + t0=0.0, + ) + grid = _grid_around(vm, cell_size=0.5) + cfg = ValueMapConfig(alpha_similarity=0.7, recency_mid_s=5.0, recency_scale_s=2.0) + text_emb = np.eye(D)[0] + values = compute_value_maps(vm, grid, text_emb=text_emb, now_t=0.0, cfg=cfg) + + # Cell with matching feature should have HIGHER combined value than the + # non-matching observed cell at the same age. + snap_xyz = vm.snapshot().xyz + iz_m = int((snap_xyz[0, 2] - grid.origin_z) / grid.cell_size) + ix_m = int((snap_xyz[0, 0] - grid.origin_x) / grid.cell_size) + iz_n = int((snap_xyz[1, 2] - grid.origin_z) / grid.cell_size) + ix_n = int((snap_xyz[1, 0] - grid.origin_x) / grid.cell_size) + assert values.combined[iz_m, ix_m] > values.combined[iz_n, ix_n] + + +def test_pick_best_frontier_prefers_high_value_cell(): + classes = np.full((6, 6), UNOBSERVED, dtype=np.int8) + classes[1:5, 1:5] = NAVIGABLE + grid = OccupancyGrid(classes=classes, cell_size=0.5, origin_x=0.0, origin_z=0.0, ground_y=0.0) + frontier_cells = np.array([[1, 1], [4, 4]], dtype=np.int32) + from lerobot.navigation.value_map import ValueMaps + + # Make cell (4, 4) more valuable than (1, 1). + combined = np.zeros((6, 6), dtype=np.float32) + combined[1, 1] = 0.2 + combined[4, 4] = 0.9 + values = ValueMaps( + last_time=np.full((6, 6), -math.inf), + recency=combined.copy(), + similarity=None, + combined=combined, + ) + best_idx, (x, z), d, score = pick_best_frontier_cell( + grid, + frontier_cells, + values, + robot_position_xz=(0.0, 0.0), + cfg=ValueMapConfig(distance_discount_per_meter=0.0), + ) + assert best_idx == 1 + assert score > 0.8 + + +def test_distance_discount_prefers_closer_when_values_equal(): + classes = np.full((6, 6), UNOBSERVED, dtype=np.int8) + classes[0:6, 0:6] = NAVIGABLE + grid = OccupancyGrid(classes=classes, cell_size=1.0, origin_x=0.0, origin_z=0.0, ground_y=0.0) + frontier_cells = np.array([[0, 0], [5, 5]], dtype=np.int32) + from lerobot.navigation.value_map import ValueMaps + + same = np.ones((6, 6), dtype=np.float32) + values = ValueMaps( + last_time=np.full((6, 6), -math.inf), + recency=same.copy(), + similarity=None, + combined=same, + ) + best_idx, _, _, _ = pick_best_frontier_cell( + grid, + frontier_cells, + values, + robot_position_xz=(0.0, 0.0), + cfg=ValueMapConfig(distance_discount_per_meter=0.5), + ) + # Robot at origin → (0, 0) is closer than (5, 5). + assert best_idx == 0 + + +def test_compute_value_maps_with_empty_voxel_map_returns_unknown(): + vm = VoxelMap() + classes = np.full((4, 4), UNOBSERVED, dtype=np.int8) + grid = OccupancyGrid(classes=classes, cell_size=1.0, origin_x=0.0, origin_z=0.0, ground_y=0.0) + values = compute_value_maps(vm, grid) + np.testing.assert_allclose(values.recency, 1.0) + assert values.similarity is None