From 209b6d81f6bfa6a3bfb046e51c7d4ac419171ef4 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Sun, 10 May 2026 20:47:26 -0400
Subject: [PATCH 1/2] Fix trajectory shake: smooth IK outliers, fix sim dt
 jitter, more IK samples
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

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).
---
 examples/precision.py                         | 61 +++++++-----
 parol6/config.py                              |  7 +-
 parol6/motion/trajectory.py                   | 99 ++++++++++++++++++-
 .../transports/mock_serial_transport.py       |  9 +-
 4 files changed, 146 insertions(+), 30 deletions(-)

diff --git a/examples/precision.py b/examples/precision.py
index f52af15..91b9b83 100644
--- a/examples/precision.py
+++ b/examples/precision.py
@@ -13,14 +13,23 @@
 HOST = "127.0.0.1"
 PORT = 5001
 
+HOME_ANGLES = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0]
+HOME_TOLERANCE_DEG = 2.0
+
 with Robot(host=HOST, port=PORT, normalize_logs=True) as robot:
     rbt = robot.create_sync_client(timeout=2.0)
     rbt.wait_ready(timeout=5.0)
     rbt.simulator(True)
 
+    # Select tool, and home only if not already near the home pose
     rbt.select_tool("SSG-48")
     rbt.tool.calibrate()
-    rbt.home(wait=True)
+    current = rbt.angles()
+    if (
+        current is None
+        or max(abs(a - h) for a, h in zip(current, HOME_ANGLES)) > HOME_TOLERANCE_DEG
+    ):
+        rbt.home(wait=True)
 
     PRECISION_POSE = [0, -250, 350, -90, 0, -90]
     rbt.move_j(pose=PRECISION_POSE, speed=0.5, wait=True)
@@ -33,44 +42,44 @@
 
     # Approach pencil: move_j to 100mm above, descend linearly, grab, retract
     PENCIL_ABOVE = [-90, -81.6, 161.8, 0, -69.4, 180]
-    rbt.move_j(angles=PENCIL_ABOVE, speed=0.3, wait=True)
-    rbt.move_l([0, 0, -100, 0, 0, 0], rel=True, speed=0.2, wait=True)
+    rbt.move_j(angles=PENCIL_ABOVE, speed=0.8, wait=True)
+    rbt.move_l([0, 0, -93, 0, 0, 0], rel=True, speed=0.4, wait=True)
     rbt.tool.close(wait=True)
-    rbt.move_l([0, 0, 100, 0, 0, 0], rel=True, speed=0.2, wait=True)
-    rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True)
+    rbt.move_l([0, 0, 93, 0, 0, 0], rel=True, speed=0.4, wait=True)
+    rbt.move_j(pose=PRECISION_POSE, speed=0.8, wait=True)
 
-    # Offset TCP to pencil tip (~100mm exposed below gripper)
-    rbt.set_tcp_offset(0, 0, -100)
+    # Offset TCP to pencil tip (~100mm exposed below gripper). The pencil is
+    # clamped perpendicular to the gripper's jaw-closing direction, hanging
+    # along tool -X — that's the axis the offset goes on, not Z.
+    rbt.set_tcp_offset(-100, 0, 0)
 
     # Pencil tip traces straight lines (linear precision demo)
-    # Forward/back (tool Z = world -Y at this pose)
-    rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
-    rbt.move_l([0, 0, -200, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
-    rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
-    # Side to side (tool Y = world -X at this pose)
-    rbt.move_l([0, 60, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
-    rbt.move_l([0, -120, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
-    rbt.move_l([0, 60, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True)
+    rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.8, frame="TRF", rel=True, wait=True)
+    rbt.move_l([0, 0, -200, 0, 0, 0], speed=0.8, frame="TRF", rel=True, wait=True)
+    rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.8, frame="TRF", rel=True, wait=True)
 
-    # Precision TRF rotations — pencil tip stays stationary while wrist rotates
-    SWEEP = 20
+    # Precision TRF rotations — pencil tip stays stationary while wrist rotates.
+    # 40° is the largest sweep that keeps every axis IK-reachable from this pose
+    # with the 100mm pencil offset.
+    SWEEP = 40
     for axis in range(3):
         delta = [0, 0, 0, 0, 0, 0]
         delta[3 + axis] = -SWEEP
-        rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
+        rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True)
         delta[3 + axis] = SWEEP
-        rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
-        rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
+        rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True)
+        rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True)
         delta[3 + axis] = -SWEEP
-        rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True)
+        rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True)
 
     # Place pencil back: descend linearly, release, retract
     rbt.set_tcp_offset(0, 0, 0)
-    rbt.move_j(angles=PENCIL_ABOVE, speed=0.3, wait=True)
-    rbt.move_l([0, 0, -100, 0, 0, 0], rel=True, speed=0.2, wait=True)
+    rbt.move_j(angles=PENCIL_ABOVE, speed=0.8, wait=True)
+    rbt.move_l([0, 0, -93, 0, 0, 0], rel=True, speed=0.4, wait=True)
     rbt.tool.open(wait=True)
-    rbt.move_l([0, 0, 100, 0, 0, 0], rel=True, speed=0.2, wait=True)
+    rbt.move_l([0, 0, 93, 0, 0, 0], rel=True, speed=0.4, wait=True)
 
-    rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True)
-    rbt.home(wait=True)
+    # Return to home position (joint move, not the full homing sequence)
+    rbt.move_j(pose=PRECISION_POSE, speed=0.8, wait=True)
+    rbt.move_j(angles=HOME_ANGLES, speed=0.8, wait=True)
     print("Done!")
diff --git a/parol6/config.py b/parol6/config.py
index 8c3ef16..476b9c6 100644
--- a/parol6/config.py
+++ b/parol6/config.py
@@ -48,8 +48,11 @@ def _trace(self, msg, *args, **kwargs):
 NEAR_MM_TOL_MM: float = 2.0  # Proximity threshold for considering positions "near" (mm)
 ENTRY_MM_TOL_MM: float = 5.0  # Entry trajectory threshold for smooth motion (mm)
 
-# Trajectory path sampling (fixed samples for TOPP-RA input)
-PATH_SAMPLES: int = int(os.getenv("PAROL6_PATH_SAMPLES", "50"))
+# Trajectory path sampling (IK waypoints fed to TOPP-RA's piecewise-linear path).
+# 200 halves per-tick acceleration jitter on the wrist relative to the old 50,
+# converging by ~500. Higher values cost one extra IK solve per sample at
+# trajectory build time.
+PATH_SAMPLES: int = int(os.getenv("PAROL6_PATH_SAMPLES", "200"))
 
 # Centralized loop interval (seconds).
 INTERVAL_S: float = max(1e-6, 1.0 / max(CONTROL_RATE_HZ, 1.0))
diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py
index 95c0a20..62f1331 100644
--- a/parol6/motion/trajectory.py
+++ b/parol6/motion/trajectory.py
@@ -99,6 +99,101 @@ def from_string(cls, name: str) -> ProfileType:
             return cls.TOPPRA
 
 
