diff --git a/src/lerobot/navigation/dog_cli.py b/src/lerobot/navigation/dog_cli.py index a2c464554..562eb633e 100644 --- a/src/lerobot/navigation/dog_cli.py +++ b/src/lerobot/navigation/dog_cli.py @@ -125,15 +125,22 @@ class LiveMapper: """One perceive→integrate step of live mapping on the robot. Each :meth:`tick` reads an observation (front camera + odometry), - updates the controller's pose from odometry, runs the geometry model - and feature extractor on the frame, integrates the keyframe into the - voxel map, and feeds the safety watchdog. Constructed lazily — no SDK - or model is touched until the first tick. + updates the base pose from odometry, runs the geometry model + feature + extractor on the frame, and integrates the keyframe. + + Frame convention (important): the **odometry frame is the one world + frame**. The geometry model supplies only relative camera-frame + geometry (``local_points``/depth); those points are projected through + the base's odometry pose, so the voxel map and the robot pose live in + the same coordinates and ``goto`` drives to the right place. The + model's own ``camera_poses`` (its internal monocular frame) are not + used as the world frame. Constructed lazily — no SDK/model touched + until the first tick. """ def __init__(self, robot, base, geometry, siglip, voxel_map, pcfg=None) -> None: self.robot = robot - self.base = base # RobotBaseController (unwrapped) for feed_observation + self.base = base # RobotBaseController (unwrapped) for feed_observation/pose self.safe = None # optional SafeBaseController for the watchdog self.geometry = geometry self.siglip = siglip @@ -148,11 +155,13 @@ class LiveMapper: KeyframeContext, PipelineConfig, integrate_keyframe, + local_points_to_world, upsample_features_to_view, ) obs = self.robot.get_observation() - self.base.feed_observation(obs) + self.base.feed_observation(obs) # updates the odometry world pose + pose = self.base.pose() # camera-to-world in the odometry frame frame = obs.get("front") if frame is None: @@ -166,14 +175,17 @@ class LiveMapper: patches = self.siglip.encode_views(views)[0] # (Hp, Wp, D) feat_map = upsample_features_to_view(patches, h, w) + # World points come from the model's camera-frame geometry projected + # through the odometry pose — NOT the model's own world frame. + points_world = local_points_to_world(geo.local_points[0], pose) ctx = KeyframeContext( frame_idx=self._frame, t_sec=t_sec, rgb_uint8=views[0], - points_world=geo.points[0], + points_world=points_world, local_points=geo.local_points[0], conf=geo.conf[0], - pose=geo.camera_poses[0], + pose=pose, feat_map=feat_map, ) integrate_keyframe(self.voxel_map, ctx, self.pcfg or PipelineConfig()) @@ -185,30 +197,59 @@ class LiveMapper: def _build_live( network_interface: str = "eth0", device: str = "cuda", + camera_hfov_deg: float = 90.0, + max_lin_speed: float = 0.4, + max_yaw_rate: float = 0.8, ) -> tuple[DogController, LiveMapper]: """Wire the controller + live mapper against a real Unitree Go2. Nothing here touches the SDK or loads a model — construction is lazy; the DDS connection and model loads happen on first use. + + ``camera_hfov_deg`` sets the pinhole focal length used for free-space + carving (``focal = W / (2·tan(HFOV/2))``). Calibrate it to the Go2 + front camera for correct carving; a wrong value only degrades dynamic + removal, not the additive map. Speed caps are deliberately low for + first bring-up. """ - from lerobot.navigation.base_controller import RobotBaseController, SafeBaseController + import math + + from lerobot.navigation.base_controller import ( + RobotBaseController, + RobotBaseControllerConfig, + SafeBaseController, + ) from lerobot.navigation.features import SiglipFeatureExtractor from lerobot.navigation.geometry import LingBotMapRunner + from lerobot.navigation.pipeline import PipelineConfig from lerobot.navigation.voxel_map import VoxelMap from lerobot.robots.unitree_go2 import UnitreeGo2, UnitreeGo2Config - robot = UnitreeGo2(UnitreeGo2Config(network_interface=network_interface)) - inner = RobotBaseController(robot) - safe = SafeBaseController(inner=inner) + robot_cfg = UnitreeGo2Config(network_interface=network_interface) + robot = UnitreeGo2(robot_cfg) + inner = RobotBaseController( + robot, RobotBaseControllerConfig(max_lin_speed=max_lin_speed, max_yaw_rate=max_yaw_rate) + ) + safe = SafeBaseController(inner=inner, max_lin_speed=max_lin_speed, max_yaw_rate=max_yaw_rate) voxel_map = VoxelMap(voxel_size=0.05) siglip = SiglipFeatureExtractor(device=device) geometry = LingBotMapRunner(device=device) + w = robot_cfg.front_camera_width + focal_px = w / (2.0 * math.tan(math.radians(camera_hfov_deg) / 2.0)) + pcfg = PipelineConfig(focal_px=focal_px) + skills = SpatialSkills(voxel_map, safe, siglip, SkillsConfig(cell_size=0.05)) controller = DogController(skills, DeterministicAgent(skills, AgentConfig())) - mapper = LiveMapper(robot, inner, geometry, siglip, voxel_map) + mapper = LiveMapper(robot, inner, geometry, siglip, voxel_map, pcfg=pcfg) mapper.safe = safe - LOG.info("live stack wired (iface=%s, device=%s) — connect the dog and run", network_interface, device) + LOG.info( + "live stack wired (iface=%s, device=%s, focal=%.1fpx, vmax=%.2f m/s) — connect the dog and run", + network_interface, + device, + focal_px, + max_lin_speed, + ) return controller, mapper @@ -306,6 +347,14 @@ def main(argv: list[str] | None = None) -> int: ) ap.add_argument("--network-interface", default="eth0", help="Host interface wired to the dog.") ap.add_argument("--device", default="cuda", help="Torch device for the geometry/feature models.") + ap.add_argument( + "--camera-hfov-deg", + type=float, + default=90.0, + help="Go2 front-camera horizontal FOV, for the carve focal length. Calibrate to your camera.", + ) + ap.add_argument("--max-lin-speed", type=float, default=0.4, help="Body linear speed cap (m/s).") + ap.add_argument("--max-yaw-rate", type=float, default=0.8, help="Yaw-rate cap (rad/s).") 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) @@ -315,7 +364,13 @@ def main(argv: list[str] | None = None) -> int: ) if args.live: - controller, mapper = _build_live(args.network_interface, args.device) + controller, mapper = _build_live( + args.network_interface, + args.device, + camera_hfov_deg=args.camera_hfov_deg, + max_lin_speed=args.max_lin_speed, + max_yaw_rate=args.max_yaw_rate, + ) return run_live_repl(controller, mapper) if not args.dry_run: diff --git a/src/lerobot/navigation/pipeline.py b/src/lerobot/navigation/pipeline.py index 663dca6d5..bc845b910 100644 --- a/src/lerobot/navigation/pipeline.py +++ b/src/lerobot/navigation/pipeline.py @@ -98,6 +98,27 @@ def integrate_keyframe( return carve, stats +def local_points_to_world(local_points: np.ndarray, pose: np.ndarray) -> np.ndarray: + """Transform camera-frame points ``(H, W, 3)`` into the world frame using + a 4×4 camera-to-world ``pose``. + + On the robot the world frame is the odometry frame (from the base + controller), and the geometry model supplies only relative + camera-frame geometry — so projecting through the odometry pose keeps + the voxel map and the robot pose in ONE consistent frame. Returns + ``(H, W, 3)`` float32. + """ + if local_points.ndim != 3 or local_points.shape[-1] != 3: + raise ValueError(f"expected (H, W, 3), got {local_points.shape}") + if pose.shape != (4, 4): + raise ValueError(f"pose must be (4, 4); got {pose.shape}") + r = pose[:3, :3].astype(np.float64) + t = pose[:3, 3].astype(np.float64) + flat = local_points.reshape(-1, 3).astype(np.float64) + world = flat @ r.T + t + return world.reshape(local_points.shape).astype(np.float32) + + def upsample_features_to_view( patch_feats_one_view: np.ndarray, view_h: int, diff --git a/tests/navigation/test_live_mapper.py b/tests/navigation/test_live_mapper.py new file mode 100644 index 000000000..6b02c7e64 --- /dev/null +++ b/tests/navigation/test_live_mapper.py @@ -0,0 +1,115 @@ +#!/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. + +"""Exercise the live mapping loop (LiveMapper.tick) with no hardware. + +A fake robot serves canned front-camera frames + odometry; FakeGeometryRunner +supplies planar depth. This validates the perceive → project-through-odometry +→ integrate path — the logic that runs on the real dog — including the +frame-consistency fix (voxels land in the odometry world frame). +""" + +from __future__ import annotations + +import numpy as np + +from lerobot.navigation.base_controller import RobotBaseController +from lerobot.navigation.dog_cli import LiveMapper +from lerobot.navigation.geometry import FakeGeometryRunner +from lerobot.navigation.pipeline import PipelineConfig +from lerobot.navigation.voxel_map import VoxelMap + + +class FakeGo2: + """Robot stand-in: canned front frame + programmable odometry.""" + + def __init__(self, h=14, w=14): + self.h, self.w = h, w + self.odom = {"x.pos": 0.0, "y.pos": 0.0, "theta.pos": 0.0} + self.actions = [] + + def get_observation(self): + return { + "front": np.full((self.h, self.w, 3), 120, dtype=np.uint8), + **self.odom, + } + + def send_action(self, action): + self.actions.append(action) + return action + + +def _mapper(**pcfg_kwargs): + robot = FakeGo2() + base = RobotBaseController(robot) + vm = VoxelMap(voxel_size=0.05) + geom = FakeGeometryRunner(depth=2.0, focal_px=100.0) + pcfg = PipelineConfig(focal_px=100.0, **pcfg_kwargs) + mapper = LiveMapper(robot, base, geom, siglip=None, voxel_map=vm, pcfg=pcfg) + return mapper, robot, base, vm + + +def test_tick_populates_voxel_map(): + mapper, _, _, vm = _mapper() + mapper.tick(0.0) + assert len(vm) > 0 + + +def test_tick_places_voxels_in_odometry_frame(): + """With the dog at origin facing +z, the planar floor at depth 2 lands + around world z≈2 — i.e. in the odometry world frame, not the model's.""" + mapper, robot, base, vm = _mapper() + mapper.tick(0.0) + snap = vm.snapshot() + # Median z of the observed floor should be near the camera depth (2 m). + assert 1.0 < float(np.median(snap.xyz[:, 2])) < 3.0 + + +def test_tick_follows_odometry_translation(): + """Move the dog forward 5 m in odometry; the new floor voxels shift with + it — proof the map tracks the odometry world frame.""" + mapper, robot, base, vm = _mapper() + mapper.tick(0.0) + z0 = float(np.median(vm.snapshot().xyz[:, 2])) + + robot.odom = {"x.pos": 5.0, "y.pos": 0.0, "theta.pos": 0.0} # forward 5 m + vm2 = VoxelMap(voxel_size=0.05) + mapper.voxel_map = vm2 + mapper.tick(1.0) + z1 = float(np.median(vm2.snapshot().xyz[:, 2])) + # Forward odometry (+x_odom → +z_world) shifts the floor ~5 m in world z. + assert z1 - z0 > 4.0 + + +def test_tick_without_front_frame_is_safe(): + mapper, robot, _, vm = _mapper() + robot.get_observation = lambda: {"x.pos": 0.0, "y.pos": 0.0, "theta.pos": 0.0} + mapper.tick(0.0) # no 'front' → no-op, must not raise + assert len(vm) == 0 + + +def test_tick_feeds_watchdog_when_present(): + from lerobot.navigation.base_controller import SafeBaseController + + mapper, _, base, _ = _mapper() + safe = SafeBaseController(inner=base) + safe.e_stop_latched = True # will clear on a healthy feed via reset elsewhere + mapper.safe = safe + # feed_watchdog just refreshes the timer; assert tick calls it (no raise, + # and the timestamp advances). + before = safe._last_keyframe_walltime + mapper.tick(0.0) + assert safe._last_keyframe_walltime >= before