Sync fork: v0.2.5 + fixes (trajectory shake, gripper stall, TCP offset, async cleanup)

CLAUDE.md

@@ -132,18 +132,13 @@ For streamable commands (`streamable = True`), `do_setup()` also runs at high fr
 ## Code Style
 
 - **Comments**: Describe the final code state, not what changed. Avoid "changed X to Y" or "added this because..." comments.
-- **Git commits/PRs**: No emoji, no "Generated by..." footers, no co-author boilerplate.
 - **Type annotations**: Fix type errors properly instead of using `# type: ignore`. Prefer:
   - `@overload` decorators for functions with different return types based on input
   - `assert` statements to narrow types after None checks
   - `cast()` from typing when the type is known but the checker can't infer it
   - `np.atleast_1d()` or similar to guarantee array returns from numpy functions
   - Only use `# type: ignore` as a last resort for genuine type checker limitations (e.g., numpy's `ArrayLike` being too broad)
 
-## Shell Command Style
-
-- **Do NOT use compound `cd` commands** like `cd foo && git status` or `cd foo && pytest`. Compound commands that cross directories require explicit user approval and slow things down. Instead, issue a standalone `cd foo` command first, then run the action as a separate command. The Bash tool's working directory persists between calls.
-
 ## Testing Guidelines
 
 **When CI tests fail, fix them.** Don't waste time analyzing whether failures are "related to your changes" — just fix all failing tests. The goal is a green CI, not attribution.

examples/demo_showcase.py

@@ -121,7 +121,7 @@ def circle_pt(cx, cz, angle_deg):
     rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True)
 
     # Offset TCP to pencil tip (~100mm exposed below gripper)
-    rbt.set_tcp_offset(-100, 0, 0)
+    rbt.set_tcp_offset(0, 0, -100)
 
     # Pencil tip traces straight lines (linear precision demo)
     # Forward/back (tool Z = world -Y at this pose)

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)
+    # 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!")

parol6/client/sync_client.py

@@ -102,7 +102,7 @@ def _run(coro: Coroutine[Any, Any, T]) -> T:
     # A loop is running in this thread; blocking would be unsafe.
     raise RuntimeError(
         "RobotClient was used while an event loop is running.\n"
-        "Use AsyncRobotClient and `await` the method instead."
+        "Construct an AsyncRobotClient in this loop and `await` it instead."
     )
 
 
@@ -166,11 +166,6 @@ def __enter__(self) -> "RobotClient":
     def __exit__(self, exc_type, exc, tb) -> None:
         self.close()
 
-    @property
-    def async_client(self) -> AsyncRobotClient:
-        """Access the underlying async client if you need it."""
-        return self._inner
-
     # Expose common configuration attributes
     @property
     def host(self) -> str:

parol6/commands/gripper_commands.py

@@ -87,13 +87,28 @@ class ElectricGripperCommand(MotionCommand[ElectricGripperParams]):
 
     PARAMS_TYPE = None  # Not wire-registered — instantiated by ToolActionCommand
 
+    # Stall detection: track the smallest distance-to-target seen so far and
+    # stall when no forward progress is made for ``_STALL_TICKS`` consecutive
+    # ticks past the grace period. "Forward progress" means closing the gap
+    # to target by at least ``_STALL_DEAD_BAND`` units below the best seen.
+    # This is robust to PID hunting around the stall point — oscillation
+    # never beats the best, so the counter keeps draining. Backstop for
+    # cases where the firmware's object-detection bit is too flaky for the
+    # debouncer to latch.
+    _STALL_TICKS: int = 20  # 200 ms at 100 Hz
+    _STALL_DEAD_BAND: int = 2  # progress threshold, 2/255 ≈ 0.8% of full range
+    _GRACE_TICKS: int = 10  # 100 ms before stall checks begin
+
     __slots__ = (
         "state",
         "timeout_counter",
         "object_debouncer",
         "wait_counter",
         "_hw_position",
         "_hw_speed",
+        "_stall_best_distance",
+        "_stall_remaining",
+        "_grace_remaining",
     )
 
     def __init__(self, p: ElectricGripperParams):
@@ -104,6 +119,9 @@ def __init__(self, p: ElectricGripperParams):
         self.wait_counter = 0
         self._hw_position = 0
         self._hw_speed = 1
