Fix trajectory shake: smooth IK outliers, fix sim dt jitter#3
Merged
Conversation
…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>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Three independent fixes that together eliminate visible robot shake during the precision sweep section of the demo, plus an
@njitrewrite of the IK-outlier smoother with regression tests.Why
Recording multicast status during
precision.pyplayback 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: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.PATH_SAMPLESdefault 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
parol6/motion/trajectory.py—_smooth_singularity_outliersruns afterbatch_iksucceeds:_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 toPATH_SAMPLES, speed, and move type._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.@njit(cache=True)with explicit scalar loops; only allocations are onenp.empty(n-1)for step magnitudes and whatevernp.mediandoes internally — both bounded by path length, called once per plan, not per tick.parol6/utils/warmup.pyso the JIT cost is paid at server startup, not on first plan.parol6/server/transports/mock_serial_transport.py— passcfg.INTERVAL_Sto the JIT motion simulator instead of wallclock dt. Sim now advances by exactly one control tick per call, matching firmware behavior.parol6/config.py— bumpPATH_SAMPLESdefault 50 → 200. Denser sampling makes the median-step estimate robust on short moves.PAROL6_PATH_SAMPLESenv override unchanged.examples/precision.py— sync to match the currentdemo_showcase.pyprecision section.tests/unit/test_motion.py— newTestSmoothSingularityOutliersclass 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 < 3no-op, zero-motion no-op (median=0), sub-threshold and at-threshold non-trigger, and a randomized fuzz that verifiespositions[0]andpositions[-1]are always preserved.Test plan
pre-commit run --all-filescleanpytestclean (231 passed, 0 failed)precision.pyruns without visible J4/J6 shake during RX/RY/RZ sweepsNotes
_IK_OUTLIER_RATIOis insensitive across [10, 20+]; chosen at 10× as a safe lower bound.all_validbranch ofJointPath.from_poses— partial paths bypass it (no behavior change in failure cases).main(abc765f) and is inherited here.