+# Threshold (radians) for detecting single-sample IK outliers in batch_ik
+# output. At wrist singularities (J5 ≈ 0) the IK has a 1-parameter family of
+# valid solutions; pinokin's LM picks one without continuity bias and can
+# return a sample whose joints are several degrees off the natural chain
+# even though FK matches. ~3° in joint norm catches the wrist-flip pattern
+# (J4 ±X / J6 ∓X) without false-positiving on legitimate sharp turns.
+_IK_OUTLIER_THRESHOLD_RAD: float = 0.0524  # ≈ 3°
+
+# Number of "good" samples on each side of an outlier run to also re-interp.
+# Spreading the singular-region joint motion across more samples lowers
+# the residual velocity pulse TOPPRA produces (the patched samples otherwise
+# have ~15% steeper per-sample step than the surrounding chain).
+_IK_OUTLIER_PADDING: int = 8
+
+# Bookend / path-length ratio under which a run is treated as an LM outlier
+# (chain doubles back on itself). A near-1.0 ratio means the chain is
+# straight in joint space — legitimate fast motion, not an outlier. We've
+# seen ratios ~0.03 for true LM outliers vs. 1.0 for the legitimate fast
+# wrist motion at the start of sweeps from a singular pose.
+_IK_OUTLIER_RATIO: float = 0.5
+
+
+def _smooth_singularity_outliers(positions: NDArray[np.float64]) -> int:
+    """In-place repair of IK-chain outlier runs from wrist-singularity branch hops.
+
+    pinokin's LM IK has no continuity preference, so at wrist-singular poses
+    (J5 ≈ 0) it can pick an IK solution several degrees away from the natural
+    chain. Both single-sample and multi-sample outliers occur; both look like
+    a wrist flip in J4 / J6 since J4+J6 is the only invariant at the
+    singularity.
+
+    Algorithm:
+      1. Mark a sample "bad" if either adjacent step exceeds a joint-norm
+         threshold (catches the LM-picked off-branch samples).
+      2. For each contiguous run of bad samples, also pull in some good
+         samples on either side ("padding") and linear-interp the whole
+         expanded window. Padding spreads the necessary joint motion across
+         more samples so TOPPRA's time profile doesn't have to crank up
+         joint velocity locally — eliminates the residual velocity pulse
+         that pure run-replacement leaves behind.
+
+    The interp preserves the J4+J6 invariant exactly and gives sub-mm FK
+    error since both bookends lie on the cartesian path.
+
+    Returns the number of samples patched.
+    """
+    n = len(positions)
+    if n < 3:
+        return 0
+    threshold = _IK_OUTLIER_THRESHOLD_RAD
+    pad = _IK_OUTLIER_PADDING
+
+    # Per-step joint-norm magnitudes
+    diffs = np.linalg.norm(np.diff(positions, axis=0), axis=1)
+
+    # Mark a sample bad if either adjacent step is over threshold. Endpoints
+    # always treated as good (they're the IK seed / final target).
+    bad = np.zeros(n, dtype=bool)
+    bad[1:-1] = (diffs[:-1] > threshold) | (diffs[1:] > threshold)
+
+    n_patched = 0
+    i = 1
+    while i < n - 1:
+        if not bad[i]:
+            i += 1
+            continue
+        # Find end of run
+        j = i
+        while j < n - 1 and bad[j]:
+            j += 1
+        # Run is [i, j); bookended by good samples i-1 and j.
+        # Distinguish LM outlier ("out and back" jump) from legitimate fast
+        # joint motion: outliers have a small bookend distance vs. their
+        # intra-run path length, i.e. the chain doubles back on itself.
+        # Legitimate fast motion has bookend ≈ path length (ratio ≈ 1).
+        bookend_dq = float(np.linalg.norm(positions[j] - positions[i - 1]))
+        path_len = float(diffs[i - 1 : j].sum())
+        if path_len > 0 and (bookend_dq / path_len) < _IK_OUTLIER_RATIO:
+            # Treat as LM outlier — replace the run + padding with linear
+            # interp between bookends.
+            lo = max(1, i - pad)
+            hi = min(n - 1, j + pad)
+            run_span = hi - (lo - 1)
+            delta = positions[hi] - positions[lo - 1]
+            for k in range(lo, hi):
+                alpha = (k - (lo - 1)) / run_span
+                positions[k] = positions[lo - 1] + alpha * delta
+                n_patched += 1
+            i = hi + 1
+        else:
+            # Legitimate fast joint motion — leave it alone.
+            i = j + 1
+    return n_patched
+
+
 @dataclass
 class JointPath:
     """
@@ -190,7 +285,9 @@ def from_poses(
         )
 
         if result.all_valid:
-            return cls(positions=result.joint_positions)
+            positions = np.asarray(result.joint_positions, dtype=np.float64)
+            _smooth_singularity_outliers(positions)
+            return cls(positions=positions)
 
         valid = np.array(result.valid, dtype=np.bool_)
         # Count consecutive valid from start
diff --git a/parol6/server/transports/mock_serial_transport.py b/parol6/server/transports/mock_serial_transport.py
index 4cb4c38..4434bd2 100644
--- a/parol6/server/transports/mock_serial_transport.py
+++ b/parol6/server/transports/mock_serial_transport.py
@@ -550,8 +550,15 @@ def tick_simulation(
         if not self._connected:
             return
 
+        # Use the controller's fixed control interval rather than wallclock
+        # dt so the simulator behaves like the firmware (which reads encoders
+        # on its own fixed-rate timer). Wallclock dt inherits the host's
+        # scheduling jitter, which causes the simulator to under- or
+        # over-advance Position_in around slow ticks — visible as ghost
+        # spikes in recorded velocities even though the commanded trajectory
+        # is bit-identical across runs.
         now = time.perf_counter()
-        dt = now - self._state.last_update
+        dt = cfg.INTERVAL_S
         self._state.last_update = now
 
         # Snap gripper position if teleport requested

From ea4afc9627b0258a2b6132d307a5cf785854e3d5 Mon Sep 17 00:00:00 2001
From: jepson2k <55201008+Jepson2k@users.noreply.github.com>
Date: Wed, 13 May 2026 16:18:57 -0400
Subject: [PATCH 2/2] njit IK-outlier smoother, warmup hookup, regression tests
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

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>
---
 parol6/motion/trajectory.py | 121 +++++++++++++--------------------
 parol6/utils/warmup.py      |   9 +++
 tests/unit/test_motion.py   | 130 ++++++++++++++++++++++++++++++++++++
 3 files changed, 185 insertions(+), 75 deletions(-)

diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py
index 62f1331..bbcddb0 100644
--- a/parol6/motion/trajectory.py
+++ b/parol6/motion/trajectory.py
@@ -18,6 +18,7 @@
 from enum import Enum
 
 import numpy as np
+from numba import njit
 from numpy.typing import NDArray
 from ruckig import InputParameter, OutputParameter, Result, Ruckig  # type: ignore[unresolved-import, ty:unresolved-import]
 
@@ -99,98 +100,68 @@ def from_string(cls, name: str) -> ProfileType:
             return cls.TOPPRA
 
 
-# Threshold (radians) for detecting single-sample IK outliers in batch_ik
-# output. At wrist singularities (J5 ≈ 0) the IK has a 1-parameter family of
-# valid solutions; pinokin's LM picks one without continuity bias and can
-# return a sample whose joints are several degrees off the natural chain
-# even though FK matches. ~3° in joint norm catches the wrist-flip pattern
-# (J4 ±X / J6 ∓X) without false-positiving on legitimate sharp turns.
-_IK_OUTLIER_THRESHOLD_RAD: float = 0.0524  # ≈ 3°
+# Step is anomalous if its magnitude exceeds this multiple of the chain's
+# median step. Relative threshold keeps the rule invariant to sample density,
+# speed, and move type. Insensitive in [10, 20+]; real LM hops exceed 20×.
+_IK_OUTLIER_RATIO: float = 10.0
 
-# Number of "good" samples on each side of an outlier run to also re-interp.
-# Spreading the singular-region joint motion across more samples lowers
-# the residual velocity pulse TOPPRA produces (the patched samples otherwise
-# have ~15% steeper per-sample step than the surrounding chain).
-_IK_OUTLIER_PADDING: int = 8
-
-# Bookend / path-length ratio under which a run is treated as an LM outlier
-# (chain doubles back on itself). A near-1.0 ratio means the chain is
-# straight in joint space — legitimate fast motion, not an outlier. We've
-# seen ratios ~0.03 for true LM outliers vs. 1.0 for the legitimate fast
-# wrist motion at the start of sweeps from a singular pose.
-_IK_OUTLIER_RATIO: float = 0.5
+# Symmetric padding around each outlier run; absorbs LM seed-bleed into the
+# samples just after the hop. FK deviation scales linearly with pad.
+_IK_OUTLIER_PADDING: int = 4
 
 
+@njit(cache=True)
 def _smooth_singularity_outliers(positions: NDArray[np.float64]) -> int:
-    """In-place repair of IK-chain outlier runs from wrist-singularity branch hops.
-
-    pinokin's LM IK has no continuity preference, so at wrist-singular poses
-    (J5 ≈ 0) it can pick an IK solution several degrees away from the natural
-    chain. Both single-sample and multi-sample outliers occur; both look like
-    a wrist flip in J4 / J6 since J4+J6 is the only invariant at the
-    singularity.
-
-    Algorithm:
-      1. Mark a sample "bad" if either adjacent step exceeds a joint-norm
-         threshold (catches the LM-picked off-branch samples).
-      2. For each contiguous run of bad samples, also pull in some good
-         samples on either side ("padding") and linear-interp the whole
-         expanded window. Padding spreads the necessary joint motion across
-         more samples so TOPPRA's time profile doesn't have to crank up
-         joint velocity locally — eliminates the residual velocity pulse
-         that pure run-replacement leaves behind.
-
-    The interp preserves the J4+J6 invariant exactly and gives sub-mm FK
-    error since both bookends lie on the cartesian path.
-
-    Returns the number of samples patched.
+    """In-place repair of LM-IK branch hops near wrist singularities.
+
+    pinokin's LM picks IK solutions without a continuity preference, so at
+    J5 ≈ 0 it can jump several degrees off the natural chain in one or two
+    samples. Detected as steps > _IK_OUTLIER_RATIO × the chain's median
+    step; replaced by linear interpolation over the run plus padding.
     """
