Skip to content

Fix trajectory shake: smooth IK outliers, fix sim dt jitter#3

Merged
Jepson2k merged 2 commits into
mainfrom
fix-trajectory-shake
May 13, 2026
Merged

Fix trajectory shake: smooth IK outliers, fix sim dt jitter#3
Jepson2k merged 2 commits into
mainfrom
fix-trajectory-shake

Conversation

@Jepson2k
Copy link
Copy Markdown
Owner

@Jepson2k Jepson2k commented May 11, 2026

Summary

Three independent fixes that together eliminate visible robot shake during the precision sweep section of the demo, plus an @njit rewrite of the IK-outlier smoother with regression tests.

Why

Recording multicast status during precision.py playback showed real-position discontinuities of ~4° per 20ms status frame on J4/J6 during the RX/RY/RZ wrist sweeps. After ruling out controller pacing, sim drift, and downstream rendering, the root cause was traced to three independent issues:

  1. pinokin LM IK has no continuity preference at wrist singularities. Near J5 ≈ 0, J4 and J6 axes coincide; the IK has a one-parameter family of valid solutions and the LM solver can pick one several degrees off the natural chain. FK still matches, but TOPP-RA's time parameterization treats the off-branch sample as a steep joint motion and cranks up local velocity.
  2. Wallclock dt in the simulator inherits the host scheduler's jitter. On slow ticks the simulator under-advances Position_in; on fast follow-up ticks it over-advances. Visible as ghost velocity spikes in recorded status even when the commanded trajectory is bit-identical across runs.
  3. PATH_SAMPLES default of 50 produces ~7mm cartesian spacing on a 350mm linear move — sparse enough that the IK-chain step size varies enough between samples to confuse threshold-based outlier detection.

Implementation

  1. parol6/motion/trajectory.py_smooth_singularity_outliers runs after batch_ik succeeds:

    • Detection is median-relative: a chain step is an outlier if its magnitude exceeds _IK_OUTLIER_RATIO (10×) the chain's own median step. No absolute angle threshold; no separate bookend/path-length test. The ratio knob is well-behaved because real LM hops exceed 20× while normal motion stays within ~5×, and the rule is invariant to PATH_SAMPLES, speed, and move type.
    • For each contiguous run of outlier samples, linearly interpolate over the run plus _IK_OUTLIER_PADDING (4) samples on each side. Padding absorbs the LM seed-bleed that contaminates a few samples after the hop. FK deviation is sub-mm since both bookends lie on the cartesian path.
    • Implemented as @njit(cache=True) with explicit scalar loops; only allocations are one np.empty(n-1) for step magnitudes and whatever np.median does internally — both bounded by path length, called once per plan, not per tick.
    • Pre-compiled in parol6/utils/warmup.py so the JIT cost is paid at server startup, not on first plan.
  2. parol6/server/transports/mock_serial_transport.py — pass cfg.INTERVAL_S to the JIT motion simulator instead of wallclock dt. Sim now advances by exactly one control tick per call, matching firmware behavior.

  3. parol6/config.py — bump PATH_SAMPLES default 50 → 200. Denser sampling makes the median-step estimate robust on short moves. PAROL6_PATH_SAMPLES env override unchanged.

  4. examples/precision.py — sync to match the current demo_showcase.py precision section.

  5. tests/unit/test_motion.py — new TestSmoothSingularityOutliers class covers: smooth-path pass-through, single-sample hop (3 bad samples + 2·pad patched), wide rising shelf (one contiguous run), padding clamping at array start/end, n < 3 no-op, zero-motion no-op (median=0), sub-threshold and at-threshold non-trigger, and a randomized fuzz that verifies positions[0] and positions[-1] are always preserved.

Test plan

  • pre-commit run --all-files clean
  • pytest clean (231 passed, 0 failed)
  • Manual: precision.py runs without visible J4/J6 shake during RX/RY/RZ sweeps
  • Recording multicast status during a full demo: peak per-frame J4 velocity stays below the 2.45 rad/s limit (was peaking at 4+ rad/s before)

Notes

  • _IK_OUTLIER_RATIO is insensitive across [10, 20+]; chosen at 10× as a safe lower bound.
  • The smoother only runs on the all_valid branch of JointPath.from_poses — partial paths bypass it (no behavior change in failure cases).
  • pinokin pin to v0.1.6 was already on main (abc765f) and is inherited here.

Jepson2k and others added 2 commits May 10, 2026 20:47
…samples

Three independent fixes that together eliminate visible robot shake during
the precision sweep section of the demo:

1. parol6/motion/trajectory.py: detect and linearly-interp IK-chain outlier
   runs from pinokin's LM branch hops at wrist singularities (J5 near 0).
   Both single-sample and multi-sample outliers occur; bookend/path-length
   ratio < 0.5 reliably distinguishes 'chain doubles back' (outlier) from
   legitimate fast joint motion (e.g. opening sweep from a singular pose).
   Padding spreads patched joint motion across more samples so TOPP-RA
   doesn't have to crank up local joint velocity.

2. parol6/server/transports/mock_serial_transport.py: pass cfg.INTERVAL_S
   as dt instead of wallclock dt. Wallclock dt inherits the host scheduler's
   jitter, causing the simulator to under-/over-advance Position_in around
   slow ticks — visible as ghost velocity spikes in recordings even when
   the commanded trajectory is bit-identical across runs.

3. parol6/config.py: bump PATH_SAMPLES default 50 -> 200. Denser cartesian
   sampling reduces per-sample joint-norm spread and lets the IK outlier
   detector be more selective.

4. examples/precision.py: synced to match demo_showcase.py's precision
   section (was out of date).

The shake symptom was a real-position discontinuity of ~4 deg per 20ms
status frame on J4/J6 during RX/RY/RZ wrist sweeps — caught by recording
the multicast status while precision.py ran in WC and plotting velocity.
Root cause was LM IK picking off-branch solutions at the singular pose;
the smoother repairs the chain in joint space while preserving the
J4+J6 invariant exactly (sub-mm FK error).
The relative-threshold smoother (introduced in 209b6d8) was a pure-Python
function with per-sample broadcasts that allocated on every patched index.
Promote it to @njit(cache=True) with explicit scalar loops, leaving one
np.empty(n-1) scratch for the step magnitudes and whatever np.median does
internally — both bounded by path length, called once per plan.

Add a synthetic-outlier warmup call so the JIT compile happens at server
startup rather than first plan, and trim the docstring/constant comments
to just the why.

Cover the smoother with regression tests: smooth-path pass-through, single
and multi-sample hops, start/end padding clamping, short-path/zero-motion
no-ops, sub-threshold and at-threshold steps, plus a randomized fuzz that
verifies endpoint invariance.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
@Jepson2k Jepson2k merged commit 9450fa1 into main May 13, 2026
26 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant