From 21dbf01879a345ee7d13af75d8dbc552879a933a Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Fri, 3 Jul 2026 20:51:42 +0200 Subject: [PATCH 1/5] Add rate-limiting safety harness retargeters for bare position-servo followers Low-cost followers like the SO-101 track whatever position command reaches the bus at full servo torque, with no controller-side handling of anomalous targets: one bad frame (IK jump, tracking glitch, leader-stream hiccup, operator flick) becomes a full-speed slew that can strip gears or stall motors on real hardware. Add two drop-in harness nodes that bound per-frame command velocity without changing the stream contract: - EePoseRateLimiter: clamps linear [m/s] and angular [rad/s] velocity of an absolute 7-D ee_pose stream (same contract as SO101ClutchRetargeter / Se3AbsRetargeter, inserts before the reorderer). - JointRateLimiter: clamps per-joint velocity of a name-keyed joint_targets group (same contract as JointStateRetargeter's joint mode), with optional per-joint overrides. Both latch on the first valid frame after construction/reset (engage-time placement stays owned by the upstream clutch/align logic), clamp against the last emitted command, derive dt from graph real_time_ns clamped to [min_dt, max_dt] so a pipeline stall cannot authorize a teleport, hold last on dropped frames, and re-latch on reset. Below the limit they are exact pass-throughs (no smoothing lag). Covered by sim-free unit tests (pure clamp math + compute-level behavior, mirroring test_so101_retargeters.py patterns). Signed-off-by: johnnynunez --- .../python/test_rate_limiter.py | 502 ++++++++++++++++++ src/retargeters/__init__.py | 9 + src/retargeters/rate_limiter.py | 451 ++++++++++++++++ 3 files changed, 962 insertions(+) create mode 100644 src/core/retargeting_engine_tests/python/test_rate_limiter.py create mode 100644 src/retargeters/rate_limiter.py diff --git a/src/core/retargeting_engine_tests/python/test_rate_limiter.py b/src/core/retargeting_engine_tests/python/test_rate_limiter.py new file mode 100644 index 000000000..286801ab8 --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_rate_limiter.py @@ -0,0 +1,502 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Sim-free unit tests for the rate-limiting safety harness retargeters. + +Covers the safety-harness nodes that bound per-frame command velocity for bare +position-servo followers (e.g. the SO-101): + +* :class:`~isaacteleop.retargeters.EePoseRateLimiter` -- linear/angular velocity + bounds on an absolute 7-D ``ee_pose`` stream. +* :class:`~isaacteleop.retargeters.JointRateLimiter` -- per-joint velocity bounds + on a name-keyed ``joint_targets`` group. + +Each limiter is exercised at the pure-math level (the module-private clamp +helpers) and at the ``BaseRetargeter.compute`` level (build inputs/outputs, drive +frames with controlled ``GraphTime`` stamps, read the emitted tensors), with no +``gym.make``, USD, GPU, or XR device. +""" + +import math + +import numpy as np +import pytest + +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + ExecutionState, + OptionalTensorGroup, + TensorGroup, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import GraphTime +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalTensorGroupType, +) +from isaacteleop.retargeters import ( + EePoseRateLimiter, + JointRateLimiter, + RateLimiterConfig, +) +from isaacteleop.retargeters.rate_limiter import ( + EE_POSE_KEY, + JOINT_TARGETS_KEY, + _clamp_orientation_step, + _clamp_position_step, + _clamped_dt, +) + +# --------------------------------------------------------------------------- +# Helpers (mirror the patterns in test_so101_retargeters.py) +# --------------------------------------------------------------------------- + +_ID_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) +_NS = 1_000_000_000 + + +def _make_context( + *, + reset: bool = False, + state: ExecutionState = ExecutionState.RUNNING, + t_ns: int = 0, +) -> ComputeContext: + """Build a ComputeContext with the given reset flag, state, and timestamp.""" + return ComputeContext( + graph_time=GraphTime(sim_time_ns=t_ns, real_time_ns=t_ns), + execution_events=ExecutionEvents(reset=reset, execution_state=state), + ) + + +def _build_io(retargeter): + """Construct empty input/output containers for a retargeter (optionals start absent).""" + inputs = {} + for k, v in retargeter.input_spec().items(): + inputs[k] = ( + OptionalTensorGroup(v) + if isinstance(v, OptionalTensorGroupType) + else TensorGroup(v) + ) + outputs = {} + for k, v in retargeter.output_spec().items(): + outputs[k] = ( + OptionalTensorGroup(v) + if isinstance(v, OptionalTensorGroupType) + else TensorGroup(v) + ) + return inputs, outputs + + +def _pose_group(spec_type, pos, ori=_ID_QUAT) -> TensorGroup: + """Build a present 7-D ee_pose TensorGroup from a position and quaternion.""" + tg = TensorGroup(spec_type.inner_type) + tg[0] = np.concatenate( + [np.asarray(pos, dtype=np.float32), np.asarray(ori, dtype=np.float32)] + ) + return tg + + +def _set_pose_input(limiter, inputs, pos, ori=_ID_QUAT) -> None: + """Replace the limiter's ee_pose input with a present pose group.""" + spec = limiter.input_spec()[EE_POSE_KEY] + inputs[EE_POSE_KEY] = _pose_group(spec, pos, ori) + + +def _set_joint_input(limiter, inputs, values) -> None: + """Replace the limiter's joint_targets input with a present group.""" + spec = limiter.input_spec()[JOINT_TARGETS_KEY] + tg = TensorGroup(spec.inner_type) + for i, v in enumerate(values): + tg[i] = float(v) + inputs[JOINT_TARGETS_KEY] = tg + + +def _read_pose(outputs) -> np.ndarray: + """Read the 7-D ee_pose output as a numpy array.""" + return np.asarray(np.from_dlpack(outputs[EE_POSE_KEY][0]), dtype=np.float64) + + +def _read_joints(outputs, n: int) -> np.ndarray: + """Read the joint_targets output as a numpy array.""" + return np.array( + [float(outputs[JOINT_TARGETS_KEY][i]) for i in range(n)], dtype=np.float64 + ) + + +def _quat_xyzw(axis, angle_rad: float) -> np.ndarray: + """Build an [x, y, z, w] quaternion for a rotation of ``angle_rad`` about ``axis``.""" + axis = np.asarray(axis, dtype=np.float64) + axis = axis / np.linalg.norm(axis) + half = 0.5 * angle_rad + xyz = axis * math.sin(half) + return np.array([xyz[0], xyz[1], xyz[2], math.cos(half)], dtype=np.float64) + + +def _quat_angle(a: np.ndarray, b: np.ndarray) -> float: + """Geodesic angle [rad] between two unit quaternions (double-cover aware).""" + dot = abs(float(np.dot(a, b))) + return 2.0 * math.acos(min(1.0, dot)) + + +# =========================================================================== +# Pure math helpers +# =========================================================================== + + +class TestClampedDt: + """The ``_clamped_dt`` frame-delta helper.""" + + def test_no_previous_uses_nominal(self): + """With no previous stamp the nominal dt is returned.""" + assert _clamped_dt(None, 123, 0.02, 1e-4, 0.1) == pytest.approx(0.02) + + def test_duplicate_stamp_uses_nominal(self): + """A non-advancing clock falls back to the nominal dt (no frozen limiter).""" + assert _clamped_dt(500, 500, 0.02, 1e-4, 0.1) == pytest.approx(0.02) + assert _clamped_dt(500, 400, 0.02, 1e-4, 0.1) == pytest.approx(0.02) + + def test_normal_delta_passes(self): + """A delta within [min_dt, max_dt] is returned as-is.""" + assert _clamped_dt(0, 20_000_000, 0.02, 1e-4, 0.1) == pytest.approx(0.02) + + def test_stall_clamps_to_max(self): + """A multi-second stall is clamped to max_dt (no huge authorized step).""" + assert _clamped_dt(0, 5 * _NS, 0.02, 1e-4, 0.1) == pytest.approx(0.1) + + def test_tiny_delta_clamps_to_min(self): + """A near-zero delta is clamped up to min_dt.""" + assert _clamped_dt(0, 10, 0.02, 1e-4, 0.1) == pytest.approx(1e-4) + + +class TestClampPositionStep: + """The ``_clamp_position_step`` Euclidean step clamp.""" + + def test_within_limit_passes_through(self): + """A step under the limit is returned untouched.""" + prev = np.zeros(3) + tgt = np.array([0.001, 0.0, 0.0]) + out = _clamp_position_step(tgt, prev, 0.01) + np.testing.assert_allclose(out, tgt) + + def test_over_limit_is_clamped_along_line(self): + """An over-limit step lands exactly max_step along the straight line.""" + prev = np.zeros(3) + tgt = np.array([1.0, 0.0, 0.0]) + out = _clamp_position_step(tgt, prev, 0.01) + np.testing.assert_allclose(out, [0.01, 0.0, 0.0], atol=1e-12) + + def test_zero_step_is_stable(self): + """target == previous returns target (no divide-by-zero).""" + p = np.array([0.2, 0.1, 0.3]) + out = _clamp_position_step(p.copy(), p.copy(), 0.01) + np.testing.assert_allclose(out, p) + + +class TestClampOrientationStep: + """The ``_clamp_orientation_step`` geodesic rotation clamp.""" + + def test_within_limit_passes_through(self): + """A rotation under the limit reaches the target orientation.""" + tgt = _quat_xyzw([0, 0, 1], 0.01) + out = _clamp_orientation_step(tgt, _ID_QUAT.copy(), 0.1) + assert _quat_angle(out, tgt) == pytest.approx(0.0, abs=1e-9) + + def test_over_limit_advances_exactly_max_step(self): + """An over-limit rotation advances exactly max_step along the arc.""" + tgt = _quat_xyzw([0, 0, 1], 1.0) + out = _clamp_orientation_step(tgt, _ID_QUAT.copy(), 0.1) + assert _quat_angle(out, _ID_QUAT) == pytest.approx(0.1, abs=1e-9) + + def test_double_cover_takes_shortest_arc(self): + """A sign-flipped target quaternion still rotates along the shortest arc.""" + tgt = -_quat_xyzw([0, 0, 1], 1.0) # same rotation, other hemisphere + out = _clamp_orientation_step(tgt, _ID_QUAT.copy(), 0.1) + assert _quat_angle(out, _ID_QUAT) == pytest.approx(0.1, abs=1e-9) + + def test_identity_step_is_stable(self): + """target == previous is returned without axis extraction blowing up.""" + q = _quat_xyzw([0, 1, 0], 0.3) + out = _clamp_orientation_step(q.copy(), q.copy(), 0.1) + # acos-based angle recovery amplifies float rounding near 0; 1e-6 rad is + # far below any meaningful command resolution. + assert _quat_angle(out, q) == pytest.approx(0.0, abs=1e-6) + + +# =========================================================================== +# RateLimiterConfig validation +# =========================================================================== + + +class TestRateLimiterConfig: + """Constructor validation of the shared config.""" + + def test_defaults_are_valid(self): + """The default config constructs without error.""" + RateLimiterConfig() + + @pytest.mark.parametrize( + "kwargs", + [ + {"max_linear_velocity": 0.0}, + {"max_angular_velocity": -1.0}, + {"max_joint_velocity": 0.0}, + {"joint_velocity_overrides": {"elbow": 0.0}}, + {"min_dt": 0.0}, + {"min_dt": 0.05, "nominal_dt": 0.02}, + {"nominal_dt": 0.2, "max_dt": 0.1}, + ], + ) + def test_invalid_values_raise(self, kwargs): + """Non-positive limits and inconsistent dt bounds are rejected.""" + with pytest.raises(ValueError): + RateLimiterConfig(**kwargs) + + +# =========================================================================== +# EePoseRateLimiter +# =========================================================================== + + +class TestEePoseRateLimiter: + """End-to-end ``compute`` behavior of the EE-pose limiter.""" + + def _limiter(self, **cfg_kwargs) -> EePoseRateLimiter: + cfg = RateLimiterConfig(**cfg_kwargs) if cfg_kwargs else RateLimiterConfig() + return EePoseRateLimiter(name="ee_limiter", config=cfg) + + def test_first_frame_passes_through(self): + """The first valid frame latches and is emitted unclamped.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.3, 0.1, 0.2]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + np.testing.assert_allclose(_read_pose(outputs)[:3], [0.3, 0.1, 0.2], atol=1e-6) + + def test_slow_motion_is_untouched(self): + """Motion under the velocity limit is emitted exactly (no lag).""" + r = self._limiter(max_linear_velocity=0.25) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.2, 0.0, 0.1]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # 1 mm in 20 ms = 0.05 m/s, well under the 0.25 m/s limit. + _set_pose_input(r, inputs, [0.201, 0.0, 0.1]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.201, 0.0, 0.1], atol=1e-6 + ) + + def test_position_jump_is_rate_limited(self): + """A teleport-sized position jump advances only max_linear_velocity * dt.""" + r = self._limiter(max_linear_velocity=0.25) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # 0.5 m jump in 20 ms -> clamp to 0.25 * 0.02 = 5 mm. + _set_pose_input(r, inputs, [0.5, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.005, 0.0, 0.0], atol=1e-6 + ) + + def test_persistent_target_is_approached_at_bounded_speed(self): + """A held far target is approached stepwise, never faster than the limit.""" + r = self._limiter(max_linear_velocity=0.25) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + last = np.zeros(3) + for k in range(1, 6): + _set_pose_input(r, inputs, [0.5, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=k * 20_000_000)) + pos = _read_pose(outputs)[:3] + step = float(np.linalg.norm(pos - last)) + assert step <= 0.25 * 0.02 + 1e-9 + last = pos + # Monotonic progress toward the target. + assert last[0] == pytest.approx(5 * 0.005, abs=1e-6) + + def test_orientation_jump_is_rate_limited(self): + """A large orientation flip advances only max_angular_velocity * dt.""" + r = self._limiter(max_angular_velocity=1.5) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.2, 0.0, 0.1], _ID_QUAT) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # pi/2 flip in 20 ms -> clamp to 1.5 * 0.02 = 0.03 rad. + tgt = _quat_xyzw([0, 0, 1], math.pi / 2) + _set_pose_input(r, inputs, [0.2, 0.0, 0.1], tgt) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + emitted = _read_pose(outputs)[3:7] + # The emitted pose is float32; angle recovery through acos loses a few + # ulps, so compare at 1e-4 rad (~0.006 deg), far below command resolution. + assert _quat_angle(emitted, _ID_QUAT) == pytest.approx(0.03, abs=1e-4) + + def test_stall_does_not_authorize_teleport(self): + """A 5 s pipeline stall authorizes at most max_linear_velocity * max_dt.""" + r = self._limiter(max_linear_velocity=0.25, max_dt=0.1) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_pose_input(r, inputs, [1.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=5 * _NS)) + # 0.25 m/s * 0.1 s = 25 mm, not 1 m. + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.025, 0.0, 0.0], atol=1e-6 + ) + + def test_dropped_frame_holds_last(self): + """An absent input frame re-emits the last limited pose.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.3, 0.1, 0.2]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + inputs2, outputs2 = _build_io(r) # optionals start absent + r.compute(inputs2, outputs2, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_pose(outputs2)[:3], [0.3, 0.1, 0.2], atol=1e-6) + + def test_reset_relatches_without_clamping(self): + """After a reset the next frame passes through instead of slewing from the old pose.""" + r = self._limiter(max_linear_velocity=0.25) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # Reset, then a far-away pose (the task has physically reset the arm). + _set_pose_input(r, inputs, [0.4, 0.2, 0.1]) + r.compute(inputs, outputs, _make_context(reset=True, t_ns=20_000_000)) + np.testing.assert_allclose(_read_pose(outputs)[:3], [0.4, 0.2, 0.1], atol=1e-6) + + def test_degenerate_orientation_holds_previous(self): + """A zero quaternion in the target holds the last emitted orientation.""" + r = self._limiter() + inputs, outputs = _build_io(r) + start_ori = _quat_xyzw([0, 1, 0], 0.2) + _set_pose_input(r, inputs, [0.2, 0.0, 0.1], start_ori) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_pose_input(r, inputs, [0.2, 0.0, 0.1], np.zeros(4)) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + emitted = _read_pose(outputs)[3:7] + assert _quat_angle(emitted, start_ori) == pytest.approx(0.0, abs=1e-6) + + +# =========================================================================== +# JointRateLimiter +# =========================================================================== + +_JOINTS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"] + + +class TestJointRateLimiter: + """End-to-end ``compute`` behavior of the joint-space limiter.""" + + def _limiter(self, **cfg_kwargs) -> JointRateLimiter: + cfg = RateLimiterConfig(**cfg_kwargs) if cfg_kwargs else RateLimiterConfig() + return JointRateLimiter(name="joint_limiter", joint_names=_JOINTS, config=cfg) + + def test_empty_joint_names_raises(self): + """An empty joint list is rejected at construction.""" + with pytest.raises(ValueError): + JointRateLimiter(name="bad", joint_names=[]) + + def test_unknown_override_raises(self): + """A velocity override for a joint not in joint_names is rejected.""" + with pytest.raises(ValueError): + JointRateLimiter( + name="bad", + joint_names=["a"], + config=RateLimiterConfig(joint_velocity_overrides={"zz": 1.0}), + ) + + def test_first_frame_passes_through(self): + """The first valid frame latches and is emitted unclamped.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.1, -0.5, 1.2, 0.0, 0.3]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + np.testing.assert_allclose( + _read_joints(outputs, 5), [0.1, -0.5, 1.2, 0.0, 0.3], atol=1e-6 + ) + + def test_slow_motion_is_untouched(self): + """Per-joint motion under the limit is emitted exactly.""" + r = self._limiter(max_joint_velocity=1.5) + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # 0.01 rad in 20 ms = 0.5 rad/s, under the 1.5 rad/s limit. + _set_joint_input(r, inputs, [0.01] * 5) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.01] * 5, atol=1e-6) + + def test_jump_is_rate_limited_per_joint(self): + """An over-limit jump advances each joint by at most limit * dt, signed.""" + r = self._limiter(max_joint_velocity=1.5) + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # +-2 rad jumps in 20 ms -> clamp to +-1.5 * 0.02 = +-0.03 rad. + _set_joint_input(r, inputs, [2.0, -2.0, 2.0, -2.0, 2.0]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose( + _read_joints(outputs, 5), [0.03, -0.03, 0.03, -0.03, 0.03], atol=1e-6 + ) + + def test_per_joint_override_applies(self): + """A per-joint override clamps that joint tighter than the default.""" + r = self._limiter( + max_joint_velocity=1.5, + joint_velocity_overrides={"shoulder_lift": 0.5}, + ) + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_joint_input(r, inputs, [2.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + joints = _read_joints(outputs, 5) + assert joints[1] == pytest.approx(0.5 * 0.02, abs=1e-6) # overridden + assert joints[0] == pytest.approx(1.5 * 0.02, abs=1e-6) # default + + def test_dropped_frame_holds_last(self): + """An absent input frame re-emits the last limited targets.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.1, 0.2, 0.3, 0.4, 0.5]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + inputs2, outputs2 = _build_io(r) # optionals start absent + r.compute(inputs2, outputs2, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose( + _read_joints(outputs2, 5), [0.1, 0.2, 0.3, 0.4, 0.5], atol=1e-6 + ) + + def test_reset_relatches_without_clamping(self): + """After a reset the next frame passes through (fresh episode, fresh baseline).""" + r = self._limiter(max_joint_velocity=1.5) + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_joint_input(r, inputs, [1.0, -1.0, 0.5, -0.5, 0.2]) + r.compute(inputs, outputs, _make_context(reset=True, t_ns=20_000_000)) + np.testing.assert_allclose( + _read_joints(outputs, 5), [1.0, -1.0, 0.5, -0.5, 0.2], atol=1e-6 + ) + + def test_stall_does_not_authorize_jump(self): + """A long stall authorizes at most limit * max_dt per joint.""" + r = self._limiter(max_joint_velocity=1.5, max_dt=0.1) + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_joint_input(r, inputs, [3.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=10 * _NS)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.15] * 5, atol=1e-6) diff --git a/src/retargeters/__init__.py b/src/retargeters/__init__.py index ae3a9de19..c9a8d281a 100644 --- a/src/retargeters/__init__.py +++ b/src/retargeters/__init__.py @@ -19,6 +19,7 @@ - SO101ClutchRetargeter: Clutch-rebased absolute EE pose for the SO-101 5-DOF arm - SO101GripperRetargeter: Proportional (analog) jaw closedness for the SO-101 gripper - JointStateRetargeter: Generic joint-space device (leader arm, exoskeleton) -> joint or EE action + - EePoseRateLimiter / JointRateLimiter: Safety-harness velocity bounds for EE-pose / joint streams - SharpaHandRetargeter: Pinocchio/Pink IK-based retargeting for Sharpa hand - SharpaBiManualRetargeter: Bimanual version of SharpaHandRetargeter - Se3AbsRetargeter: Absolute EE pose control @@ -123,6 +124,10 @@ "JointStateRetargeterConfig", None, ), + # .rate_limiter (safety harness: per-frame velocity bounds for EE / joint streams) + "EePoseRateLimiter": (".rate_limiter", "EePoseRateLimiter", None), + "JointRateLimiter": (".rate_limiter", "JointRateLimiter", None), + "RateLimiterConfig": (".rate_limiter", "RateLimiterConfig", None), # .se3_retargeter (requires retargeters-lite extra: scipy) "Se3AbsRetargeter": (".se3_retargeter", "Se3AbsRetargeter", "retargeters-lite"), "Se3RelRetargeter": (".se3_retargeter", "Se3RelRetargeter", "retargeters-lite"), @@ -220,6 +225,10 @@ def __getattr__(name: str): # Generic joint-space device retargeters (leader arms, exoskeletons, ...) "JointStateRetargeter", "JointStateRetargeterConfig", + # Safety-harness rate limiters (per-frame velocity bounds) + "EePoseRateLimiter", + "JointRateLimiter", + "RateLimiterConfig", "Se3AbsRetargeter", "Se3RelRetargeter", "Se3RetargeterConfig", diff --git a/src/retargeters/rate_limiter.py b/src/retargeters/rate_limiter.py new file mode 100644 index 000000000..a8f8566aa --- /dev/null +++ b/src/retargeters/rate_limiter.py @@ -0,0 +1,451 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Rate-limiting safety harness retargeters (EE-pose and joint-space). + +Low-cost follower arms such as the SO-101 run bare position servos with no +controller-side handling of anomalous targets: whatever position command reaches the +bus is tracked at full servo torque. A single bad frame -- an IK jump near a +singularity, a controller tracking glitch, a leader-stream hiccup, an operator +flick -- becomes a full-speed slew that can strip gears or stall motors on real +hardware. + +This module provides two composable harness nodes that bound the *step per frame* +of an existing command stream without changing its contract, so they can be +inserted between a producing retargeter and the downstream action wiring: + +* :class:`EePoseRateLimiter` -- wraps a 7-D absolute ``ee_pose`` stream (e.g. the + output of ``SO101ClutchRetargeter`` or ``Se3AbsRetargeter``), clamping linear + velocity [m/s] and angular velocity [rad/s]. +* :class:`JointRateLimiter` -- wraps a name-keyed joint-target group (e.g. the + ``joint`` mode output of ``JointStateRetargeter``), clamping per-joint velocity + [rad/s or m/s], with optional per-joint overrides. + +Both limiters are pure trajectory governors: + +- The **first valid frame** after construction or a pipeline reset passes through + unclamped and latches the baseline. Engage-time placement is owned by the + upstream clutch / align logic (which already guarantees no snap on engage); the + limiter bounds *motion between frames*, not absolute placement. +- Each subsequent frame may move at most ``max_velocity * dt`` from the last + *emitted* command (not the last input), so a persistent far-away target is + approached at bounded speed instead of in one jump. +- ``dt`` is derived from ``context.graph_time.real_time_ns`` and clamped to + ``[min_dt, max_dt]``: a stalled or resumed pipeline (huge wall-clock gap) must + not authorize a proportionally huge step, and a duplicate timestamp must not + freeze the limiter. When no usable timestamp is available (both deltas zero or + negative), ``nominal_dt`` is used. +- A dropped input frame holds the last emitted command (matching the upstream + retargeters' hold-last convention) and does not advance the time baseline. +- A pipeline **reset** clears the baseline: the next valid frame passes through + and re-latches (the owning task resets the arm pose; re-clamping against a + stale pre-reset baseline would slew the arm across the workspace). + +The clamp is intentionally a hard per-frame velocity bound (a first-order rate +limiter), not a smoothing filter: it adds no lag below the limit -- teleop feel is +untouched until the harness actually has to intervene. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np + +from isaacteleop.retargeting_engine.interface import ( + BaseRetargeter, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + DLDataType, + FloatType, + NDArrayType, +) + +# Output group key of the EE-pose limiter -- matches the upstream SO-101 clutch / +# Se3 retargeters so the limiter is a drop-in insertion in the pipeline wiring. +EE_POSE_KEY = "ee_pose" +# Output group key of the joint limiter -- matches JointStateRetargeter's +# ``joint`` mode output so the limiter is a drop-in insertion. +JOINT_TARGETS_KEY = "joint_targets" + +# Numerical floor below which a quaternion norm is treated as degenerate and the +# previous orientation is held instead (never divide by ~zero; mirrors the +# defense-in-depth net in the SO-101 clutch retargeter). +_MIN_QUAT_NORM = 1e-6 +# Angular steps below this [rad] pass through without axis extraction: the +# rotation axis is numerically meaningless near identity and the step is +# guaranteed under any sane limit. +_MIN_ANGLE_RAD = 1e-9 + + +@dataclass +class RateLimiterConfig: + """Configuration shared by the rate-limiting harness nodes. + + Args: + max_linear_velocity: EE limiter only -- maximum linear speed [m/s] of the + emitted position. The default is a deliberately conservative + tabletop-arm bring-up value; raise it once the setup is trusted. + max_angular_velocity: EE limiter only -- maximum angular speed [rad/s] of + the emitted orientation (geodesic angle per second). + max_joint_velocity: Joint limiter only -- default maximum per-joint speed + [rad/s (revolute) or m/s (prismatic)] applied to every joint not + overridden in ``joint_velocity_overrides``. + joint_velocity_overrides: Joint limiter only -- per-joint ``{name: limit}`` + overrides of ``max_joint_velocity`` (e.g. a slower shoulder, a faster + gripper). + nominal_dt: Fallback frame period [s] used when the graph timestamps do + not advance (first frame after a latch, duplicate timestamps). + max_dt: Upper clamp [s] on the wall-clock frame delta. Bounds the step + authorized after a pipeline stall/resume; a gap larger than this is + treated as ``max_dt``. + min_dt: Lower clamp [s] on the wall-clock frame delta, guarding against + zero/near-zero deltas producing a frozen limiter. + """ + + max_linear_velocity: float = 0.25 + max_angular_velocity: float = 1.5 + max_joint_velocity: float = 1.5 + joint_velocity_overrides: dict[str, float] = field(default_factory=dict) + nominal_dt: float = 1.0 / 60.0 + max_dt: float = 0.1 + min_dt: float = 1e-4 + + def __post_init__(self) -> None: + if self.max_linear_velocity <= 0.0: + raise ValueError("max_linear_velocity must be > 0") + if self.max_angular_velocity <= 0.0: + raise ValueError("max_angular_velocity must be > 0") + if self.max_joint_velocity <= 0.0: + raise ValueError("max_joint_velocity must be > 0") + for name, limit in self.joint_velocity_overrides.items(): + if limit <= 0.0: + raise ValueError(f"joint_velocity_overrides[{name!r}] must be > 0") + if not 0.0 < self.min_dt <= self.nominal_dt <= self.max_dt: + raise ValueError("require 0 < min_dt <= nominal_dt <= max_dt") + + +def _clamped_dt( + prev_ns: int | None, now_ns: int, nominal: float, lo: float, hi: float +) -> float: + """Frame delta [s] from two ``real_time_ns`` stamps, clamped to ``[lo, hi]``. + + Falls back to ``nominal`` when there is no previous stamp or the clock did not + advance (duplicate or non-monotonic timestamps). + """ + if prev_ns is None: + return nominal + delta = (now_ns - prev_ns) * 1e-9 + if delta <= 0.0: + return nominal + return min(max(delta, lo), hi) + + +def _clamp_position_step( + target: np.ndarray, previous: np.ndarray, max_step: float +) -> np.ndarray: + """Clamp the Euclidean step from ``previous`` toward ``target`` to ``max_step`` [m]. + + Returns ``target`` unchanged when it is within reach, else the point at + ``max_step`` along the straight line from ``previous`` to ``target``. + """ + delta = target - previous + dist = float(np.linalg.norm(delta)) + if dist <= max_step or dist == 0.0: + return target + return previous + delta * (max_step / dist) + + +def _quat_normalize(q: np.ndarray) -> np.ndarray | None: + """Return ``q`` normalized, or ``None`` when degenerate (zero / non-finite).""" + norm = float(np.linalg.norm(q)) + if not np.isfinite(norm) or norm < _MIN_QUAT_NORM: + return None + return q / norm + + +def _quat_mul(a: np.ndarray, b: np.ndarray) -> np.ndarray: + """Hamilton product ``a (x) b`` of two ``[x, y, z, w]`` quaternions (scalar-last).""" + ax, ay, az, aw = a + bx, by, bz, bw = b + return np.array( + [ + aw * bx + ax * bw + ay * bz - az * by, + aw * by - ax * bz + ay * bw + az * bx, + aw * bz + ax * by - ay * bx + az * bw, + aw * bw - ax * bx - ay * by - az * bz, + ], + dtype=np.float64, + ) + + +def _quat_conjugate(q: np.ndarray) -> np.ndarray: + """Conjugate (== inverse for unit quaternions) of an ``[x, y, z, w]`` quaternion.""" + return np.array([-q[0], -q[1], -q[2], q[3]], dtype=np.float64) + + +def _clamp_orientation_step( + target: np.ndarray, previous: np.ndarray, max_step: float +) -> np.ndarray: + """Clamp the geodesic rotation from ``previous`` toward ``target`` to ``max_step`` [rad]. + + Both quaternions are ``[x, y, z, w]`` unit quaternions. Returns ``target`` + (sign-aligned with the shortest arc) when the relative rotation is within + ``max_step``, else the orientation reached by rotating ``max_step`` along the + shortest arc from ``previous`` toward ``target``. + """ + # Relative rotation in the previous frame's body frame: q_rel = prev^-1 (x) target. + q_rel = _quat_mul(_quat_conjugate(previous), target) + # Shortest arc: flip the double-cover sign so w >= 0. + if q_rel[3] < 0.0: + q_rel = -q_rel + sin_half = float(np.linalg.norm(q_rel[:3])) + angle = 2.0 * float(np.arctan2(sin_half, q_rel[3])) + if angle <= max_step or angle < _MIN_ANGLE_RAD: + # Within the limit: emit the target, but from the shortest-arc branch so + # consecutive emitted quaternions never flip hemisphere spuriously. + return _quat_mul(previous, q_rel) + axis = q_rel[:3] / sin_half + half = 0.5 * max_step + q_step = np.array( + [ + axis[0] * np.sin(half), + axis[1] * np.sin(half), + axis[2] * np.sin(half), + np.cos(half), + ], + dtype=np.float64, + ) + return _quat_mul(previous, q_step) + + +class EePoseRateLimiter(BaseRetargeter): + """Bounds the per-frame linear/angular velocity of an absolute 7-D ``ee_pose`` stream. + + Input and output are both a single 7-D ``ee_pose`` (position [m] + orientation + quaternion ``[x, y, z, w]``), the exact contract of ``SO101ClutchRetargeter`` / + ``Se3AbsRetargeter``, so this node inserts between the pose producer and the + downstream reorderer without any wiring changes. + + The first valid frame after construction / reset passes through and latches the + baseline; each later frame is clamped to ``max_linear_velocity * dt`` [m] and + ``max_angular_velocity * dt`` [rad] from the last **emitted** pose (see the + module docstring for the dt and reset semantics). + """ + + def __init__(self, name: str, config: RateLimiterConfig | None = None) -> None: + """Initialize the EE-pose rate limiter. + + Args: + name: Name identifier for this retargeter node. + config: Velocity limits and dt clamping; ``None`` uses the + conservative :class:`RateLimiterConfig` defaults. + """ + self._cfg = config if config is not None else RateLimiterConfig() + super().__init__(name=name) + self._last_pose: np.ndarray | None = None + self._last_time_ns: int | None = None + + def input_spec(self) -> RetargeterIOType: + """Requires an (optional) absolute 7-D ``ee_pose`` to govern.""" + return { + EE_POSE_KEY: OptionalType( + TensorGroupType( + EE_POSE_KEY, + [ + NDArrayType( + "pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + ) + } + + def output_spec(self) -> RetargeterIOType: + """Outputs the rate-limited absolute 7-D ``ee_pose``.""" + return { + EE_POSE_KEY: TensorGroupType( + EE_POSE_KEY, + [ + NDArrayType( + "pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Emits the input pose clamped to the configured per-frame step.""" + if context.execution_events.reset: + # Fresh episode: forget the baseline. The next valid frame passes + # through and re-latches -- clamping against the stale pre-reset pose + # would command a long cross-workspace slew. + self._last_pose = None + self._last_time_ns = None + + out = outputs[EE_POSE_KEY] + inp = inputs[EE_POSE_KEY] + if inp.is_none: + # Dropped frame: hold the last emitted pose (upstream convention). + # Keep the time baseline -- max_dt bounds the catch-up step anyway. + if self._last_pose is not None: + out[0] = self._last_pose.astype(np.float32) + return + + pose = np.asarray(np.from_dlpack(inp[0]), dtype=np.float64) + ori = _quat_normalize(pose[3:7]) + + if self._last_pose is None: + if ori is None: + # Degenerate first orientation: nothing sane to latch; hold off. + return + # First valid frame: pass through, latch the baseline. + latched = np.concatenate([pose[:3], ori]) + self._last_pose = latched + self._last_time_ns = int(context.graph_time.real_time_ns) + out[0] = latched.astype(np.float32) + return + + now_ns = int(context.graph_time.real_time_ns) + dt = _clamped_dt( + self._last_time_ns, + now_ns, + self._cfg.nominal_dt, + self._cfg.min_dt, + self._cfg.max_dt, + ) + + pos = _clamp_position_step( + pose[:3], self._last_pose[:3], self._cfg.max_linear_velocity * dt + ) + if ori is None: + # Degenerate target orientation: keep the last emitted one. + ori_limited = self._last_pose[3:7] + else: + ori_limited = _clamp_orientation_step( + ori, self._last_pose[3:7], self._cfg.max_angular_velocity * dt + ) + + limited = np.concatenate([pos, ori_limited]) + self._last_pose = limited + self._last_time_ns = now_ns + out[0] = limited.astype(np.float32) + + +class JointRateLimiter(BaseRetargeter): + """Bounds the per-frame velocity of a name-keyed joint-target group. + + Input and output are both a ``joint_targets`` group with one ``FloatType`` per + configured joint name, the exact contract of ``JointStateRetargeter``'s + ``joint`` mode, so this node inserts between the leader mirror and the action + wiring without changes. Every joint is clamped to ``limit * dt`` per frame, + where ``limit`` is ``config.joint_velocity_overrides[name]`` when present, + else ``config.max_joint_velocity``. + + The first valid frame after construction / reset passes through and latches the + baseline (startup alignment is owned upstream); later frames are clamped + against the last **emitted** targets (see the module docstring). + """ + + def __init__( + self, + name: str, + joint_names: list[str], + config: RateLimiterConfig | None = None, + ) -> None: + """Initialize the joint-space rate limiter. + + Args: + name: Name identifier for this retargeter node. + joint_names: Ordered joint names; must match the upstream producer's + output element names/order (checked by the graph at connect time). + config: Velocity limits and dt clamping; ``None`` uses the + conservative :class:`RateLimiterConfig` defaults. + """ + if not joint_names: + raise ValueError("joint_names must be non-empty") + self._cfg = config if config is not None else RateLimiterConfig() + self._joint_names = list(joint_names) + unknown = set(self._cfg.joint_velocity_overrides) - set(self._joint_names) + if unknown: + raise ValueError( + f"joint_velocity_overrides for unknown joints: {sorted(unknown)}" + ) + super().__init__(name=name) + self._limits = np.array( + [ + self._cfg.joint_velocity_overrides.get(n, self._cfg.max_joint_velocity) + for n in self._joint_names + ], + dtype=np.float64, + ) + self._last_targets: np.ndarray | None = None + self._last_time_ns: int | None = None + + def input_spec(self) -> RetargeterIOType: + """Requires an (optional) name-keyed joint-target group to govern.""" + return { + JOINT_TARGETS_KEY: OptionalType( + TensorGroupType( + JOINT_TARGETS_KEY, [FloatType(n) for n in self._joint_names] + ) + ) + } + + def output_spec(self) -> RetargeterIOType: + """Outputs the rate-limited joint targets, one ``FloatType`` per joint.""" + return { + JOINT_TARGETS_KEY: TensorGroupType( + JOINT_TARGETS_KEY, [FloatType(n) for n in self._joint_names] + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Emits the input targets clamped to the configured per-frame step.""" + if context.execution_events.reset: + # Fresh episode: forget the baseline (see EePoseRateLimiter). + self._last_targets = None + self._last_time_ns = None + + out = outputs[JOINT_TARGETS_KEY] + jin = inputs[JOINT_TARGETS_KEY] + n = len(self._joint_names) + if jin.is_none: + # Dropped frame: hold the last emitted targets. + if self._last_targets is not None: + for i in range(n): + out[i] = float(self._last_targets[i]) + return + + targets = np.array([float(jin[i]) for i in range(n)], dtype=np.float64) + + if self._last_targets is None: + # First valid frame: pass through, latch the baseline. + self._last_targets = targets + self._last_time_ns = int(context.graph_time.real_time_ns) + for i in range(n): + out[i] = float(targets[i]) + return + + now_ns = int(context.graph_time.real_time_ns) + dt = _clamped_dt( + self._last_time_ns, + now_ns, + self._cfg.nominal_dt, + self._cfg.min_dt, + self._cfg.max_dt, + ) + + max_step = self._limits * dt + delta = np.clip(targets - self._last_targets, -max_step, max_step) + limited = self._last_targets + delta + self._last_targets = limited + self._last_time_ns = now_ns + for i in range(n): + out[i] = float(limited[i]) From 22574b87e8061fa2a4e490a1ed4d0168f96b98fb Mon Sep 17 00:00:00 2001 From: Johnny Date: Sat, 4 Jul 2026 03:59:50 +0200 Subject: [PATCH 2/5] Update src/retargeters/rate_limiter.py Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- src/retargeters/rate_limiter.py | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/retargeters/rate_limiter.py b/src/retargeters/rate_limiter.py index a8f8566aa..6739b0ad1 100644 --- a/src/retargeters/rate_limiter.py +++ b/src/retargeters/rate_limiter.py @@ -118,16 +118,23 @@ class RateLimiterConfig: min_dt: float = 1e-4 def __post_init__(self) -> None: - if self.max_linear_velocity <= 0.0: - raise ValueError("max_linear_velocity must be > 0") - if self.max_angular_velocity <= 0.0: - raise ValueError("max_angular_velocity must be > 0") - if self.max_joint_velocity <= 0.0: - raise ValueError("max_joint_velocity must be > 0") + for field_name in ( + "max_linear_velocity", + "max_angular_velocity", + "max_joint_velocity", + "nominal_dt", + "max_dt", + "min_dt", + ): + value = getattr(self, field_name) + if not np.isfinite(value) or value <= 0.0: + raise ValueError(f"{field_name} must be finite and > 0") for name, limit in self.joint_velocity_overrides.items(): - if limit <= 0.0: - raise ValueError(f"joint_velocity_overrides[{name!r}] must be > 0") - if not 0.0 < self.min_dt <= self.nominal_dt <= self.max_dt: + if not np.isfinite(limit) or limit <= 0.0: + raise ValueError( + f"joint_velocity_overrides[{name!r}] must be finite and > 0" + ) + if not self.min_dt <= self.nominal_dt <= self.max_dt: raise ValueError("require 0 < min_dt <= nominal_dt <= max_dt") From 6bbb6484e1c5e850e7b04d14dc15d1526ac064ba Mon Sep 17 00:00:00 2001 From: Johnny Date: Sat, 4 Jul 2026 04:00:08 +0200 Subject: [PATCH 3/5] Update src/retargeters/rate_limiter.py Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- src/retargeters/rate_limiter.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/retargeters/rate_limiter.py b/src/retargeters/rate_limiter.py index 6739b0ad1..344525c7d 100644 --- a/src/retargeters/rate_limiter.py +++ b/src/retargeters/rate_limiter.py @@ -306,6 +306,10 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N return pose = np.asarray(np.from_dlpack(inp[0]), dtype=np.float64) + if not np.all(np.isfinite(pose[:3])): + if self._last_pose is not None: + out[0] = self._last_pose.astype(np.float32) + return ori = _quat_normalize(pose[3:7]) if self._last_pose is None: From bcf3cd61aef3e719e1eda30a446b3938c959af78 Mon Sep 17 00:00:00 2001 From: Johnny Date: Sat, 4 Jul 2026 04:00:20 +0200 Subject: [PATCH 4/5] Update src/retargeters/rate_limiter.py Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- src/retargeters/rate_limiter.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/retargeters/rate_limiter.py b/src/retargeters/rate_limiter.py index 344525c7d..26b798548 100644 --- a/src/retargeters/rate_limiter.py +++ b/src/retargeters/rate_limiter.py @@ -435,6 +435,11 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N return targets = np.array([float(jin[i]) for i in range(n)], dtype=np.float64) + if not np.all(np.isfinite(targets)): + if self._last_targets is not None: + for i in range(n): + out[i] = float(self._last_targets[i]) + return if self._last_targets is None: # First valid frame: pass through, latch the baseline. From e0af85eee26a48dab61d56f1585988b5de9ce660 Mon Sep 17 00:00:00 2001 From: Johnny Date: Sat, 4 Jul 2026 22:33:38 +0200 Subject: [PATCH 5/5] Add anomaly-rejection tier to the rate-limiting retargeters Clamping alone still walks the arm toward an anomalous target at the velocity limit. When the anomaly is an IK divergence or tracking teleport (observed: raw joint commands flipping ~100 deg in one frame), that motion should not be executed at all. Add optional reject_linear/angular/joint_velocity thresholds to RateLimiterConfig: a frame whose input velocity (vs the last accepted input, over the clamped dt) exceeds the threshold is refused outright -- the limiter holds the last emitted command and does not advance its baseline. A single bad joint rejects the whole joint frame. After max_consecutive_rejections consecutive anomalous frames (default 30, ~1 s at 30 FPS) the input is treated as a persistent new target and re-accepted, still clamped, so rejection cannot deadlock the pipeline; None holds indefinitely. Thresholds must be >= the corresponding clamp limits; rejection is disabled by default (existing behavior unchanged). Covered by 20 new unit tests including glitch-recovery, whole-frame rejection on a single bad joint, cap re-acceptance, and config validation; mutation-checked against disabled detection in both nodes. Signed-off-by: Johnny --- .../python/test_rate_limiter.py | 265 +++++++++++++++++- src/retargeters/rate_limiter.py | 175 +++++++++++- 2 files changed, 435 insertions(+), 5 deletions(-) diff --git a/src/core/retargeting_engine_tests/python/test_rate_limiter.py b/src/core/retargeting_engine_tests/python/test_rate_limiter.py index 286801ab8..d2e7ef992 100644 --- a/src/core/retargeting_engine_tests/python/test_rate_limiter.py +++ b/src/core/retargeting_engine_tests/python/test_rate_limiter.py @@ -243,13 +243,35 @@ def test_defaults_are_valid(self): {"min_dt": 0.0}, {"min_dt": 0.05, "nominal_dt": 0.02}, {"nominal_dt": 0.2, "max_dt": 0.1}, + {"reject_linear_velocity": 0.0}, + {"reject_linear_velocity": float("nan")}, + # Rejection threshold below the clamp limit would refuse motion the + # clamp band is meant to allow. + {"max_linear_velocity": 0.25, "reject_linear_velocity": 0.1}, + {"max_angular_velocity": 1.5, "reject_angular_velocity": 1.0}, + {"max_joint_velocity": 1.5, "reject_joint_velocity": 1.0}, + # ... including per-joint overrides above the rejection threshold. + { + "joint_velocity_overrides": {"elbow": 5.0}, + "reject_joint_velocity": 4.0, + }, + {"max_consecutive_rejections": 0}, ], ) def test_invalid_values_raise(self, kwargs): - """Non-positive limits and inconsistent dt bounds are rejected.""" + """Non-positive limits and inconsistent dt/rejection bounds are rejected.""" with pytest.raises(ValueError): RateLimiterConfig(**kwargs) + def test_rejection_thresholds_accept_valid_values(self): + """Rejection thresholds at or above the clamp limits construct fine.""" + RateLimiterConfig( + reject_linear_velocity=0.25, + reject_angular_velocity=1.5, + reject_joint_velocity=1.5, + max_consecutive_rejections=None, + ) + # =========================================================================== # EePoseRateLimiter @@ -500,3 +522,244 @@ def test_stall_does_not_authorize_jump(self): _set_joint_input(r, inputs, [3.0] * 5) r.compute(inputs, outputs, _make_context(t_ns=10 * _NS)) np.testing.assert_allclose(_read_joints(outputs, 5), [0.15] * 5, atol=1e-6) + + +# =========================================================================== +# Anomaly rejection (reject_* thresholds) +# =========================================================================== + + +class TestEePoseAnomalyRejection: + """Rejection tier of the EE-pose limiter: anomalous frames are not executed.""" + + def _limiter(self, **cfg_kwargs) -> EePoseRateLimiter: + cfg = RateLimiterConfig( + max_linear_velocity=0.25, + reject_linear_velocity=2.0, + reject_angular_velocity=10.0, + **cfg_kwargs, + ) + return EePoseRateLimiter(name="ee_limiter", config=cfg) + + def test_anomalous_position_jump_is_not_approached(self): + """A teleport beyond the reject envelope holds the pose entirely (no slew).""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # 0.5 m in 20 ms = 25 m/s >> 2 m/s reject threshold: hold, do not creep. + _set_pose_input(r, inputs, [0.5, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_pose(outputs)[:3], [0.0, 0.0, 0.0], atol=1e-6) + + def test_anomalous_orientation_flip_is_not_approached(self): + """An orientation teleport beyond the reject envelope holds entirely.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.2, 0.0, 0.1], _ID_QUAT) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # pi rad in 20 ms = ~157 rad/s >> 10 rad/s reject threshold. + tgt = _quat_xyzw([0, 0, 1], math.pi - 0.01) + _set_pose_input(r, inputs, [0.2, 0.0, 0.1], tgt) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + emitted = _read_pose(outputs)[3:7] + assert _quat_angle(emitted, _ID_QUAT) == pytest.approx(0.0, abs=1e-6) + + def test_fast_but_legal_motion_is_clamped_not_rejected(self): + """Motion between the clamp and reject thresholds is clamped, not refused.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # 20 mm in 20 ms = 1 m/s: above the 0.25 m/s clamp, below the 2 m/s + # reject threshold -> the clamp band handles it (5 mm step). + _set_pose_input(r, inputs, [0.02, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.005, 0.0, 0.0], atol=1e-6 + ) + + def test_glitch_recovery_resumes_from_held_pose(self): + """After a one-frame teleport glitch, sane frames resume without a jump.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.1, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # Glitch frame: rejected, held at 0.1. + _set_pose_input(r, inputs, [0.9, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_pose(outputs)[:3], [0.1, 0.0, 0.0], atol=1e-6) + + # Recovery frame near the pre-glitch input: accepted, small step allowed. + _set_pose_input(r, inputs, [0.101, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=40_000_000)) + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.101, 0.0, 0.0], atol=1e-6 + ) + + def test_persistent_target_reaccepted_after_cap(self): + """After max_consecutive_rejections the far target is re-accepted, clamped.""" + r = self._limiter(max_consecutive_rejections=3) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # 3 rejected frames, then the 4th trips the cap and re-accepts. + for k in range(1, 4): + _set_pose_input(r, inputs, [0.5, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=k * 20_000_000)) + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.0, 0.0, 0.0], atol=1e-6 + ) + _set_pose_input(r, inputs, [0.5, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=4 * 20_000_000)) + pos = _read_pose(outputs)[:3] + # Re-accepted but still clamped: at most max_linear_velocity * max_dt. + assert 0.0 < pos[0] <= 0.25 * 0.1 + 1e-9 + + def test_cap_none_holds_indefinitely(self): + """With max_consecutive_rejections=None an anomalous stream never executes.""" + r = self._limiter(max_consecutive_rejections=None) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + for k in range(1, 100): + _set_pose_input(r, inputs, [0.5, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=k * 20_000_000)) + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.0, 0.0, 0.0], atol=1e-6 + ) + + def test_reset_clears_rejection_state(self): + """A reset drops the rejection baseline: the next frame latches fresh.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_pose_input(r, inputs, [0.9, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) # rejected + + # Reset: the same far pose is now a fresh episode's first frame. + _set_pose_input(r, inputs, [0.9, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(reset=True, t_ns=40_000_000)) + np.testing.assert_allclose(_read_pose(outputs)[:3], [0.9, 0.0, 0.0], atol=1e-6) + + def test_rejection_disabled_by_default(self): + """Without reject thresholds a teleport is clamped (legacy behavior), not refused.""" + r = EePoseRateLimiter( + name="ee_limiter", config=RateLimiterConfig(max_linear_velocity=0.25) + ) + inputs, outputs = _build_io(r) + _set_pose_input(r, inputs, [0.0, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_pose_input(r, inputs, [0.5, 0.0, 0.0]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose( + _read_pose(outputs)[:3], [0.005, 0.0, 0.0], atol=1e-6 + ) + + +class TestJointAnomalyRejection: + """Rejection tier of the joint limiter: anomalous frames are not executed.""" + + def _limiter(self, **cfg_kwargs) -> JointRateLimiter: + cfg = RateLimiterConfig( + max_joint_velocity=1.5, + reject_joint_velocity=15.0, + **cfg_kwargs, + ) + return JointRateLimiter(name="joint_limiter", joint_names=_JOINTS, config=cfg) + + def test_anomalous_flip_is_not_approached(self): + """An IK-divergence-sized flip (>> reject threshold) holds all joints.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # ~100 deg (1.75 rad) in 20 ms = 87 rad/s >> 15 rad/s: refuse outright. + _set_joint_input(r, inputs, [1.75] * 5) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.0] * 5, atol=1e-6) + + def test_single_bad_joint_rejects_whole_frame(self): + """One joint over the reject threshold refuses the whole frame.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # Joints move together: executing the sane joints of an insane frame + # still produces an insane configuration. + _set_joint_input(r, inputs, [0.01, 0.01, 1.75, 0.01, 0.01]) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.0] * 5, atol=1e-6) + + def test_fast_but_legal_motion_is_clamped_not_rejected(self): + """Motion between the clamp and reject thresholds is clamped, not refused.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + # 0.1 rad in 20 ms = 5 rad/s: above the 1.5 rad/s clamp, below the + # 15 rad/s reject threshold -> clamped to 0.03 rad. + _set_joint_input(r, inputs, [0.1] * 5) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.03] * 5, atol=1e-6) + + def test_glitch_recovery_resumes_from_held_targets(self): + """After a one-frame glitch, sane frames resume without a jump.""" + r = self._limiter() + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.1] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_joint_input(r, inputs, [1.75] * 5) # glitch: rejected + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.1] * 5, atol=1e-6) + + _set_joint_input(r, inputs, [0.11] * 5) # recovery: accepted + r.compute(inputs, outputs, _make_context(t_ns=40_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.11] * 5, atol=1e-6) + + def test_persistent_target_reaccepted_after_cap(self): + """After max_consecutive_rejections the far target is re-accepted, clamped.""" + r = self._limiter(max_consecutive_rejections=2) + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + for k in range(1, 3): + _set_joint_input(r, inputs, [1.75] * 5) + r.compute(inputs, outputs, _make_context(t_ns=k * 20_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.0] * 5, atol=1e-6) + + _set_joint_input(r, inputs, [1.75] * 5) + r.compute(inputs, outputs, _make_context(t_ns=3 * 20_000_000)) + joints = _read_joints(outputs, 5) + # Re-accepted but still clamped: at most max_joint_velocity * max_dt. + assert np.all(joints > 0.0) + assert np.all(joints <= 1.5 * 0.1 + 1e-9) + + def test_rejection_disabled_by_default(self): + """Without reject_joint_velocity a flip is clamped (legacy behavior).""" + r = JointRateLimiter( + name="joint_limiter", + joint_names=_JOINTS, + config=RateLimiterConfig(max_joint_velocity=1.5), + ) + inputs, outputs = _build_io(r) + _set_joint_input(r, inputs, [0.0] * 5) + r.compute(inputs, outputs, _make_context(t_ns=0)) + + _set_joint_input(r, inputs, [1.75] * 5) + r.compute(inputs, outputs, _make_context(t_ns=20_000_000)) + np.testing.assert_allclose(_read_joints(outputs, 5), [0.03] * 5, atol=1e-6) diff --git a/src/retargeters/rate_limiter.py b/src/retargeters/rate_limiter.py index 26b798548..5be2ff82b 100644 --- a/src/retargeters/rate_limiter.py +++ b/src/retargeters/rate_limiter.py @@ -40,10 +40,21 @@ - A pipeline **reset** clears the baseline: the next valid frame passes through and re-latches (the owning task resets the arm pose; re-clamping against a stale pre-reset baseline would slew the arm across the workspace). - -The clamp is intentionally a hard per-frame velocity bound (a first-order rate -limiter), not a smoothing filter: it adds no lag below the limit -- teleop feel is -untouched until the harness actually has to intervene. +- **Anomaly rejection** (second tier, above the clamp): a frame whose *input* + velocity relative to the last accepted input exceeds the ``reject_*`` + thresholds is a discontinuity -- an IK divergence, a tracking teleport, a + stream catch-up step -- not motion to follow, and is **not executed at all**: + the limiter holds the last emitted command instead of slewing toward the + anomaly. After ``max_consecutive_rejections`` consecutive anomalous frames the + input is treated as a persistent new target and re-accepted (still velocity + clamped), so rejection cannot deadlock the pipeline; ``None`` holds + indefinitely until the input returns within the envelope or a reset arrives. + +The result is a three-band governor: normal motion passes through untouched, +modest overshoot is clamped to the velocity limit, and wild discontinuity is +refused outright. The clamp is intentionally a hard per-frame velocity bound (a +first-order rate limiter), not a smoothing filter: it adds no lag below the +limit -- teleop feel is untouched until the harness actually has to intervene. """ from __future__ import annotations @@ -107,6 +118,29 @@ class RateLimiterConfig: treated as ``max_dt``. min_dt: Lower clamp [s] on the wall-clock frame delta, guarding against zero/near-zero deltas producing a frozen limiter. + reject_linear_velocity: EE limiter only -- input linear speed [m/s] + (measured against the last **accepted input**, over the clamped + ``dt``) above which the frame is rejected outright instead of being + approached at the clamp limit. ``None`` (default) disables + rejection. Must be >= ``max_linear_velocity`` when set: a rejection + threshold below the clamp limit would refuse motion the clamp band + is meant to allow. + reject_angular_velocity: EE limiter only -- input angular speed [rad/s] + above which the frame is rejected outright. ``None`` disables. + Must be >= ``max_angular_velocity`` when set. + reject_joint_velocity: Joint limiter only -- per-joint input speed + [rad/s or m/s] above which the whole frame is rejected outright + (any single joint over the threshold rejects the frame -- joints + move together; executing the sane joints of an insane frame still + produces an insane configuration). ``None`` disables. Must be + >= ``max_joint_velocity`` and every ``joint_velocity_overrides`` + entry when set. + max_consecutive_rejections: Number of consecutive rejected frames after + which the input is re-accepted as a persistent new target (then + still approached at the clamp limits). Guards against a legitimate + regime change (e.g. the leader really did move far during a stream + outage) freezing the follower forever. ``None`` holds indefinitely; + the default trips after ~1 s of anomalous input at 30 FPS. """ max_linear_velocity: float = 0.25 @@ -116,6 +150,10 @@ class RateLimiterConfig: nominal_dt: float = 1.0 / 60.0 max_dt: float = 0.1 min_dt: float = 1e-4 + reject_linear_velocity: float | None = None + reject_angular_velocity: float | None = None + reject_joint_velocity: float | None = None + max_consecutive_rejections: int | None = 30 def __post_init__(self) -> None: for field_name in ( @@ -136,6 +174,29 @@ def __post_init__(self) -> None: ) if not self.min_dt <= self.nominal_dt <= self.max_dt: raise ValueError("require 0 < min_dt <= nominal_dt <= max_dt") + for reject_name, clamp_name in ( + ("reject_linear_velocity", "max_linear_velocity"), + ("reject_angular_velocity", "max_angular_velocity"), + ("reject_joint_velocity", "max_joint_velocity"), + ): + reject = getattr(self, reject_name) + if reject is None: + continue + if not np.isfinite(reject) or reject <= 0.0: + raise ValueError(f"{reject_name} must be finite and > 0") + if reject < getattr(self, clamp_name): + raise ValueError(f"require {reject_name} >= {clamp_name}") + if self.reject_joint_velocity is not None: + for name, limit in self.joint_velocity_overrides.items(): + if self.reject_joint_velocity < limit: + raise ValueError( + "require reject_joint_velocity >= " + f"joint_velocity_overrides[{name!r}]" + ) + if self.max_consecutive_rejections is not None and ( + self.max_consecutive_rejections < 1 + ): + raise ValueError("max_consecutive_rejections must be >= 1 or None") def _clamped_dt( @@ -197,6 +258,12 @@ def _quat_conjugate(q: np.ndarray) -> np.ndarray: return np.array([-q[0], -q[1], -q[2], q[3]], dtype=np.float64) +def _quat_geodesic_angle(a: np.ndarray, b: np.ndarray) -> float: + """Geodesic angle [rad] between two unit quaternions (double-cover aware).""" + dot = min(1.0, abs(float(np.dot(a, b)))) + return 2.0 * float(np.arccos(dot)) + + def _clamp_orientation_step( target: np.ndarray, previous: np.ndarray, max_step: float ) -> np.ndarray: @@ -244,6 +311,12 @@ class EePoseRateLimiter(BaseRetargeter): baseline; each later frame is clamped to ``max_linear_velocity * dt`` [m] and ``max_angular_velocity * dt`` [rad] from the last **emitted** pose (see the module docstring for the dt and reset semantics). + + When ``reject_linear_velocity`` / ``reject_angular_velocity`` are set, a frame + whose **input** moves faster than those thresholds relative to the last + accepted input is rejected outright -- the last emitted pose is held and the + anomalous target is never approached (see the module docstring's anomaly + rejection tier). """ def __init__(self, name: str, config: RateLimiterConfig | None = None) -> None: @@ -258,6 +331,13 @@ def __init__(self, name: str, config: RateLimiterConfig | None = None) -> None: super().__init__(name=name) self._last_pose: np.ndarray | None = None self._last_time_ns: int | None = None + # Anomaly-rejection reference: the last *accepted* input (position + + # normalized orientation), distinct from the last *emitted* pose -- the + # emitted pose lags a far target by design, and measuring input velocity + # against it would misread bounded catch-up as an anomaly. + self._last_input_pos: np.ndarray | None = None + self._last_input_ori: np.ndarray | None = None + self._rejections: int = 0 def input_spec(self) -> RetargeterIOType: """Requires an (optional) absolute 7-D ``ee_pose`` to govern.""" @@ -287,6 +367,24 @@ def output_spec(self) -> RetargeterIOType: ) } + def _is_anomalous(self, pos: np.ndarray, ori: np.ndarray | None, dt: float) -> bool: + """True when the input step from the last accepted input breaks the reject envelope.""" + cfg = self._cfg + if ( + cfg.reject_linear_velocity is not None + and self._last_input_pos is not None + and float(np.linalg.norm(pos - self._last_input_pos)) + > cfg.reject_linear_velocity * dt + ): + return True + return ( + cfg.reject_angular_velocity is not None + and ori is not None + and self._last_input_ori is not None + and _quat_geodesic_angle(ori, self._last_input_ori) + > cfg.reject_angular_velocity * dt + ) + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: """Emits the input pose clamped to the configured per-frame step.""" if context.execution_events.reset: @@ -295,6 +393,9 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N # would command a long cross-workspace slew. self._last_pose = None self._last_time_ns = None + self._last_input_pos = None + self._last_input_ori = None + self._rejections = 0 out = outputs[EE_POSE_KEY] inp = inputs[EE_POSE_KEY] @@ -320,6 +421,9 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N latched = np.concatenate([pose[:3], ori]) self._last_pose = latched self._last_time_ns = int(context.graph_time.real_time_ns) + self._last_input_pos = pose[:3].copy() + self._last_input_ori = ori.copy() + self._rejections = 0 out[0] = latched.astype(np.float32) return @@ -332,6 +436,23 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N self._cfg.max_dt, ) + if self._is_anomalous(pose[:3], ori, dt): + self._rejections += 1 + cap = self._cfg.max_consecutive_rejections + if cap is None or self._rejections <= cap: + # Anomalous frame (IK divergence, tracking teleport, stream + # catch-up): refuse it entirely -- hold the last emitted pose + # and advance no baseline (mirrors the dropped-frame path), so + # the arm never starts moving toward a discontinuity. + out[0] = self._last_pose.astype(np.float32) + return + # Cap tripped: the far input is persistent, so it is a new regime, + # not a glitch. Fall through and re-accept it -- the clamp below + # still bounds the approach velocity. + self._rejections = 0 + else: + self._rejections = 0 + pos = _clamp_position_step( pose[:3], self._last_pose[:3], self._cfg.max_linear_velocity * dt ) @@ -343,6 +464,9 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N ori, self._last_pose[3:7], self._cfg.max_angular_velocity * dt ) + self._last_input_pos = pose[:3].copy() + if ori is not None: + self._last_input_ori = ori.copy() limited = np.concatenate([pos, ori_limited]) self._last_pose = limited self._last_time_ns = now_ns @@ -362,6 +486,12 @@ class JointRateLimiter(BaseRetargeter): The first valid frame after construction / reset passes through and latches the baseline (startup alignment is owned upstream); later frames are clamped against the last **emitted** targets (see the module docstring). + + When ``reject_joint_velocity`` is set, a frame in which **any** joint's input + moves faster than that threshold relative to the last accepted input is + rejected outright -- the last emitted targets are held and the anomalous + frame is never approached (joints move together, so the whole frame is + refused; see the module docstring's anomaly rejection tier). """ def __init__( @@ -398,6 +528,11 @@ def __init__( ) self._last_targets: np.ndarray | None = None self._last_time_ns: int | None = None + # Anomaly-rejection reference: the last *accepted* input, distinct from + # the last *emitted* targets (which lag a far target by design; measuring + # input velocity against them would misread catch-up as an anomaly). + self._last_input_targets: np.ndarray | None = None + self._rejections: int = 0 def input_spec(self) -> RetargeterIOType: """Requires an (optional) name-keyed joint-target group to govern.""" @@ -423,6 +558,8 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N # Fresh episode: forget the baseline (see EePoseRateLimiter). self._last_targets = None self._last_time_ns = None + self._last_input_targets = None + self._rejections = 0 out = outputs[JOINT_TARGETS_KEY] jin = inputs[JOINT_TARGETS_KEY] @@ -445,6 +582,8 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N # First valid frame: pass through, latch the baseline. self._last_targets = targets self._last_time_ns = int(context.graph_time.real_time_ns) + self._last_input_targets = targets.copy() + self._rejections = 0 for i in range(n): out[i] = float(targets[i]) return @@ -458,10 +597,38 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N self._cfg.max_dt, ) + if ( + self._cfg.reject_joint_velocity is not None + and self._last_input_targets is not None + and bool( + np.any( + np.abs(targets - self._last_input_targets) + > self._cfg.reject_joint_velocity * dt + ) + ) + ): + self._rejections += 1 + cap = self._cfg.max_consecutive_rejections + if cap is None or self._rejections <= cap: + # Anomalous frame (e.g. IK output flipping tens of degrees in + # one frame): refuse the whole frame -- hold the last emitted + # targets and advance no baseline (mirrors the dropped-frame + # path), so the servos never start slewing toward it. + for i in range(n): + out[i] = float(self._last_targets[i]) + return + # Cap tripped: the far input is persistent -- a new regime, not a + # glitch. Fall through and re-accept it; the clamp below still + # bounds the approach velocity. + self._rejections = 0 + else: + self._rejections = 0 + max_step = self._limits * dt delta = np.clip(targets - self._last_targets, -max_step, max_step) limited = self._last_targets + delta self._last_targets = limited self._last_time_ns = now_ns + self._last_input_targets = targets.copy() for i in range(n): out[i] = float(limited[i])