-    n = len(positions)
+    n = positions.shape[0]
+    dims = positions.shape[1]
     if n < 3:
         return 0
-    threshold = _IK_OUTLIER_THRESHOLD_RAD
-    pad = _IK_OUTLIER_PADDING
 
-    # Per-step joint-norm magnitudes
-    diffs = np.linalg.norm(np.diff(positions, axis=0), axis=1)
+    diffs = np.empty(n - 1)
+    for i in range(n - 1):
+        s = 0.0
+        for c in range(dims):
+            v = positions[i + 1, c] - positions[i, c]
+            s += v * v
+        diffs[i] = np.sqrt(s)
 
-    # Mark a sample bad if either adjacent step is over threshold. Endpoints
-    # always treated as good (they're the IK seed / final target).
-    bad = np.zeros(n, dtype=bool)
-    bad[1:-1] = (diffs[:-1] > threshold) | (diffs[1:] > threshold)
+    median_step = np.median(diffs)
+    if median_step == 0.0:
+        return 0
+    threshold = _IK_OUTLIER_RATIO * median_step
 
     n_patched = 0
     i = 1
     while i < n - 1:
-        if not bad[i]:
+        if diffs[i - 1] <= threshold and diffs[i] <= threshold:
             i += 1
             continue
-        # Find end of run
         j = i
