navigation: port value maps, features, skills, agent + dog-nav CLI

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 <noreply@anthropic.com>

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 == []

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)

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

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