diff --git a/parol6/motion/geometry.py b/parol6/motion/geometry.py index e36cb0b..b581a79 100644 --- a/parol6/motion/geometry.py +++ b/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) diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 231d7e0..204f90f 100644 --- a/parol6/protocol/wire.py +++ b/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)}") diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 3eb9ef5..080e2a2 100644 --- a/parol6/server/segment_player.py +++ b/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 diff --git a/parol6/server/transports/serial_transport.py b/parol6/server/transports/serial_transport.py index ace4b03..a26e822 100644 --- a/parol6/server/transports/serial_transport.py +++ b/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: diff --git a/pyproject.toml b/pyproject.toml index 4a08877..78015cc 100644 --- a/pyproject.toml +++ b/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", ]