mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-23 17:56:07 +00:00
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>
This commit is contained in:
Pepijn
@@ -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",
|
||||
]
|
||||
|
||||
@@ -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",
|
||||
)
|
||||
@@ -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())
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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,
|
||||
)
|
||||
@@ -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])
|
||||
@@ -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:]
|
||||
|
||||
@@ -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 == []
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user