From 15472a7bacbebf375f156eb52951aa60091ea370 Mon Sep 17 00:00:00 2001 From: devdeepr Date: Thu, 7 May 2026 20:37:35 +0000 Subject: [PATCH 1/2] fix(retargeting): derive locomotion hip-height dt from graph_time The hip-height integration in LocomotionRootCmdRetargeter used a hardcoded config.dt=1/60 regardless of the actual control rate, so any deployment running at 90/120 Hz integrated 1.5x/2x too slowly. Read dt from ComputeContext.graph_time.real_time_ns instead, with a fallback_dt for the first frame after init/reset. Add a rate-invariance test that drives the retargeter at 60/90/120 Hz with the same stick value over the same wall duration and asserts the hip delta matches across rates. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/source/references/retargeting/index.rst | 3 +- .../python/test_locomotion_retargeter.py | 158 ++++++++++++++++++ src/retargeters/locomotion_retargeter.py | 17 +- 3 files changed, 175 insertions(+), 3 deletions(-) create mode 100644 src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 81da6c89b..0b7b07c68 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -196,7 +196,8 @@ Available Retargeters ``[vel_x, vel_y, rot_vel_z, hip_height]``. Left thumbstick: linear velocity (X, Y). Right thumbstick X: angular velocity (Z). Right thumbstick Y: hip height adjustment. ``LocomotionRootCmdRetargeterConfig`` includes ``initial_hip_height``, ``movement_scale``, - ``rotation_scale``, and ``dt`` (time step for height integration). + ``rotation_scale``, and ``fallback_dt`` (first-frame nominal step; steady-state ``dt`` is + derived from ``ComputeContext.graph_time``). .. dropdown:: LocomotionFixedRootCmdRetargeter diff --git a/src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py b/src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py new file mode 100644 index 000000000..6bd3ef9d5 --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py @@ -0,0 +1,158 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for LocomotionRootCmdRetargeter — hip-height integration uses real +elapsed time from ``ComputeContext.graph_time``, so the result is invariant to +the control rate (60/90/120 Hz).""" + +import math + +from isaacteleop.retargeters.locomotion_retargeter import ( + LocomotionRootCmdRetargeter, + LocomotionRootCmdRetargeterConfig, +) +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + GraphTime, + OptionalTensorGroup, + OptionalType, + TensorGroup, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, + DLDataType, + NDArrayType, +) + + +def _make_controller_group(thumbstick_y: float) -> OptionalTensorGroup: + """Build a ``controller_*`` input with only the thumbstick fields set — + those are the only fields the locomotion retargeter reads. Writing any + field flips ``is_none`` to False.""" + group = OptionalTensorGroup(OptionalType(ControllerInput())) + group[ControllerInputIndex.THUMBSTICK_X] = 0.0 + group[ControllerInputIndex.THUMBSTICK_Y] = thumbstick_y + return group + + +def _make_output_group() -> TensorGroup: + return TensorGroup( + TensorGroupType( + "root_command", + [NDArrayType("command", shape=(4,), dtype=DLDataType.FLOAT, dtype_bits=32)], + ) + ) + + +def _run_at_rate(rate_hz: float, duration_s: float, right_y: float) -> float: + """Drive the retargeter at ``rate_hz`` for ``duration_s`` with a constant + right-stick Y value. Returns the final hip height.""" + config = LocomotionRootCmdRetargeterConfig( + initial_hip_height=0.72, + rotation_scale=0.35, + ) + r = LocomotionRootCmdRetargeter(config, name="locomotion") + + period_s = 1.0 / rate_hz + period_ns = int(round(period_s * 1e9)) + n_steps = int(round(duration_s / period_s)) + + inputs = { + "controller_left": _make_controller_group(0.0), + "controller_right": _make_controller_group(right_y), + } + outputs = {"root_command": _make_output_group()} + + t_ns = 0 + for _ in range(n_steps): + ctx = ComputeContext(graph_time=GraphTime(sim_time_ns=t_ns, real_time_ns=t_ns)) + r._compute_fn(inputs, outputs, ctx) + t_ns += period_ns + + return float(outputs["root_command"][0][3]) + + +class TestLocomotionDtIntegration: + """Hip-height integration is rate-invariant when dt is derived from + ``GraphTime``.""" + + def test_hip_height_matches_across_rates(self): + """Equal wall-clock duration with the same stick value should produce + the same hip-height delta at 60, 90, and 120 Hz.""" + duration_s = 1.0 + right_y = 0.5 + + h60 = _run_at_rate(60.0, duration_s, right_y) + h90 = _run_at_rate(90.0, duration_s, right_y) + h120 = _run_at_rate(120.0, duration_s, right_y) + + # 60Hz integrates the first frame with the nominal fallback dt (1/60), + # so we tolerate ~1 frame of drift at the highest rate. The point is + # that 90/120 Hz are NOT scaled by their period mismatch with 60 Hz + # (which would have produced 1.5x / 2x the delta). + tol = 1.0 / 60.0 * 0.35 * abs(right_y) + 1e-6 + assert math.isclose(h60, h90, abs_tol=tol), (h60, h90) + assert math.isclose(h60, h120, abs_tol=tol), (h60, h120) + assert math.isclose(h90, h120, abs_tol=tol), (h90, h120) + + def test_hip_height_unchanged_when_stick_zero(self): + """Zero thumbstick input must leave hip height at the configured initial.""" + config = LocomotionRootCmdRetargeterConfig(initial_hip_height=0.72) + r = LocomotionRootCmdRetargeter(config, name="locomotion") + + inputs = { + "controller_left": _make_controller_group(0.0), + "controller_right": _make_controller_group(0.0), + } + outputs = {"root_command": _make_output_group()} + + for i in range(60): + t_ns = i * (1_000_000_000 // 60) + ctx = ComputeContext( + graph_time=GraphTime(sim_time_ns=t_ns, real_time_ns=t_ns) + ) + r._compute_fn(inputs, outputs, ctx) + + assert float(outputs["root_command"][0][3]) == 0.72 + + def test_reset_clears_previous_timestamp(self): + """After a reset, the next step uses the fallback dt rather than a + huge gap from the pre-reset timestamp.""" + config = LocomotionRootCmdRetargeterConfig( + initial_hip_height=0.72, + rotation_scale=0.35, + ) + r = LocomotionRootCmdRetargeter(config, name="locomotion") + + inputs = { + "controller_left": _make_controller_group(0.0), + "controller_right": _make_controller_group(1.0), + } + outputs = {"root_command": _make_output_group()} + + # Advance the clock by 10 seconds across two steps. + t_a = 0 + t_b = 10_000_000_000 # 10s later + ctx_a = ComputeContext(graph_time=GraphTime(sim_time_ns=t_a, real_time_ns=t_a)) + r._compute_fn(inputs, outputs, ctx_a) + + # Reset between steps: the next step should NOT integrate over the + # 10-second gap, only over the fallback dt. + ctx_b = ComputeContext( + graph_time=GraphTime(sim_time_ns=t_b, real_time_ns=t_b), + execution_events=ExecutionEvents(reset=True), + ) + r._compute_fn(inputs, outputs, ctx_b) + h_after_reset = float(outputs["root_command"][0][3]) + + # After reset, the integration starts from initial_hip_height and uses + # the fallback dt for this frame — NOT the 10s gap from the prior step. + config = LocomotionRootCmdRetargeterConfig() + expected = 0.72 + 1.0 * config.fallback_dt * 0.35 + assert math.isclose(h_after_reset, expected, abs_tol=1e-9), h_after_reset + # And verify it is nowhere near what 10s of integration would produce + # (which would saturate at the 1.0 hip ceiling). + assert h_after_reset < 0.73 diff --git a/src/retargeters/locomotion_retargeter.py b/src/retargeters/locomotion_retargeter.py index 72ac7d81e..9c794980a 100644 --- a/src/retargeters/locomotion_retargeter.py +++ b/src/retargeters/locomotion_retargeter.py @@ -80,7 +80,10 @@ class LocomotionRootCmdRetargeterConfig: initial_hip_height: float = 0.72 movement_scale: float = 0.5 rotation_scale: float = 0.35 - dt: float = 1.0 / 60.0 # Assumed time step if not provided externally + # First-frame fallback only; steady-state dt is derived from + # ``ComputeContext.graph_time`` so the integration is correct at any + # control rate (60/90/120 Hz), not just 60 Hz. + fallback_dt: float = 1.0 / 60.0 class LocomotionRootCmdRetargeter(BaseRetargeter): @@ -97,6 +100,7 @@ def __init__(self, config: LocomotionRootCmdRetargeterConfig, name: str) -> None super().__init__(name=name) self._config = config self._hip_height = config.initial_hip_height + self._prev_real_time_ns: int | None = None def input_spec(self) -> RetargeterIOType: """Requires left and right controller inputs (Optional).""" @@ -122,6 +126,7 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N """Computes root command from controller inputs.""" if context.execution_events.reset: self._hip_height = self._config.initial_hip_height + self._prev_real_time_ns = None left_thumbstick_x = 0.0 left_thumbstick_y = 0.0 @@ -156,7 +161,15 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N # Update hip height # Right stick Y controls height change (OpenXR up=+1 = raise, so add) - dt = self._config.dt + # Derive dt from the per-step real-time clock so the integration is + # correct at any control rate. The first frame after init/reset has no + # previous timestamp, so fall back to the configured nominal dt. + now_ns = context.graph_time.real_time_ns + if self._prev_real_time_ns is None: + dt = self._config.fallback_dt + else: + dt = max(0.0, (now_ns - self._prev_real_time_ns) / 1e9) + self._prev_real_time_ns = now_ns self._hip_height += right_thumbstick_y * dt * self._config.rotation_scale self._hip_height = max(0.4, min(1.0, self._hip_height)) From 64ea03f3249e75e0f73122020b0b2485acf0459e Mon Sep 17 00:00:00 2001 From: devdeepr Date: Thu, 7 May 2026 20:43:34 +0000 Subject: [PATCH 2/2] test(retargeting): use float32 tolerance in locomotion assertions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two assertions in test_locomotion_retargeter.py compared the float32 hip-height output against a Python float with exact equality / 1e-9 tolerance. CI on Ubuntu (Python 3.10–3.13) round-tripped the values slightly off (0.7200000286 ≠ 0.72), failing both tests. Switch both checks to math.isclose with float32-friendly absolute tolerances (1e-6 for the zero-input case, 1e-5 for the post-reset fallback-dt case). The rate-invariance test already passed; only the overly tight equality checks needed to relax. Co-Authored-By: Claude Opus 4.7 (1M context) --- .../python/test_locomotion_retargeter.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py b/src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py index 6bd3ef9d5..f1957477d 100644 --- a/src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py +++ b/src/core/retargeting_engine_tests/python/test_locomotion_retargeter.py @@ -116,7 +116,8 @@ def test_hip_height_unchanged_when_stick_zero(self): ) r._compute_fn(inputs, outputs, ctx) - assert float(outputs["root_command"][0][3]) == 0.72 + # Output is float32, so allow one ulp of round-trip slack from 0.72. + assert math.isclose(float(outputs["root_command"][0][3]), 0.72, abs_tol=1e-6) def test_reset_clears_previous_timestamp(self): """After a reset, the next step uses the fallback dt rather than a @@ -150,9 +151,9 @@ def test_reset_clears_previous_timestamp(self): # After reset, the integration starts from initial_hip_height and uses # the fallback dt for this frame — NOT the 10s gap from the prior step. - config = LocomotionRootCmdRetargeterConfig() + # Output rounds to float32, so use a tolerance compatible with that. expected = 0.72 + 1.0 * config.fallback_dt * 0.35 - assert math.isclose(h_after_reset, expected, abs_tol=1e-9), h_after_reset + assert math.isclose(h_after_reset, expected, abs_tol=1e-5), h_after_reset # And verify it is nowhere near what 10s of integration would produce # (which would saturate at the 1.0 hip ceiling). assert h_after_reset < 0.73