+        self._stall_best_distance = 256  # larger than any possible distance
+        self._stall_remaining = self._STALL_TICKS
+        self._grace_remaining = self._GRACE_TICKS
 
     @classmethod
     def from_tool_action(
@@ -183,6 +201,22 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode:
                     self.finish()
                     return ExecutionStatusCode.COMPLETED
 
+            distance = abs(current_position - self._hw_position)
+            if self._grace_remaining > 0:
+                self._grace_remaining -= 1
+                self._stall_best_distance = distance
+            elif distance + self._STALL_DEAD_BAND <= self._stall_best_distance:
+                self._stall_best_distance = distance
+                self._stall_remaining = self._STALL_TICKS
+            else:
+                self._stall_remaining -= 1
+                if self._stall_remaining <= 0:
+                    # Leave move_active set so the firmware keeps applying
+                    # motor force on the grasped object — clearing it would
+                    # release the grip and the item would slip out.
+                    self.finish()
+                    return ExecutionStatusCode.COMPLETED
+
             return ExecutionStatusCode.EXECUTING
 
         self.fail(make_error(ErrorCode.MOTN_GRIPPER_UNKNOWN))

parol6/commands/system_commands.py

@@ -11,7 +11,7 @@
 import os
 from typing import TYPE_CHECKING
 
-from parol6.commands.base import ExecutionStatusCode, SystemCommand
+from parol6.commands.base import ExecutionStatusCode, MotionCommand, SystemCommand
 from parol6.config import save_com_port
 from parol6.protocol.wire import (
     CmdType,
@@ -180,8 +180,14 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode:
 
 
 @register_command(CmdType.SET_TCP_OFFSET)
-class SetTcpOffsetCommand(SystemCommand[SetTcpOffsetCmd]):
-    """Set the TCP offset in the tool's local frame."""
+class SetTcpOffsetCommand(MotionCommand[SetTcpOffsetCmd]):
+    """Set the TCP offset in the tool's local frame.
+
+    Routed through the planner (like SELECT_TOOL) so the planner subprocess
+    updates its own robot model — otherwise subsequent trajectory IK would
+    compute against the old TCP and rotations would pivot around the flange
+    instead of the offset point.
+    """
 
     PARAMS_TYPE = SetTcpOffsetCmd
 

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))

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,6 +100,71 @@ def from_string(cls, name: str) -> ProfileType:
             return cls.TOPPRA
 
 
+# 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
+
+# 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 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 = positions.shape[0]
+    dims = positions.shape[1]
+    if n < 3:
+        return 0
+
+    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)
+
+    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 diffs[i - 1] <= threshold and diffs[i] <= threshold:
+            i += 1
+            continue
+        j = i
+        while j < n - 1 and (diffs[j - 1] > threshold or diffs[j] > threshold):
+            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
+
+
 @dataclass
 class JointPath:
     """
@@ -190,7 +256,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

parol6/server/controller.py

@@ -10,7 +10,7 @@
 import sys
 import threading
 import time
-from dataclasses import dataclass
+from dataclasses import dataclass, replace
 from typing import Any
 
 
@@ -407,9 +407,10 @@ def _tick_tool_cmd(self, state: ControllerState) -> None:
                 type(self._tool_cmd).__name__,
                 self._tool_cmd.robot_error,
             )
-            state.error = self._tool_cmd.robot_error or make_error(
+            raw_error = self._tool_cmd.robot_error or make_error(
                 ErrorCode.MOTN_TICK_FAILED, detail=type(self._tool_cmd).__name__
             )
+            state.error = replace(raw_error, command_index=self._tool_cmd_index)
             state.action_state = ActionState.ERROR
             state.completed_command_index = max(
                 state.completed_command_index, self._tool_cmd_index
@@ -696,6 +697,11 @@ def _handle_motion_command(
         # Tool actions bypass planner — execute directly via side channel
         # (writes to gripper_hw, not Position_out, so concurrent with everything)
         if isinstance(command.p, ToolActionCmd):
+            # Clear error state from previous failure (same as non-streaming path)
+            if state.error is not None:
+                state.error = None
+                state.action_state = ActionState.IDLE
+
             cmd_obj, _, error_msg = create_command_from_struct(command.p)
             if cmd_obj is None:
                 logger.error("Failed to create tool command: %s", error_msg)

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

parol6/utils/ik.py

@@ -86,7 +86,7 @@ def _ensure_cache(robot: Robot) -> None:
         damping=_Damping.Sugihara,
         tol=1e-12,
         lm_lambda=0.0,
-        max_iter=10,
+        max_iter=20,
         max_restarts=10,
     )
     _cached_buffered_min = qlim[0, :] + IK_SAFETY_MARGINS_RAD[:, 0]