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/parol6/tools.py b/parol6/tools.py index c24f9fc..28f1521 100644 --- a/parol6/tools.py +++ b/parol6/tools.py @@ -651,22 +651,68 @@ 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( @@ -674,16 +720,19 @@ def _make_tcp_transform( 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( @@ -691,7 +740,7 @@ def _make_tcp_transform( 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( @@ -699,7 +748,7 @@ def _make_tcp_transform( 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, ), ), 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", ] diff --git a/tests/unit/test_collision_integration.py b/tests/unit/test_collision_integration.py index 59269cd..37e41bc 100644 --- a/tests/unit/test_collision_integration.py +++ b/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")