-        while j < n - 1 and bad[j]:
+        while j < n - 1 and (diffs[j - 1] > threshold or diffs[j] > threshold):
             j += 1
-        # Run is [i, j); bookended by good samples i-1 and j.
-        # Distinguish LM outlier ("out and back" jump) from legitimate fast
-        # joint motion: outliers have a small bookend distance vs. their
-        # intra-run path length, i.e. the chain doubles back on itself.
-        # Legitimate fast motion has bookend ≈ path length (ratio ≈ 1).
-        bookend_dq = float(np.linalg.norm(positions[j] - positions[i - 1]))
-        path_len = float(diffs[i - 1 : j].sum())
-        if path_len > 0 and (bookend_dq / path_len) < _IK_OUTLIER_RATIO:
-            # Treat as LM outlier — replace the run + padding with linear
-            # interp between bookends.
-            lo = max(1, i - pad)
-            hi = min(n - 1, j + pad)
-            run_span = hi - (lo - 1)
-            delta = positions[hi] - positions[lo - 1]
-            for k in range(lo, hi):
-                alpha = (k - (lo - 1)) / run_span
-                positions[k] = positions[lo - 1] + alpha * delta
-                n_patched += 1
-            i = hi + 1
-        else:
-            # Legitimate fast joint motion — leave it alone.
-            i = j + 1
+        lo = i - _IK_OUTLIER_PADDING
+        if lo < 1:
+            lo = 1
+        hi = j + _IK_OUTLIER_PADDING
+        if hi > n - 1:
+            hi = n - 1
+        span = hi - (lo - 1)
+        inv_span = 1.0 / span
+        for k in range(lo, hi):
+            alpha = (k - (lo - 1)) * inv_span
+            for c in range(dims):
+                positions[k, c] = positions[lo - 1, c] + alpha * (
+                    positions[hi, c] - positions[lo - 1, c]
+                )
+            n_patched += 1
+        i = hi + 1
     return n_patched
 
 
