Add optional rate-limiting retargeters for bare position-servo followers (SO-101 class)

[johnnynunez]

What

Adds two optional retargeter nodes under src/retargeters/ that govern commands before they reach a follower robot:

• JointRateLimiter — per-joint velocity clamp (with per-joint overrides, e.g. a faster gripper) against the last emitted command.
• EePoseRateLimiter — end-effector pose clamp: linear velocity [m/s] + geodesic angular velocity [rad/s] (double-cover safe).
• RateLimiterConfig — shared config (limits, min_dt/max_dt clamping, per-joint overrides, rejection thresholds).

Plus src/core/retargeting_engine_tests/python/test_rate_limiter.py (sim-free unit tests, same pattern as test_so101_retargeters.py).

Both nodes now implement a three-band governor:

1. Pass-through — motion under the velocity limits is emitted untouched (no lag, no filtering).
2. Clamp — motion over max_*_velocity is slewed toward the target at the limit.
3. Reject (new, optional) — a frame whose input velocity relative to the last accepted input exceeds reject_linear_velocity / reject_angular_velocity / reject_joint_velocity is a discontinuity (IK divergence, tracking teleport, stream catch-up step), not motion to follow, and is not executed at all: the limiter holds the last emitted command and does not creep toward the anomaly. For the joint limiter, a single joint over the threshold rejects the whole frame — joints move together, and executing the sane joints of an insane frame still produces an insane configuration.

Rejection anti-deadlock: after max_consecutive_rejections consecutive anomalous frames (default 30, ~1 s at 30 FPS) the input is treated as a persistent new target — a legitimate regime change, e.g. the leader really did move during a stream outage — and re-accepted, still clamped by band 2. None holds indefinitely until the input returns within the envelope or a reset arrives. Rejection is disabled by default (reject_* = None), so existing clamp-only behavior is unchanged; config validation enforces reject_* >= max_* (a rejection threshold below the clamp limit would refuse motion the clamp band is meant to allow).

Why

Follower arms built on bare position servos (SO-101 Feetech bus and similar) execute whatever position reaches the bus at full torque — there is no controller-side velocity/accel envelope. Any anomalous retargeting frame becomes a full-torque slew:

• IK divergence near singularities / unreachable targets (observed: raw joint commands flipping ±100° at frame rate when the target left the workspace),
• XR tracking glitches (controller pose teleports),
• stream hiccups (a stale-then-recovered leader stream emits a large catch-up step).

This class of failure physically damaged a gear train on one of our SO-101 followers during XR teleop (context: [Slack thread — SO101 harness MR into Isaac Teleop, Lior Ben Horin]).

Clamping alone bounds the damage but still walks the arm toward the anomaly at the velocity limit — a ±100° IK flip at a 90 °/s clamp is 90 °/s of confident motion toward a garbage target for as long as the divergence lasts. The rejection tier closes that gap: a frame that asks for e.g. 3000 °/s is not evidence of intent, it is evidence of malfunction, and the correct amount of motion to execute toward it is zero.

Design notes

• Clamps against the last emitted command, not the last input — a far-away target is approached at bounded velocity; it never jumps.
• Rejects against the last accepted input, not the last emitted command — the emitted pose intentionally lags a far target during catch-up; measuring input velocity against it would misread bounded catch-up as an anomaly.
• dt is clamped to [min_dt, max_dt] — a multi-second pipeline stall does not authorize a multi-second-sized step on the next frame.
• Pass-through below the limits — no added latency or filtering in normal operation.
• A rejected frame does not advance the time baseline (mirrors the dropped-frame path), so a glitch cannot inflate the next frame's authorized step.
• Honest scope: this bounds velocity only. It does not bound torque, acceleration, or collision forces — a collision at legal velocity still pushes at full servo torque. It is a safety net for anomalous frames, not a substitute for workspace/collision safety.

Testing

• 104 tests pass (test_rate_limiter.py incl. 20 new rejection/validation tests + existing test_so101_retargeters.py + test_joint_state_retargeter.py) against the locally built wheel.
• Mutation-checked: disabling _is_anomalous in the EE limiter fails 5 tests; disabling the joint-frame rejection predicate fails 4 tests; removing the position clamp or the joint clip fails the corresponding clamp tests.
• Rejection scenario exercised end-to-end against the installed wheel: a 30 FPS joint stream at ~30 °/s with one injected +100°-in-one-frame IK flip (≈3000 °/s) — the anomalous frame is held (0° motion), and the recovered stream resumes pass-through with no jump. With rejection disabled the same flip is clamped (legacy behavior preserved).
• Previously exercised on real hardware (SO-101 follower + Quest 3 XR teleop at 30 FPS, and the LeRobot isaac_teleop_to_so101 example wrapping JointRateLimiter between device.compute() and robot.send_action()): anomalous IK frames (±100° raw flips) were clamped to ≤3°/frame steps at 90 °/s; normal teleop is pass-through.
• SKIP=check-copyright-year pre-commit clean; DCO signed.

[github-actions]

📝 Docs preview is not auto-deployed for fork PRs.

A maintainer with write access to NVIDIA/IsaacTeleop can deploy a preview by
commenting /preview-docs on this PR. Once deployed, the preview
will live at:

https://nvidia.github.io/IsaacTeleop/preview/pr-727/

[coderabbitai]

📝 Walkthrough

Walkthrough

This PR adds a new rate-limiting safety-harness module (src/retargeters/rate_limiter.py) providing RateLimiterConfig, EePoseRateLimiter, and JointRateLimiter, which bound per-frame linear/angular/joint velocity based on wall-clock dt derived from graph time, with reset and dropped-frame handling. The retargeters package __init__.py is updated to lazily export these new symbols. A new test module validates clamping helper functions, config validation, and end-to-end compute behavior for both limiters.

Estimated code review effort: 4 (Complex) | ~60 minutes

Sequence Diagram(s)

sequenceDiagram
  participant Pipeline
  participant EePoseRateLimiter
  participant JointRateLimiter
  participant GraphTime

  Pipeline->>GraphTime: read real_time_ns
  Pipeline->>EePoseRateLimiter: compute(ee_pose input, context)
  EePoseRateLimiter->>EePoseRateLimiter: _clamped_dt(prev_time, real_time_ns)
  EePoseRateLimiter->>EePoseRateLimiter: clamp position/orientation step
  EePoseRateLimiter-->>Pipeline: clamped ee_pose

  Pipeline->>JointRateLimiter: compute(joint_targets input, context)
  JointRateLimiter->>JointRateLimiter: _clamped_dt(prev_time, real_time_ns)
  JointRateLimiter->>JointRateLimiter: clamp per-joint delta
  JointRateLimiter-->>Pipeline: clamped joint_targets

Loading

Estimated code review effort

Estimated code review effort: 4 (Complex) | ~60 minutes

Suggested labels

Suggested labels: enhancement, needs-review

Suggested reviewers

Suggested reviewers: retargeting-engine maintainers

🚥 Pre-merge checks | ✅ 5

✅ Passed checks (5 passed)

Check name	Status	Explanation
Docstring Coverage	✅ Passed	No functions found in the changed files to evaluate docstring coverage. Skipping docstring coverage check.
Linked Issues check	✅ Passed	Check skipped because no linked issues were found for this pull request.
Out of Scope Changes check	✅ Passed	Check skipped because no linked issues were found for this pull request.
Description Check	✅ Passed	Check skipped - CodeRabbit’s high-level summary is enabled.
Title check	✅ Passed	The title clearly matches the main change: adding optional rate-limiting retargeters for bare position-servo followers.

✨ Finishing Touches

🧪 Generate unit tests (beta)

• Create PR with unit tests

---

_{Comment @coderabbitai help to get the list of available commands.}

[coderabbitai]

Actionable comments posted: 3

🤖 Prompt for all review comments with AI agents

Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.

Inline comments:
In `@src/retargeters/rate_limiter.py`:
- Around line 301-338: The pose handling in the rate limiter path should reject
non-finite EE positions before latching or clamping, since a malformed position
can be stored as the baseline or propagate NaNs through _clamp_position_step.
Update the pose-processing logic in the rate-limiter flow around
_quat_normalize, _clamp_position_step, and the self._last_pose latch so that any
input with NaN/Inf in pose[:3] is treated as invalid and does not update the
emitted pose or internal state. Preserve the existing degenerate-orientation
handling, but add a finite-position guard before both the first-frame latch and
the later position-limiting branch.
- Around line 120-131: Update __post_init__ in the rate limiter dataclass to
reject non-finite configuration values before the existing positivity checks:
validate max_linear_velocity, max_angular_velocity, max_joint_velocity, each
entry in joint_velocity_overrides, and the dt bounds using a finiteness check so
inf and NaN are not accepted. Keep the current ordering constraints for min_dt,
nominal_dt, and max_dt, but ensure any non-finite value raises ValueError with a
clear message from __post_init__.
- Around line 426-451: Guard joint targets against NaN in the rate limiting
path: the current `np.clip` flow in the target update logic can preserve invalid
values and then store them in `_last_targets`, which poisons future outputs.
Update the frame-handling code in the target limiter method that builds
`targets` and `limited` so it detects or filters non-finite joint values before
applying the clamp, and only writes valid values into `_last_targets` and `out`;
if any target is invalid, keep the previous state or substitute a safe fallback
instead of propagating it.

🪄 Autofix (Beta)

Fix all unresolved CodeRabbit comments on this PR:

• Push a commit to this branch (recommended)
• Create a new PR with the fixes

---

ℹ️ Review info

⚙️ Run configuration

Configuration used: Path: .coderabbit.yaml

Review profile: CHILL

Plan: Enterprise

Run ID: 74e5b0dc-690c-4bc9-8ebd-f0a4b603eb11

📥 Commits

Reviewing files that changed from the base of the PR and between cb8aa84 and 21dbf01.

📒 Files selected for processing (3)

• src/core/retargeting_engine_tests/python/test_rate_limiter.py
• src/retargeters/__init__.py
• src/retargeters/rate_limiter.py