Merge pull request #29 from PCrnjak/fix/settle-progress

Gate trajectory settling on progress, not elapsed ticks

Author: Jepson2k

parol6/motion/geometry.py

@@ -192,9 +192,10 @@ def generate_spline(
 
         pos_splines = []
         for i in range(3):
-            bc: Any
+            # Annotated assignment keeps bc as Any: scipy-stubs' bc_type rejects
+            # the scalar derivative values scipy requires for 1-D y
             if velocity_start is not None and velocity_end is not None:
-                bc = ((1, float(velocity_start[i])), (1, float(velocity_end[i])))
+                bc: Any = ((1, float(velocity_start[i])), (1, float(velocity_end[i])))
             else:
                 bc = "not-a-knot"
             spline = CubicSpline(timestamps_arr, waypoints_arr[:, i], bc_type=bc)

parol6/protocol/wire.py

@@ -43,7 +43,7 @@
 def _enc_hook(obj: object) -> object:
     """Custom encoder hook for numpy types."""
     if isinstance(obj, np.ndarray):
-        return obj.tolist()  # type: ignore[no-matching-overload, ty:no-matching-overload]
+        return obj.tolist()  # type: ignore[no-matching-overload]
     if isinstance(obj, (np.integer, np.floating)):
         return obj.item()
     raise NotImplementedError(f"Cannot encode {type(obj)}")

parol6/server/segment_player.py

@@ -17,7 +17,6 @@
 from typing import TYPE_CHECKING
 
 import numpy as np
-from pinokin import arrays_equal_n
 
 from parol6.commands._collision_guard import guard_joint_path
 from parol6.commands.base import CommandBase, ExecutionStatusCode
@@ -60,6 +59,7 @@ class SegmentPlayer:
         "_inline_activated",
         "_settling",
         "_settle_ticks",
+        "_settle_err",
         "_last_shapes_version",
     )
 
@@ -72,6 +72,7 @@ def __init__(self, planner: MotionPlanner) -> None:
         self._inline_activated: bool = False
         self._settling: bool = False
         self._settle_ticks: int = 0
+        self._settle_err: int = -1
         self._last_shapes_version: int = 0
 
     @property
@@ -127,16 +128,36 @@ def tick(self, state: ControllerState) -> bool:
                     self._step += 1
                     self._settling = False
                     return True
-                # All waypoints sent — hold MOVE at target until Position_in converges
+                # All waypoints sent — hold MOVE at target until Position_in
+                # converges. The tick cap gates on stall, not elapsed time:
+                # while the firmware is still closing on the target (e.g. it
+                # fell behind the waypoint stream under CPU starvation) the
+                # segment stays active, so completion is never reported with
+                # the robot still in motion.
                 target = active.trajectory_steps[-1]
                 if not self._settling:
                     self._settling = True
                     self._settle_ticks = 0
+                    self._settle_err = -1
+                err = 0
+                for i in range(6):
+                    d = int(state.Position_in[i]) - int(target[i])
+                    if d < 0:
+                        d = -d
+                    if d > err:
+                        err = d
+                if self._settle_err < 0 or err < self._settle_err:
+                    self._settle_err = err
+                    self._settle_ticks = 0
                 self._settle_ticks += 1
-                if (
-                    arrays_equal_n(state.Position_in[:6], target[:6])
-                    or self._settle_ticks > SETTLE_MAX_TICKS
-                ):
+                if err == 0 or self._settle_ticks > SETTLE_MAX_TICKS:
+                    if err != 0:
+                        logger.warning(
+                            "Segment completed %d steps short of target "
+                            "(no settle progress for %d ticks)",
+                            err,
+                            SETTLE_MAX_TICKS,
+                        )
                     self._settling = False
                     self._complete_segment(active, state)
                     continue

parol6/server/transports/serial_transport.py

@@ -8,7 +8,6 @@
 import logging
 import os
 import time
-from typing import cast
 
 import numba
 import numpy as np
@@ -415,9 +414,7 @@ def get_latest_frame_view(self) -> tuple[memoryview | None, int, float]:
         Return a tuple of (memoryview|None, version:int, timestamp:float).
         The memoryview points to a stable 52-byte buffer which is updated by the reader.
         """
-        mv = cast(
-            "memoryview | None", self._frame_mv if self._frame_version > 0 else None
-        )
+        mv = self._frame_mv if self._frame_version > 0 else None
         return (mv, self._frame_version, self._frame_ts)
 
     def _update_hz_tracking(self) -> None:

pyproject.toml

@@ -63,7 +63,8 @@ dev = [
     "trimesh",
     "fast-simplification",
     "rtree",
-    "scipy-stubs",
+    "scipy-stubs==1.17.1.5; python_version < '3.12'",
+    "scipy-stubs==1.18.0.1; python_version >= '3.12'",
     "types-pyserial",
 ]