Declare the MSG gripper's built-in camera mount

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:

parol6/tools.py

@@ -651,55 +651,104 @@ def _make_tcp_transform(
     ),
 )
 
+# The MSG STLs were exported 6.5 mm proud of their mounting face: every body
+# mesh ends at z = +6.500 in the flange frame, where the SSG-48 body ends at
+# exactly 0.000. Left uncorrected the gripper is seated 6.5 mm into the wrist
+# — it interpenetrates L5, which no pair check catches because tool geometry
+# is attached to L6 and the neighbouring pairs are dropped as adjacent — and
+# what clearance remains to L4 is an artifact of that. Shifting the whole
+# assembly back puts the mounting face on the flange plane, matching every
+# other tool.
+_MSG_MOUNT_ORIGIN = (0.0, 0.0, -0.0065)
+
 _MSG_100_MESHES = (
-    MeshSpec(file="msg_ai_100_body_simplified.stl", role=MeshRole.BODY),
-    MeshSpec(file="msg_ai_100_right_jaw_simplified.stl", role=MeshRole.JAW),
-    MeshSpec(file="msg_ai_100_left_jaw_simplified.stl", role=MeshRole.JAW),
+    MeshSpec(
+        file="msg_ai_100_body_simplified.stl",
+        role=MeshRole.BODY,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
+    MeshSpec(
+        file="msg_ai_100_right_jaw_simplified.stl",
+        role=MeshRole.JAW,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
+    MeshSpec(
+        file="msg_ai_100_left_jaw_simplified.stl",
+        role=MeshRole.JAW,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
 )
 
 _MSG_150_MESHES = (
-    MeshSpec(file="msg_ai_150_body_simplified.stl", role=MeshRole.BODY),
-    MeshSpec(file="msg_ai_150_right_jaw_simplified.stl", role=MeshRole.JAW),
-    MeshSpec(file="msg_ai_150_left_jaw_simplified.stl", role=MeshRole.JAW),
+    MeshSpec(
+        file="msg_ai_150_body_simplified.stl",
+        role=MeshRole.BODY,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
+    MeshSpec(
+        file="msg_ai_150_right_jaw_simplified.stl",
+        role=MeshRole.JAW,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
+    MeshSpec(
+        file="msg_ai_150_left_jaw_simplified.stl",
+        role=MeshRole.JAW,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
 )
 
 _MSG_200_MESHES = (
-    MeshSpec(file="msg_ai_200_body_simplified.stl", role=MeshRole.BODY),
-    MeshSpec(file="msg_ai_200_right_jaw_simplified.stl", role=MeshRole.JAW),
-    MeshSpec(file="msg_ai_200_left_jaw_simplified.stl", role=MeshRole.JAW),
+    MeshSpec(
+        file="msg_ai_200_body_simplified.stl",
+        role=MeshRole.BODY,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
+    MeshSpec(
+        file="msg_ai_200_right_jaw_simplified.stl",
+        role=MeshRole.JAW,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
+    MeshSpec(
+        file="msg_ai_200_left_jaw_simplified.stl",
+        role=MeshRole.JAW,
+        origin=_MSG_MOUNT_ORIGIN,
+    ),
 )
 
 register_tool(
     "MSG",
     ElectricGripperConfig(
         name="MSG AI Stepper Gripper",
         description="MSG compliant AI stepper gripper (StepFOC)",
-        transform=_make_tcp_transform(x=-0.029, z=-0.103),
+        transform=_make_tcp_transform(x=-0.029, z=-0.1095),
         meshes=_MSG_100_MESHES,
         motions=_MSG_100_JAW_MOTION,
+        # The MSG carries a built-in camera mount; the video device is
+        # per-machine, supplied at runtime via the tool's camera override.
+        camera_spec=CameraSpec(),
         variants=(
             ToolVariant(
                 key="100mm",
                 display_name="100mm Rail",
                 meshes=_MSG_100_MESHES,
                 motions=_MSG_100_JAW_MOTION,
-                tcp_origin=(-0.029, 0.0, -0.103),
+                tcp_origin=(-0.029, 0.0, -0.1095),
                 tcp_rpy=_TCP_RPY,
             ),
             ToolVariant(
                 key="150mm",
                 display_name="150mm Rail",
                 meshes=_MSG_150_MESHES,
                 motions=_MSG_150_JAW_MOTION,
-                tcp_origin=(-0.029, 0.0, -0.103),
+                tcp_origin=(-0.029, 0.0, -0.1095),
                 tcp_rpy=_TCP_RPY,
             ),
             ToolVariant(
                 key="200mm",
                 display_name="200mm Rail",
                 meshes=_MSG_200_MESHES,
                 motions=_MSG_200_JAW_MOTION,
-                tcp_origin=(-0.029, 0.0, -0.103),
+                tcp_origin=(-0.029, 0.0, -0.1095),
                 tcp_rpy=_TCP_RPY,
             ),
         ),

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",
 ]
 

tests/unit/test_collision_integration.py

@@ -9,8 +9,11 @@
 
 from __future__ import annotations
 
+from pathlib import Path
+
 import numpy as np
 import pytest
+import trimesh
 
 import parol6.PAROL6_ROBOT as PAROL6_ROBOT
 import parol6.config  # noqa: F401 - imports trigger collision-checker init
@@ -485,3 +488,36 @@ def test_dry_run_script_set_shapes_applies_and_replays():
         assert PAROL6_ROBOT._active_shape_names == ["shape:bar2"]
     finally:
         PAROL6_ROBOT.apply_shapes([])
+
+
+def test_msg_mounts_on_the_flange_not_inside_the_wrist():
+    """The MSG STLs are exported 6.5 mm proud of their mounting face, so the
+    assembly needs a matching origin offset to seat on the flange. Without it
+    the gripper is sunk into L5 — a pair no check covers, tool geometry being
+    attached to L6 — and what little clearance is left to L4 falls inside the
+    buffer, so every pose reads as colliding and the arm can never plan its
+    way back to standby.
+    """
+    standby = np.radians(PAROL6_ROBOT.joint.standby_deg)
+    checker = PAROL6_ROBOT.collision
+    mesh_dir = Path(PAROL6_ROBOT._mesh_dir) / "meshes"
+    try:
+        PAROL6_ROBOT.apply_tool("MSG")
+        checker.update_placements(standby)
+
+        # Seated on the flange: no part of the body lies inside the wrist link.
+        body = trimesh.load_mesh(str(mesh_dir / "msg_ai_100_body.stl"))
+        body.apply_transform(checker.geometry_world_pose("tool:MSG:body"))
+        l5 = trimesh.load_mesh(str(mesh_dir / "L5.STL"))
+        l5.apply_transform(checker.geometry_world_pose("L5_0"))
+        assert not l5.contains(body.vertices).any(), "gripper seated inside L5"
+
+        # With the tool clear of the wrist, standby is outside the buffer and
+        # the return path the controller plans for HOME is accepted.
+        assert checker.in_collision(standby) is False
+        away = np.radians([90.0, -95.0, 187.0, 0.0, 6.0, 165.0])
+        guard_joint_path(
+            np.vstack([np.linspace(a, s, 25) for a, s in zip(away, standby)]).T
+        )
+    finally:
+        PAROL6_ROBOT.apply_tool("NONE")