Add Unitree Go2 robot + spatial-memory navigation base

unitree_go2 robot: high-level sport-mode body-velocity control over DDS (unitree_sdk2py) straight from the host — no companion computer, unlike the G1's low-level ZMQ bridge. Actions x.vel/y.vel/theta.vel; observation is planar odometry (rt/sportmodestate) + the built-in front camera via VideoClient. SDK imported only in connect() so configs/features/tests work without it; 20 tests via mocks. unitree_go2 pyproject extra. lerobot.navigation package: BaseController protocol + StubBaseController + SafeBaseController (velocity clamp, occupancy gate, keyframe watchdog, e-stop latch) + RobotBaseController wrapping any Robot on the standard REP-103 mobile-base contract, carrying the world<->body velocity and odometry<->world pose frame math. Robot-agnostic: the SDK lives only in the robot class. 29 tests, SDK/torch-free. First step of consolidating the dyna360 DynaMem navigation stack into lerobot; dyna360 is a source to copy from, not a runtime dependency. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
2026-07-24 02:06:15 +00:00 · 2026-07-20 17:45:41 +02:00
parent ddc2aa7a27
commit ff94e6385b
9 changed files with 1355 additions and 0 deletions
@@ -0,0 +1,320 @@
+#!/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.
+
+"""Tests for the navigation base controller.
+
+Hardware-free and SDK-free: the frame math is pure, the stub is
+kinematic, and the robot-backed controller is exercised through a fake
+Robot that records actions and serves canned odometry.
+"""
+
+from __future__ import annotations
+
+import math
+import time
+
+import numpy as np
+import pytest
+
+from lerobot.navigation.base_controller import (
+    BaseController,
+    RobotBaseController,
+    RobotBaseControllerConfig,
+    SafeBaseController,
+    StubBaseController,
+    odometry_to_world_pose,
+    world_velocity_to_body,
+)
+
+# ----- world_velocity_to_body ---------------------------------------------
+
+
+def test_forward_maps_to_body_x():
+    """heading=0, world +z (forward) → (vx>0, 0, 0)."""
+    vx_f, vy_l, vyaw = world_velocity_to_body(0.0, 0.3, 0.0, heading_rad=0.0)
+    assert vx_f == pytest.approx(0.3)
+    assert vy_l == pytest.approx(0.0)
+    assert vyaw == pytest.approx(0.0)
+
+
+def test_world_right_maps_to_negative_left():
+    """heading=0, world +x is the robot's RIGHT → negative y.vel."""
+    vx_f, vy_l, _ = world_velocity_to_body(0.3, 0.0, 0.0, heading_rad=0.0)
+    assert vx_f == pytest.approx(0.0)
+    assert vy_l == pytest.approx(-0.3)
+
+
+def test_world_x_is_forward_after_quarter_turn():
+    vx_f, vy_l, _ = world_velocity_to_body(0.3, 0.0, 0.0, heading_rad=math.pi / 2)
+    assert vx_f == pytest.approx(0.3)
+    assert vy_l == pytest.approx(0.0, abs=1e-9)
+
+
+def test_yaw_rate_sign_flips():
+    _, _, vyaw = world_velocity_to_body(0.0, 0.0, 0.5, heading_rad=0.0)
+    assert vyaw == pytest.approx(-0.5)
+
+
+def test_velocity_magnitude_preserved_under_rotation():
+    vx_f, vy_l, _ = world_velocity_to_body(0.3, 0.4, 0.0, heading_rad=1.234)
+    assert math.hypot(vx_f, vy_l) == pytest.approx(0.5)
+
+
+# ----- odometry_to_world_pose ---------------------------------------------
+
+
+def test_odometry_at_origin_is_identity():
+    pose, heading = odometry_to_world_pose(1.0, 2.0, 0.3, origin=(1.0, 2.0, 0.3))
+    np.testing.assert_allclose(pose, np.eye(4), atol=1e-12)
+    assert heading == pytest.approx(0.0)
+
+
+def test_odometry_forward_maps_to_world_z():
+    pose, heading = odometry_to_world_pose(1.0, 0.0, 0.0, origin=(0.0, 0.0, 0.0))
+    assert pose[0, 3] == pytest.approx(0.0)
+    assert pose[2, 3] == pytest.approx(1.0)
+    assert heading == pytest.approx(0.0)
+
+
+def test_odometry_left_maps_to_world_negative_x():
+    pose, _ = odometry_to_world_pose(0.0, 1.0, 0.0, origin=(0.0, 0.0, 0.0))
+    assert pose[0, 3] == pytest.approx(-1.0)
+    assert pose[2, 3] == pytest.approx(0.0)
+
+
+def test_odometry_yaw_sign_flip():
+    pose, heading = odometry_to_world_pose(0.0, 0.0, 0.5, origin=(0.0, 0.0, 0.0))
+    assert heading == pytest.approx(-0.5)
+    fwd = pose[:3, 2]
+    np.testing.assert_allclose(fwd, [math.sin(-0.5), 0.0, math.cos(-0.5)], atol=1e-12)
+
+
+def test_odometry_origin_yaw_is_derotated():
+    """Motion along the boot-time heading is always world +z, whatever
+    direction the robot faced when odometry started."""
+    origin = (0.0, 0.0, math.pi / 2)
+    pose, heading = odometry_to_world_pose(0.0, 1.0, math.pi / 2, origin=origin)
+    assert pose[0, 3] == pytest.approx(0.0, abs=1e-12)
+    assert pose[2, 3] == pytest.approx(1.0)
+    assert heading == pytest.approx(0.0)
+
+
+# ----- StubBaseController --------------------------------------------------
+
+
+def test_stub_is_basecontroller():
+    assert isinstance(StubBaseController(), BaseController)
+
+
+def test_stub_integrates_forward():
+    c = StubBaseController()
+    c.move(0.0, 0.2, dt=1.0)
+    assert c.position()[2] == pytest.approx(0.2)
+
+
+def test_stub_clamps_velocity():
+    c = StubBaseController(max_lin_speed=0.1)
+    c.move(5.0, 0.0, dt=1.0)
+    assert c.position()[0] == pytest.approx(0.1)
+
+
+# ----- RobotBaseController -------------------------------------------------
+
+
+class FakeRobot:
+    """Minimal Robot stand-in: records actions, serves canned odometry."""
+
+    def __init__(self) -> None:
+        self.actions: list[dict] = []
+        self.obs: dict = {}
+
+    def send_action(self, action: dict) -> dict:
+        self.actions.append(action)
+        return action
+
+    def get_observation(self) -> dict:
+        return self.obs
+
+
+def _robot_controller(**cfg_kwargs) -> tuple[RobotBaseController, FakeRobot]:
+    robot = FakeRobot()
+    cfg = RobotBaseControllerConfig(**cfg_kwargs)
+    return RobotBaseController(robot, cfg), robot
+
+
+def _odom(x=0.0, y=0.0, yaw=0.0) -> dict:
+    return {"x.pos": x, "y.pos": y, "theta.pos": yaw}
+
+
+def test_robot_controller_is_basecontroller():
+    ctl, _ = _robot_controller()
+    assert isinstance(ctl, BaseController)
+
+
+def test_forward_command_reaches_send_action():
+    ctl, robot = _robot_controller()
+    ctl.feed_observation(_odom())  # heading 0
+    ctl.move(vx=0.0, vz=0.3, dt=0.05)
+    assert robot.actions[-1] == {
+        "x.vel": pytest.approx(0.3),
+        "y.vel": pytest.approx(0.0),
+        "theta.vel": pytest.approx(0.0),
+    }
+
+
+def test_command_uses_odometry_heading():
+    """After the robot turns to heading +π/2, a world +x command comes out
+    as pure body-forward. First sample fixes the origin."""
+    ctl, robot = _robot_controller()
+    ctl.feed_observation(_odom())  # origin, heading 0
+    ctl.feed_observation(_odom(yaw=-math.pi / 2))  # turned; heading +π/2
+    ctl.move(vx=0.3, vz=0.0, dt=0.05)
+    assert robot.actions[-1]["x.vel"] == pytest.approx(0.3)
+    assert robot.actions[-1]["y.vel"] == pytest.approx(0.0, abs=1e-9)
+
+
+def test_command_is_clamped_before_send():
+    ctl, robot = _robot_controller(max_lin_speed=0.1)
+    ctl.feed_observation(_odom())
+    ctl.move(vx=0.0, vz=9.0, dt=0.05)
+    assert robot.actions[-1]["x.vel"] == pytest.approx(0.1)
+
+
+def test_pose_comes_from_odometry_not_integration():
+    ctl, _ = _robot_controller()
+    ctl.feed_observation(_odom())
+    ctl.move(0.0, 0.3, dt=1.0)  # would integrate 0.3 m open-loop
+    ctl.feed_observation(_odom(x=0.05))  # ...but odometry says 5 cm forward
+    assert ctl.position()[2] == pytest.approx(0.05)
+
+
+def test_origin_is_first_odometry_sample():
+    ctl, _ = _robot_controller()
+    ctl.feed_observation(_odom(x=3.0, y=-1.0, yaw=0.7))
+    np.testing.assert_allclose(ctl.pose(), np.eye(4), atol=1e-12)
+
+
+def test_open_loop_fallback_without_odometry():
+    """No odometry fed → integrate open-loop like the stub."""
+    ctl, _ = _robot_controller()
+    ctl.move(0.0, 0.2, dt=1.0)
+    assert ctl.position()[2] == pytest.approx(0.2)
+
+
+def test_stop_sends_zero_velocity():
+    ctl, robot = _robot_controller()
+    ctl.stop()
+    assert robot.actions[-1] == {"x.vel": 0.0, "y.vel": 0.0, "theta.vel": 0.0}
+    assert ctl.is_stopped
+
+
+def test_robot_controller_matches_stub_open_loop():
+    """Open-loop pose integration matches StubBaseController for the same
+    command sequence — sim runs must transfer to the real base."""
+    ctl, _ = _robot_controller(max_lin_speed=1.0)
+    stub = StubBaseController()
+    for vx, vz, yaw in [(0.2, 0.0, 0.0), (0.0, 0.3, 0.5), (0.1, 0.1, -0.2)]:
+        ctl.move(vx, vz, yaw, dt=0.5)
+        stub.move(vx, vz, yaw, dt=0.5)
+    np.testing.assert_allclose(ctl.pose(), stub.pose(), atol=1e-9)
+
+
+# ----- SafeBaseController --------------------------------------------------
+
+
+class FakeGrid:
+    """Occupancy stand-in with a single obstacle cell."""
+
+    def __init__(self, obstacle_cell=(5, 6), cell_size=0.1, origin_x=-0.5, origin_z=-0.5):
+        self.obstacle_cell = obstacle_cell
+        self.cell_size = cell_size
+        self.origin_x = origin_x
+        self.origin_z = origin_z
+
+    def world_to_cell(self, x: float, z: float) -> tuple[int, int]:
+        ix = int((x - self.origin_x) / self.cell_size)
+        iz = int((z - self.origin_z) / self.cell_size)
+        return iz, ix
+
+    def is_obstacle(self, iz: int, ix: int) -> bool:
+        return (iz, ix) == self.obstacle_cell
+
+
+def test_safe_passes_normal_moves():
+    inner = StubBaseController()
+    safe = SafeBaseController(inner=inner)
+    safe.feed_watchdog()
+    safe.move(0.0, 0.1, dt=1.0)
+    assert inner.position()[2] == pytest.approx(0.1)
+
+
+def test_safe_clamps_speed():
+    inner = StubBaseController(max_lin_speed=100.0)
+    safe = SafeBaseController(inner=inner, max_lin_speed=0.5)
+    safe.feed_watchdog()
+    safe.move(10.0, 0.0, dt=1.0)
+    assert inner.position()[0] == pytest.approx(0.5)
+
+
+def test_safe_watchdog_latches_on_stale_keyframes():
+    inner = StubBaseController()
+    safe = SafeBaseController(inner=inner, watchdog_timeout_s=0.05)
+    safe.feed_watchdog()
+    time.sleep(0.1)
+    safe.move(0.0, 0.1, dt=1.0)
+    assert safe.e_stop_latched
+    safe.move(0.0, 10.0, dt=1.0)  # refused
+    assert inner.position()[2] == pytest.approx(0.0, abs=1e-6)
+
+
+def test_safe_reset_watchdog_re_enables_motion():
+    inner = StubBaseController()
+    safe = SafeBaseController(inner=inner, watchdog_timeout_s=0.05)
+    safe.feed_watchdog()
+    time.sleep(0.1)
+    safe.move(0.0, 0.1)
+    assert safe.e_stop_latched
+    safe.reset_watchdog()
+    safe.move(0.0, 0.1, dt=1.0)
+    assert inner.position()[2] == pytest.approx(0.1)
+
+
+def test_safe_refuses_move_into_obstacle():
+    inner = StubBaseController()
+    grid = FakeGrid(obstacle_cell=(5, 6))
+    safe = SafeBaseController(inner=inner, occupancy_provider=lambda: grid)
+    safe.feed_watchdog()
+    # +0.15 m in x from origin lands mid-column ix=6 (origin_x=-0.5, cell=0.1).
+    safe.move(vx=0.15, vz=0.0, dt=1.0)
+    assert safe.e_stop_latched
+    assert inner.position()[0] == pytest.approx(0.0, abs=1e-6)
+
+
+def test_safe_allows_move_into_free_cell():
+    inner = StubBaseController()
+    grid = FakeGrid(obstacle_cell=(99, 99))
+    safe = SafeBaseController(inner=inner, occupancy_provider=lambda: grid)
+    safe.feed_watchdog()
+    safe.move(vx=0.1, vz=0.0, dt=1.0)
+    assert inner.position()[0] == pytest.approx(0.1)
+
+
+def test_safe_allows_when_no_map_yet():
+    inner = StubBaseController()
+    safe = SafeBaseController(inner=inner, occupancy_provider=lambda: None)
+    safe.feed_watchdog()
+    safe.move(vx=0.1, vz=0.0, dt=1.0)
+    assert inner.position()[0] == pytest.approx(0.1)