diff --git a/parol6/utils/warmup.py b/parol6/utils/warmup.py
index 7a968a5..d2191c5 100644
--- a/parol6/utils/warmup.py
+++ b/parol6/utils/warmup.py
@@ -33,6 +33,7 @@
     _pose_to_tangent_jit,
     _tangent_to_pose_jit,
 )
+from parol6.motion.trajectory import _smooth_singularity_outliers
 from parol6.protocol.wire import (
     _pack_bitfield,
     _pack_positions,
@@ -295,6 +296,14 @@ def warmup_jit() -> float:
     # parol6/commands/servo_commands.py
     _max_vel_ratio_jit(dummy_6f, dummy_6f)
 
+    # parol6/motion/trajectory.py — non-trivial array exercises every branch
+    # (diff loop, median, bad-detection, interp loop).
+    dummy_chain = np.zeros((10, 6), dtype=np.float64)
+    for i in range(10):
+        dummy_chain[i] = i * 0.01
+    dummy_chain[5, 3] += 1.0  # synthetic outlier so the interp loop compiles
+    _smooth_singularity_outliers(dummy_chain)
+
     elapsed = time.perf_counter() - start
     logger.info(f"\tJIT warmup completed in {elapsed * 1000:.1f}ms")
     return elapsed
diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py
index 531c0c4..dd2d3c9 100644
--- a/tests/unit/test_motion.py
+++ b/tests/unit/test_motion.py
@@ -5,6 +5,11 @@
 
 from parol6.config import INTERVAL_S, LIMITS, steps_to_rad
 from parol6.motion import JointPath, ProfileType, Trajectory, TrajectoryBuilder
+from parol6.motion.trajectory import (
+    _IK_OUTLIER_PADDING,
+    _IK_OUTLIER_RATIO,
+    _smooth_singularity_outliers,
+)
 
 
 class TestJointPath:
@@ -231,3 +236,128 @@ def test_getitem_returns_step(self):
         assert np.array_equal(traj[0], steps[0])
         assert np.array_equal(traj[5], steps[5])
         assert np.array_equal(traj[-1], steps[-1])
+
+
+class TestSmoothSingularityOutliers:
+    """Tests for _smooth_singularity_outliers: LM-IK branch-hop repair."""
+
+    @staticmethod
+    def _smooth_chain(n: int = 40, step: float = 0.01) -> np.ndarray:
+        """A 6-DOF chain with uniform per-sample step magnitude."""
+        positions = np.zeros((n, 6), dtype=np.float64)
+        for i in range(n):
+            positions[i] = i * step
+        return positions
+
+    def test_smooth_path_unchanged(self):
+        positions = self._smooth_chain()
+        original = positions.copy()
+        n = _smooth_singularity_outliers(positions)
+        assert n == 0
+        assert np.array_equal(positions, original)
+
+    def test_single_sample_hop_is_smoothed(self):
+        """One outlier sample fires bad-flags on both neighbors (both adjacent
+        diffs are large) → run of 3, plus pad on each side → 2*pad+3 patched."""
+        positions = self._smooth_chain(n=40)
+        bad_idx = 20
+        positions[bad_idx, 3] += 5.0  # 5 rad J4 jump — well past 10× median
+
+        n_patched = _smooth_singularity_outliers(positions)
+        assert n_patched == 2 * _IK_OUTLIER_PADDING + 3
+        # The outlier is gone; uniform-step chain interpolates exactly.
+        assert positions[bad_idx, 3] == pytest.approx(bad_idx * 0.01, abs=1e-9)
+        # Samples just outside the patched window are untouched.
+        outside_lo = bad_idx - 1 - _IK_OUTLIER_PADDING - 1
+        outside_hi = bad_idx + 1 + _IK_OUTLIER_PADDING
+        assert positions[outside_lo, 3] == pytest.approx(outside_lo * 0.01)
+        assert positions[outside_hi, 3] == pytest.approx(outside_hi * 0.01)
+
+    def test_multi_sample_run_is_smoothed(self):
+        """Wide hop with monotonically rising outliers → each step is large,
+        so the whole shelf forms one contiguous bad run."""
+        positions = self._smooth_chain(n=40)
+        # Use increasing magnitudes so every adjacent diff exceeds threshold.
+        positions[20, 3] += 4.0
+        positions[21, 3] += 8.0
+        positions[22, 3] += 12.0
+
+        n_patched = _smooth_singularity_outliers(positions)
+        # Bad samples: {19, 20, 21, 22, 23} → run [19, 24).
+        # Patched: [max(1,19-pad), min(n-1,23+pad)+1) = [15, 28) → 13.
+        assert n_patched == 5 + 2 * _IK_OUTLIER_PADDING
+        # All run samples land back on the chain.
+        for k in (20, 21, 22):
+            assert positions[k, 3] == pytest.approx(k * 0.01, abs=1e-9)
+
+    def test_outlier_near_start_clamps_padding(self):
+        """Padding clamps at the array boundary; bookend at index 0."""
+        positions = self._smooth_chain(n=40)
+        positions[2, 3] += 5.0
+        original_first = positions[0].copy()
+        original_last = positions[-1].copy()
+
+        _smooth_singularity_outliers(positions)
+        assert positions[2, 3] == pytest.approx(2 * 0.01, abs=1e-9)
+        # Endpoints must never be modified.
+        assert np.array_equal(positions[0], original_first)
+        assert np.array_equal(positions[-1], original_last)
+
+    def test_outlier_near_end_clamps_padding(self):
+        positions = self._smooth_chain(n=40)
+        positions[-3, 3] += 5.0
+        original_first = positions[0].copy()
+        original_last = positions[-1].copy()
+
+        _smooth_singularity_outliers(positions)
+        assert positions[-3, 3] == pytest.approx((40 - 3) * 0.01, abs=1e-9)
+        assert np.array_equal(positions[0], original_first)
+        assert np.array_equal(positions[-1], original_last)
+
+    def test_short_path_is_noop(self):
+        for n in (0, 1, 2):
+            positions = np.zeros((n, 6), dtype=np.float64)
+            assert _smooth_singularity_outliers(positions) == 0
+
+    def test_zero_motion_is_noop(self):
+        """Median step = 0 → can't form a ratio threshold; bail."""
+        positions = np.zeros((20, 6), dtype=np.float64)
+        positions[10, 3] = 5.0  # would-be outlier but median is 0
+        n = _smooth_singularity_outliers(positions)
+        assert n == 0
+        assert positions[10, 3] == 5.0
+
+    def test_below_threshold_step_not_smoothed(self):
+        """Steps within `_IK_OUTLIER_RATIO`× median are normal motion, not hops."""
+        positions = self._smooth_chain(n=40)
+        # Bump one sample by ~5× the median step — should NOT trigger.
+        positions[20, 3] += 5 * 0.01
+        original = positions.copy()
+        n = _smooth_singularity_outliers(positions)
+        assert n == 0
+        assert np.array_equal(positions, original)
+
+    def test_threshold_exactly_at_ratio_not_smoothed(self):
+        """Strict > comparison: step == threshold is NOT an outlier."""
+        positions = self._smooth_chain(n=40)
+        # Each step ~ sqrt(6) * 0.01; bump just at the threshold boundary.
+        median_step = np.sqrt(6) * 0.01
+        # Add exactly ratio * median to one component so the step magnitude
+        # is just over the legitimate motion but well within tolerance.
+        positions[20, 3] += _IK_OUTLIER_RATIO * median_step - 0.5 * median_step
+        n = _smooth_singularity_outliers(positions)
+        assert n == 0
+
+    def test_repair_preserves_endpoints(self):
+        """No matter where the hop is, positions[0] and positions[-1] are
+        always exactly preserved (they're the IK seed and final target)."""
+        rng = np.random.default_rng(0)
+        for _ in range(20):
+            positions = self._smooth_chain(n=50)
+            hop_idx = int(rng.integers(2, 48))
+            positions[hop_idx, rng.integers(0, 6)] += rng.uniform(2.0, 8.0)
+            first = positions[0].copy()
+            last = positions[-1].copy()
+            _smooth_singularity_outliers(positions)
+            assert np.array_equal(positions[0], first)
+            assert np.array_equal(positions[-1], last)