From 460e4c79248c0e38fac9cf558cba759c467c5e98 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 7 Sep 2026 11:33:04 -0400 Subject: [PATCH 1/5] Apply full TCP transforms through planning and readback --- README.md | 27 +++++ parol6/PAROL6_ROBOT.py | 10 +- parol6/ack_policy.py | 4 +- parol6/client/async_client.py | 32 +++++- parol6/client/dry_run_client.py | 39 ++++--- parol6/client/sync_client.py | 14 +++ parol6/commands/query_commands.py | 27 +++++ parol6/commands/system_commands.py | 21 ++++ parol6/protocol/wire.py | 68 +++++++++++- parol6/robot.py | 17 +-- parol6/server/controller.py | 2 + parol6/server/motion_planner.py | 45 ++++++-- parol6/server/state.py | 21 +++- parol6/server/status_cache.py | 12 ++- parol6/tools.py | 17 +++ pyproject.toml | 2 +- tests/integration/test_tcp_transform.py | 137 ++++++++++++++++++++++++ tests/unit/test_messages.py | 16 ++- tests/unit/test_tcp_offset.py | 4 +- 19 files changed, 469 insertions(+), 46 deletions(-) create mode 100644 tests/integration/test_tcp_transform.py diff --git a/README.md b/README.md index 56744d1..673fbdd 100644 --- a/README.md +++ b/README.md @@ -424,3 +424,30 @@ For consistent high-rate performance: - Keep physical E‑Stop accessible at all times when connected to hardware - The controller can halt motion via `halt()` and reacts to E‑Stop inputs when on real hardware - Prefer `simulator_on()` for development without hardware and validate motions before switching to real serial + +## TCP transforms + +Use `set_tcp_transform(x, y, z, roll, pitch, yaw)` for a full user TCP correction, +in millimetres and intrinsic XYZ degrees (`Rx · Ry · Rz`) relative to the +registered tool. Async and sync clients return a queued command index; wait for +that index before treating the correction as applied or querying it. + +```python +with RobotClient() as rbt: + index = rbt.set_tcp_transform(0, 0, 25, 0, 90, 0) + if not rbt.wait_command(index): + raise RuntimeError("TCP application was not confirmed") + applied = rbt.tcp_transform() +``` + +Live FK, Cartesian planning, TRF motion and dry-run preview use the same +transform. Pending blend paths are completed with their original TCP before a +configuration change. Cancelling a queued change preserves the applied value. +A different tool or variant clears the correction; reselecting the same tool +and variant preserves it. Physical collision meshes stay on their registered +links, independent of the user-defined tip and axes. + +The existing `set_tcp_offset(x, y, z)` clears user rotation and now returns its +queued index for confirmation. `tcp_offset()` still reads three translations; +`tcp_transform()` reads all six values. Both raise `TimeoutError` when no valid +reply arrives instead of reporting a misleading zero correction. diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py index f22bf2b..d7f1185 100644 --- a/parol6/PAROL6_ROBOT.py +++ b/parol6/PAROL6_ROBOT.py @@ -12,7 +12,7 @@ from numpy.typing import NDArray from pinokin import CollisionChecker, Robot -from parol6.tools import get_tool_transform +from parol6.tools import compose_tcp_transform, get_tool_transform logger = logging.getLogger(__name__) @@ -221,6 +221,7 @@ def apply_tool( tool_name: str, variant_key: str = "", tcp_offset_m: tuple[float, float, float] | None = None, + tcp_rotation_rad: tuple[float, float, float] | None = None, ) -> None: """Apply tool transform to the robot model. @@ -229,12 +230,7 @@ def apply_tool( """ T_tool = get_tool_transform(tool_name, variant_key=variant_key or None) - if tcp_offset_m is not None and any(v != 0 for v in tcp_offset_m): - T_offset = np.eye(4, dtype=np.float64) - T_offset[0, 3] = tcp_offset_m[0] - T_offset[1, 3] = tcp_offset_m[1] - T_offset[2, 3] = tcp_offset_m[2] - T_tool = T_tool @ T_offset + T_tool = compose_tcp_transform(T_tool, tcp_offset_m, tcp_rotation_rad) label = f"'{tool_name}:{variant_key}'" if variant_key else f"'{tool_name}'" if not np.allclose(T_tool, np.eye(4)): diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py index 3c8714c..efca3f3 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -12,7 +12,6 @@ CmdType.SELECT_PROFILE, CmdType.RESET_STATE, CmdType.WRITE_IO, - CmdType.SET_TCP_OFFSET, CmdType.SET_SHAPES, CmdType.SET_STATUS_RATE, } @@ -36,6 +35,7 @@ CmdType.PING, CmdType.IS_SIMULATOR, CmdType.TCP_OFFSET, + CmdType.TCP_TRANSFORM, CmdType.SHAPES, CmdType.STATUS_RATE, } @@ -53,6 +53,8 @@ # Queued motion commands that return a command index in their ACK QUEUED_CMD_TYPES: set[CmdType] = { + CmdType.SET_TCP_OFFSET, + CmdType.SET_TCP_TRANSFORM, CmdType.HOME, CmdType.MOVEJ, CmdType.MOVEJ_POSE, diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 378fc3c..4d6f723 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -84,6 +84,7 @@ ShapesCmd, ShapesResultStruct, SetTcpOffsetCmd, + SetTcpTransformCmd, ShapeWire, ServoJCmd, ServoJPoseCmd, @@ -94,6 +95,8 @@ StatusCmd, TcpOffsetCmd, TcpOffsetResultStruct, + TcpTransformCmd, + TcpTransformResultStruct, TcpSpeedCmd, TeleportCmd, SpeedsResultStruct, @@ -993,6 +996,8 @@ async def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int: The offset shifts the effective TCP point in the tool's local frame. Subsequent motion (especially TRF relative moves) will use the new TCP. + Returns the queued index; await ``wait_command(index)`` to confirm + application. This translation-only setter clears user TCP rotation. Call with (0, 0, 0) to reset. Changing tools resets the offset. Category: Configuration @@ -1002,6 +1007,30 @@ async def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int: """ return await self._send(SetTcpOffsetCmd(x=x, y=y, z=z)) + async def set_tcp_transform( + self, + x: float = 0, + y: float = 0, + z: float = 0, + roll: float = 0, + pitch: float = 0, + yaw: float = 0, + ) -> int: + return await self._send( + SetTcpTransformCmd(x=x, y=y, z=z, roll=roll, pitch=pitch, yaw=yaw) + ) + + async def tcp_transform(self) -> list[float]: + from math import isfinite + + resp = await self._request(TcpTransformCmd()) + if not isinstance(resp, TcpTransformResultStruct): + raise TimeoutError("Controller did not return a TCP transform") + values = [resp.x, resp.y, resp.z, resp.roll, resp.pitch, resp.yaw] + if not all(isfinite(value) for value in values): + raise ValueError("Controller returned a non-finite TCP transform") + return values + async def set_shapes(self, shapes: list[Shape]) -> int: """Replace the program-layer collision-world shapes (keep-out barriers). @@ -1063,7 +1092,7 @@ async def tcp_offset(self) -> list[float]: resp = await self._request(TcpOffsetCmd()) if isinstance(resp, TcpOffsetResultStruct): return [resp.x, resp.y, resp.z] - return [0.0, 0.0, 0.0] + raise TimeoutError("Controller did not return a TCP offset") async def select_profile(self, profile: str) -> int: """Set the motion profile (e.g. ``"TOPPRA"``). @@ -1152,6 +1181,7 @@ async def _tool_status(self) -> ToolStatus | None: fault_code=resp.fault_code, positions=tuple(resp.positions), channels=tuple(resp.channels), + variant_key=resp.variant_key, ) async def reachable(self) -> EnablementResultStruct | None: diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index c965a4b..7890425 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -42,6 +42,7 @@ HomeCmd, SelectToolCmd, SetTcpOffsetCmd, + SetTcpTransformCmd, TeleportCmd, ToolActionCmd, ) @@ -218,7 +219,6 @@ def __init__( self._max_snapshot_points = max_snapshot_points self._active_tool_key: str = "" self._active_variant_key: str = "" - self._tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) self._tool_proxy = _DryRunTool(self) @property @@ -234,11 +234,16 @@ def tool(self) -> _DryRunTool: def tcp_offset(self) -> list[float]: """Return current TCP offset in mm.""" return [ - self._tcp_offset_m[0] * 1000.0, - self._tcp_offset_m[1] * 1000.0, - self._tcp_offset_m[2] * 1000.0, + self._state.tcp_offset_m[0] * 1000.0, + self._state.tcp_offset_m[1] * 1000.0, + self._state.tcp_offset_m[2] * 1000.0, ] + def tcp_transform(self) -> list[float]: + from math import degrees + + return self.tcp_offset() + [degrees(v) for v in self._state.tcp_rotation_rad] + def flush(self) -> list[DryRunResult]: """Flush pending blend buffer. Call after script completion.""" segments = self._planner.flush() @@ -280,21 +285,24 @@ def _dispatch(self, params: Any) -> DryRunResult | None: # into a planned return move, so the preview renders the path. if isinstance(params, TeleportCmd): return self._snap_to_angles(params.angles) + results: list[DryRunResult] = [] + if isinstance(params, (SelectToolCmd, SetTcpOffsetCmd, SetTcpTransformCmd)): + # Resolve pending paths against their original TCP before changing it. + results.extend(self.flush()) if isinstance(params, SelectToolCmd): self._active_tool_key = params.tool_name.strip().upper() self._active_variant_key = params.variant_key - self._tcp_offset_m = (0.0, 0.0, 0.0) - if isinstance(params, SetTcpOffsetCmd): - self._tcp_offset_m = ( - params.x / 1000.0, - params.y / 1000.0, - params.z / 1000.0, + self._state.set_tool(self._active_tool_key, params.variant_key) + if isinstance(params, (SetTcpOffsetCmd, SetTcpTransformCmd)): + from math import radians + + rotation = ( + (radians(params.roll), radians(params.pitch), radians(params.yaw)) + if isinstance(params, SetTcpTransformCmd) + else (0.0, 0.0, 0.0) ) - self._state._tcp_offset_m = self._tcp_offset_m - PAROL6_ROBOT.apply_tool( - self._active_tool_key or "NONE", - variant_key=self._active_variant_key, - tcp_offset_m=self._tcp_offset_m, + self._state.set_tcp_transform( + (params.x / 1000.0, params.y / 1000.0, params.z / 1000.0), rotation ) # Detect jog/servo commands — planner doesn't handle streaming. # Other non-trajectory MotionCommands (SelectTool, Home) fall through @@ -313,7 +321,6 @@ def _dispatch(self, params: Any) -> DryRunResult | None: segments = self._planner.process(params) self._state.Position_in[:] = self._planner.state.Position_in - results: list[DryRunResult] = [] for seg in segments: r = self._segment_to_result(seg) if r is not None: diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 3c72873..1cc1054 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -366,6 +366,20 @@ def set_tcp_offset(self, x: float = 0, y: float = 0, z: float = 0) -> int: """Set TCP offset in mm, composed on top of the current tool transform.""" return _run(self._inner.set_tcp_offset(x=x, y=y, z=z)) + def set_tcp_transform( + self, + x: float = 0, + y: float = 0, + z: float = 0, + roll: float = 0, + pitch: float = 0, + yaw: float = 0, + ) -> int: + return _run(self._inner.set_tcp_transform(x, y, z, roll, pitch, yaw)) + + def tcp_transform(self) -> list[float]: + return _run(self._inner.tcp_transform()) + def set_shapes(self, shapes: list) -> int: """Replace the program-layer collision-world shapes (keep-out barriers). diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index 0f092a2..7fd8f49 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -42,6 +42,8 @@ SpeedsResultStruct, TcpOffsetCmd, TcpOffsetResultStruct, + TcpTransformCmd, + TcpTransformResultStruct, StatusCmd, StatusResultStruct, TcpSpeedCmd, @@ -149,6 +151,7 @@ def compute(self, state: "ControllerState") -> bytes: ts.fault_code, list(ts.positions), list(ts.channels), + ts.variant_key, ], ) ) @@ -254,6 +257,7 @@ def compute(self, state: "ControllerState") -> bytes: fault_code=ts.fault_code, positions=list(ts.positions), channels=list(ts.channels), + variant_key=ts.variant_key, ) ) @@ -426,3 +430,26 @@ def compute(self, state: "ControllerState") -> bytes: z=offset[2] * 1000, ) ) + + +@register_command(CmdType.TCP_TRANSFORM) +class TcpTransformCommand(QueryCommand[TcpTransformCmd]): + PARAMS_TYPE = TcpTransformCmd + QUERY_TYPE = QueryType.TCP_TRANSFORM + __slots__ = () + + def compute(self, state: "ControllerState") -> bytes: + from math import degrees + + xyz = state.tcp_offset_m + rpy = state.tcp_rotation_rad + return pack_response( + TcpTransformResultStruct( + x=xyz[0] * 1000, + y=xyz[1] * 1000, + z=xyz[2] * 1000, + roll=degrees(rpy[0]), + pitch=degrees(rpy[1]), + yaw=degrees(rpy[2]), + ) + ) diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index 0706d3b..bd409ed 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -20,6 +20,7 @@ ResetCmd, SelectProfileCmd, SetTcpOffsetCmd, + SetTcpTransformCmd, SimulatorCmd, StopCmd, WriteIOCmd, @@ -220,3 +221,23 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: self.finish() return ExecutionStatusCode.COMPLETED + + +@register_command(CmdType.SET_TCP_TRANSFORM) +class SetTcpTransformCommand(MotionCommand[SetTcpTransformCmd]): + """Apply a user TCP transform at its position in the motion queue.""" + + PARAMS_TYPE = SetTcpTransformCmd + __slots__ = () + + def do_setup(self, state: ControllerState) -> None: + from math import radians + + state.set_tcp_transform( + (self.p.x / 1000, self.p.y / 1000, self.p.z / 1000), + (radians(self.p.roll), radians(self.p.pitch), radians(self.p.yaw)), + ) + + def execute_step(self, state: ControllerState) -> ExecutionStatusCode: + self.finish() + return ExecutionStatusCode.COMPLETED diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 57082a7..a6566c3 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -15,6 +15,7 @@ """ import logging +import math from dataclasses import dataclass, field from collections.abc import Sequence from enum import IntEnum, auto @@ -93,6 +94,7 @@ class QueryType(IntEnum): TCP_OFFSET = auto() SHAPES = auto() STATUS_RATE = auto() + TCP_TRANSFORM = auto() class CmdType(IntEnum): @@ -164,6 +166,8 @@ class CmdType(IntEnum): # control rate it divides. SET_STATUS_RATE = auto() STATUS_RATE = auto() + SET_TCP_TRANSFORM = auto() + TCP_TRANSFORM = auto() # ============================================================================= @@ -642,6 +646,30 @@ class SetTcpOffsetCmd( z: float = 0.0 +class SetTcpTransformCmd( + msgspec.Struct, + tag=int(CmdType.SET_TCP_TRANSFORM), + array_like=True, + frozen=True, + gc=False, +): + """User TCP transform: mm and intrinsic XYZ degrees.""" + + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + roll: float = 0.0 + pitch: float = 0.0 + yaw: float = 0.0 + + def __post_init__(self) -> None: + if any( + isinstance(v, bool) or not math.isfinite(v) + for v in (self.x, self.y, self.z, self.roll, self.pitch, self.yaw) + ): + raise ValueError("TCP transform requires six finite numbers") + + class ShapeWire(msgspec.Struct, array_like=True, frozen=True, gc=False): """One workspace shape — mirrors waldoctl ``Shape.to_wire()``. @@ -780,6 +808,16 @@ class TcpOffsetCmd( pass +class TcpTransformCmd( + msgspec.Struct, + tag=int(CmdType.TCP_TRANSFORM), + array_like=True, + frozen=True, + gc=False, +): + """Read the applied user TCP transform.""" + + class ShapesCmd( msgspec.Struct, tag=int(CmdType.SHAPES), @@ -1160,7 +1198,7 @@ class ToolStatusResultStruct( frozen=True, gc=False, ): - """Tool status response — full 7-field ToolStatus.""" + """Tool status response, including the applied variant.""" tool_key: str state: ToolState @@ -1169,6 +1207,7 @@ class ToolStatusResultStruct( fault_code: int positions: list[float] channels: list[float] + variant_key: str = "" class EnablementResultStruct( @@ -1235,6 +1274,23 @@ class TcpOffsetResultStruct( z: float +class TcpTransformResultStruct( + msgspec.Struct, + tag=int(QueryType.TCP_TRANSFORM), + array_like=True, + frozen=True, + gc=False, +): + """Applied user TCP transform in mm and intrinsic XYZ degrees.""" + + x: float + y: float + z: float + roll: float + pitch: float + yaw: float + + class ShapesResultStruct( msgspec.Struct, tag=int(QueryType.SHAPES), @@ -1269,6 +1325,7 @@ class ShapesResultStruct( | TcpSpeedResultStruct | IsSimulatorResultStruct | TcpOffsetResultStruct + | TcpTransformResultStruct | ShapesResultStruct ) @@ -1449,6 +1506,7 @@ def pack_status( ts.fault_code, ts.positions, ts.channels, + ts.variant_key, ) if ts is not None else None, @@ -1575,6 +1633,7 @@ def copy(self) -> "StatusBuffer": fault_code=ts.fault_code, positions=ts.positions, channels=ts.channels, + variant_key=ts.variant_key, ), joint_en=self.joint_en.copy(), cart_en_wrf=self.cart_en_wrf.copy(), @@ -1692,6 +1751,10 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: and isinstance(raw_ts, (list, tuple)) and len(raw_ts) >= 7 ): + variant = raw_ts[7] if len(raw_ts) > 7 else "" + if not isinstance(variant, str) or len(variant) > 128: + return False + ts.variant_key = variant ts.key = raw_ts[0] ts.state = ToolState(raw_ts[1]) ts.engaged = raw_ts[2] @@ -1993,6 +2056,7 @@ def unpack_rx_frame_into( "TeleportCmd", "SelectToolCmd", "SetTcpOffsetCmd", + "SetTcpTransformCmd", "SelectProfileCmd", "ToolActionCmd", # Command structs — query @@ -2002,6 +2066,7 @@ def unpack_rx_frame_into( "ErrorCmd", "TcpSpeedCmd", "TcpOffsetCmd", + "TcpTransformCmd", "ShapesCmd", "PingCmd", "StatusCmd", @@ -2038,6 +2103,7 @@ def unpack_rx_frame_into( "TcpSpeedResultStruct", "IsSimulatorResultStruct", "TcpOffsetResultStruct", + "TcpTransformResultStruct", "ShapesResultStruct", "Response", # Message types diff --git a/parol6/robot.py b/parol6/robot.py index a1bac31..7b0a2ff 100644 --- a/parol6/robot.py +++ b/parol6/robot.py @@ -654,6 +654,10 @@ def async_client_class(self) -> type: # -- Kinematics --------------------------------------------------------- + @property + def has_tcp_transform(self) -> bool: + return True + def _load_q_buf(self, q_rad: NDArray[np.float64]) -> None: """Copy joint radians into the padded pinokin q buffer.""" n = min(len(q_rad), self._pinokin.nq) @@ -665,6 +669,8 @@ def set_active_tool( tool_key: str, tcp_offset_m: tuple[float, float, float] | None = None, variant_key: str | None = None, + *, + tcp_rotation_rad: tuple[float, float, float] | None = None, ) -> None: """Apply tool transform to the local FK/IK model. @@ -678,7 +684,7 @@ def set_active_tool( checker so client-side collision queries (preview / editing pose) see the attached tool. """ - from parol6.tools import get_tool_transform + from parol6.tools import compose_tcp_transform, get_tool_transform try: T_tool = get_tool_transform(tool_key, variant_key=variant_key) @@ -687,14 +693,9 @@ def set_active_tool( # TCP from the ToolSpec instead. T_tool = self._plugin_tool_transform(tool_key, variant_key) - if tcp_offset_m is not None and any(v != 0 for v in tcp_offset_m): - T_offset = np.eye(4) - T_offset[0, 3] = tcp_offset_m[0] - T_offset[1, 3] = tcp_offset_m[1] - T_offset[2, 3] = tcp_offset_m[2] - T_tool = T_tool @ T_offset + T_tool = compose_tcp_transform(T_tool, tcp_offset_m, tcp_rotation_rad) - if tool_key != "NONE" and not np.allclose(T_tool, np.eye(4)): + if not np.allclose(T_tool, np.eye(4)): self._pinokin.set_tool_transform(T_tool) else: self._pinokin.clear_tool_transform() diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 61ba9ca..2dda415 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -349,6 +349,7 @@ def _handle_estop(self, state: ControllerState) -> None: state.current_tool, variant_key=state.current_tool_variant, tcp_offset_m=state.tcp_offset_m, + tcp_rotation_rad=state.tcp_rotation_rad, ) self._planner.sync_shapes(state.shapes) if self._executor.active_command: @@ -853,6 +854,7 @@ def _handle_system_command( state.current_tool, variant_key=state.current_tool_variant, tcp_offset_m=state.tcp_offset_m, + tcp_rotation_rad=state.tcp_rotation_rad, ) self._planner.sync_shapes(state.shapes) diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index b502a85..4d1e2ba 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -29,6 +29,7 @@ SelectToolCmd, SetShapesCmd, SetTcpOffsetCmd, + SetTcpTransformCmd, ToolActionCmd, ) from parol6.server.command_executor import _format_cmd_params @@ -119,6 +120,7 @@ class SyncTool: tool_name: str variant_key: str = "" tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + tcp_rotation_rad: tuple[float, float, float] = (0.0, 0.0, 0.0) @dataclass @@ -162,6 +164,7 @@ class PlannerState: current_tool: str = "NONE" current_tool_variant: str = "" tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + tcp_rotation_rad: tuple[float, float, float] = (0.0, 0.0, 0.0) stop_on_failure: bool = True # Forward kinematics cache (same layout as ControllerState — needed by @@ -172,6 +175,7 @@ class PlannerState: _fkine_last_tool_name: str = "" _fkine_last_tool_variant: str = "" _fkine_last_tcp_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) + _fkine_last_tcp_rotation: tuple[float, float, float] = (0.0, 0.0, 0.0) _fkine_mat: np.ndarray = field( default_factory=lambda: np.asfortranarray(np.eye(4, dtype=np.float64)) ) @@ -271,13 +275,18 @@ def sync_tool( tool_name: str, variant_key: str = "", tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0), + tcp_rotation_rad: tuple[float, float, float] = (0.0, 0.0, 0.0), ) -> None: """Sync tool state (e.g. after E-stop cancel).""" self.state.current_tool = tool_name self.state.current_tool_variant = variant_key self.state.tcp_offset_m = tcp_offset_m + self.state.tcp_rotation_rad = tcp_rotation_rad self._robot_module.apply_tool( - tool_name, variant_key=variant_key, tcp_offset_m=tcp_offset_m + tool_name, + variant_key=variant_key, + tcp_offset_m=tcp_offset_m, + tcp_rotation_rad=tcp_rotation_rad, ) def sync_shapes(self, shapes: list) -> None: @@ -490,20 +499,37 @@ def _handle_inline(self, command_index: int, params: object) -> None: # Predict state for subsequent trajectory planning if isinstance(params, SelectToolCmd): + if (params.tool_name, params.variant_key) != ( + self.state.current_tool, + self.state.current_tool_variant, + ): + self.state.tcp_offset_m = (0.0, 0.0, 0.0) + self.state.tcp_rotation_rad = (0.0, 0.0, 0.0) self.state.current_tool = params.tool_name self.state.current_tool_variant = params.variant_key - self.state.tcp_offset_m = (0.0, 0.0, 0.0) self._robot_module.apply_tool( - params.tool_name, variant_key=params.variant_key + params.tool_name, + variant_key=params.variant_key, + tcp_offset_m=self.state.tcp_offset_m, + tcp_rotation_rad=self.state.tcp_rotation_rad, ) - elif isinstance(params, SetTcpOffsetCmd): + elif isinstance(params, (SetTcpOffsetCmd, SetTcpTransformCmd)): + from math import radians + offset_m = (params.x / 1000.0, params.y / 1000.0, params.z / 1000.0) - self.state.tcp_offset_m = offset_m + rotation_rad = ( + (radians(params.roll), radians(params.pitch), radians(params.yaw)) + if isinstance(params, SetTcpTransformCmd) + else (0.0, 0.0, 0.0) + ) self._robot_module.apply_tool( self.state.current_tool, variant_key=self.state.current_tool_variant, tcp_offset_m=offset_m, + tcp_rotation_rad=rotation_rad, ) + self.state.tcp_offset_m = offset_m + self.state.tcp_rotation_rad = rotation_rad elif isinstance(params, HomeCmd): self.state.Position_in[:] = self._home_steps self.state.Homed_in.fill(1) @@ -562,10 +588,14 @@ def apply_tool( tool_name: str, variant_key: str = "", tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0), + tcp_rotation_rad: tuple[float, float, float] = (0.0, 0.0, 0.0), ) -> None: """Sync tool state (e.g. after E-stop).""" self._planner.sync_tool( - tool_name, variant_key=variant_key, tcp_offset_m=tcp_offset_m + tool_name, + variant_key=variant_key, + tcp_offset_m=tcp_offset_m, + tcp_rotation_rad=tcp_rotation_rad, ) def apply_shapes(self, shapes: list) -> None: @@ -653,6 +683,7 @@ def motion_planner_main( msg.tool_name, variant_key=msg.variant_key, tcp_offset_m=msg.tcp_offset_m, + tcp_rotation_rad=msg.tcp_rotation_rad, ) continue @@ -799,6 +830,7 @@ def sync_tool( tool_name: str, variant_key: str = "", tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0), + tcp_rotation_rad: tuple[float, float, float] = (0.0, 0.0, 0.0), ) -> None: """Update the planner's tool state.""" self.submit( @@ -806,6 +838,7 @@ def sync_tool( tool_name=tool_name, variant_key=variant_key, tcp_offset_m=tcp_offset_m, + tcp_rotation_rad=tcp_rotation_rad, ) ) diff --git a/parol6/server/state.py b/parol6/server/state.py index 2bc35ac..a35182e 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -181,6 +181,7 @@ class ControllerState: _current_tool: str = "NONE" _current_tool_variant: str = "" _tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + _tcp_rotation_rad: tuple[float, float, float] = (0.0, 0.0, 0.0) # Robot telemetry and command buffers - using ndarray for efficiency Command_out: CommandCode = CommandCode.IDLE # The command code to send to firmware @@ -316,6 +317,7 @@ class ControllerState: _fkine_last_tool_name: str = "" _fkine_last_tool_variant: str = "" _fkine_last_tcp_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) + _fkine_last_tcp_rotation: tuple[float, float, float] = (0.0, 0.0, 0.0) _fkine_mat: np.ndarray = field( default_factory=lambda: np.asfortranarray(np.eye(4, dtype=np.float64)) ) @@ -357,6 +359,7 @@ def reset(self) -> None: self._current_tool = "NONE" self._current_tool_variant = "" self._tcp_offset_m = (0.0, 0.0, 0.0) + self._tcp_rotation_rad = (0.0, 0.0, 0.0) PAROL6_ROBOT.apply_tool("NONE") # Command and telemetry buffers - zero out @@ -433,6 +436,7 @@ def set_tool(self, tool_name: str, variant_key: str = "") -> None: self._current_tool = tool_name self._current_tool_variant = variant_key self._tcp_offset_m = (0.0, 0.0, 0.0) + self._tcp_rotation_rad = (0.0, 0.0, 0.0) PAROL6_ROBOT.apply_tool(tool_name, variant_key=variant_key) label = f"{tool_name}:{variant_key}" if variant_key else tool_name logger.info(f"Tool changed to {label}") @@ -456,12 +460,25 @@ def tcp_offset_m(self) -> tuple[float, float, float]: def set_tcp_offset(self, offset_m: tuple[float, float, float]) -> None: """Set TCP offset and reapply tool transform with the composed offset.""" - self._tcp_offset_m = offset_m + self.set_tcp_transform(offset_m, (0.0, 0.0, 0.0)) + + @property + def tcp_rotation_rad(self) -> tuple[float, float, float]: + return self._tcp_rotation_rad + + def set_tcp_transform( + self, + offset_m: tuple[float, float, float], + rotation_rad: tuple[float, float, float], + ) -> None: PAROL6_ROBOT.apply_tool( self._current_tool, variant_key=self._current_tool_variant, tcp_offset_m=offset_m, + tcp_rotation_rad=rotation_rad, ) + self._tcp_offset_m = offset_m + self._tcp_rotation_rad = rotation_rad logger.debug( "TCP offset set to (%.1f, %.1f, %.1f) mm", offset_m[0] * 1000, @@ -570,6 +587,7 @@ def ensure_fkine_updated(state: ControllerState) -> None: state.current_tool != state._fkine_last_tool_name or state.current_tool_variant != state._fkine_last_tool_variant or state.tcp_offset_m != state._fkine_last_tcp_offset + or state.tcp_rotation_rad != state._fkine_last_tcp_rotation ) if pos_changed or tool_changed: @@ -587,6 +605,7 @@ def ensure_fkine_updated(state: ControllerState) -> None: state._fkine_last_tool_name = state.current_tool state._fkine_last_tool_variant = state.current_tool_variant state._fkine_last_tcp_offset = state.tcp_offset_m + state._fkine_last_tcp_rotation = state.tcp_rotation_rad def get_fkine_se3(state: ControllerState | None = None) -> np.ndarray: diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 4babe2a..32f5758 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -36,7 +36,7 @@ ik_enablement_worker_main, ) from parol6.server.state import ControllerState, get_fkine_flat_mm, get_fkine_se3 -from parol6.tools import get_tool_transform +from parol6.tools import compose_tcp_transform, get_tool_transform from parol6 import config as _cfg # Drive-fault labels indexed by (overtemperature | following-error << 1). @@ -158,6 +158,8 @@ def __init__(self) -> None: self.last_serial_s: float = 0.0 # last time a fresh serial frame was observed self._last_tool_name: str = "NONE" # Track tool changes self._last_tool_variant: str = "" # Track variant changes + self._last_tcp_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) + self._last_tcp_rotation: tuple[float, float, float] = (0.0, 0.0, 0.0) self._last_tool_positions: tuple[float, ...] = () # Track tool DOF changes # Action tracking fields @@ -423,6 +425,8 @@ def update_from_state(self, state: ControllerState) -> None: tool_changed = ( state.current_tool != self._last_tool_name or state.current_tool_variant != self._last_tool_variant + or state.tcp_offset_m != self._last_tcp_offset + or state.tcp_rotation_rad != self._last_tcp_rotation ) # Convert speeds from steps/s to rad/s when they change @@ -432,6 +436,8 @@ def update_from_state(self, state: ControllerState) -> None: if tool_changed: self._last_tool_name = state.current_tool self._last_tool_variant = state.current_tool_variant + self._last_tcp_offset = state.tcp_offset_m + self._last_tcp_rotation = state.tcp_rotation_rad self._tcp_pos_initialized = ( False # avoid speed spike from TCP offset change ) @@ -439,6 +445,9 @@ def update_from_state(self, state: ControllerState) -> None: T_tool = get_tool_transform( state.current_tool, variant_key=state.current_tool_variant ) + T_tool = compose_tcp_transform( + T_tool, state.tcp_offset_m, state.tcp_rotation_rad + ) self._ik_input_tool_view.reshape(4, 4)[:] = T_tool # Sync the tool's collision geometry to the IK worker's checker self._sync_ik_geometry( @@ -488,6 +497,7 @@ def update_from_state(self, state: ControllerState) -> None: # Populate tool status from hardware state via the tool config ts = self.tool_status ts.key = state.current_tool + ts.variant_key = state.current_tool_variant # Reset value fields to defaults first so a tool whose populate_status is # a no-op (NONE / passive / plugin tools) doesn't inherit the previous # tool's engaged/part_detected/positions/etc. (in-place, zero-alloc). diff --git a/parol6/tools.py b/parol6/tools.py index 28f1521..2cc4e8f 100644 --- a/parol6/tools.py +++ b/parol6/tools.py @@ -467,6 +467,23 @@ def get_tool_transform( # --------------------------------------------------------------------------- +def compose_tcp_transform( + registered: np.ndarray, + offset_m: tuple[float, float, float] | None = None, + rotation_rad: tuple[float, float, float] | None = None, +) -> np.ndarray: + """Compose an explicit user transform after the registered physical tool.""" + if offset_m is None and rotation_rad is None: + return registered + translation = offset_m if offset_m is not None else (0.0, 0.0, 0.0) + rotation = rotation_rad if rotation_rad is not None else (0.0, 0.0, 0.0) + if not all(math.isfinite(v) for v in (*translation, *rotation)): + raise ValueError("TCP transform must be finite") + user = np.empty((4, 4), dtype=np.float64) + se3_from_rpy(*translation, *rotation, user) + return registered @ user + + def _make_tcp_transform( x: float = 0.0, y: float = 0.0, diff --git a/pyproject.toml b/pyproject.toml index dcc440e..0bc318d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -46,7 +46,7 @@ dependencies = [ "psutil>=5.9", "msgspec>=0.18", "ormsgpack>=1.4.0", - "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.14.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.15.0", ] [tool.setuptools.packages.find] diff --git a/tests/integration/test_tcp_transform.py b/tests/integration/test_tcp_transform.py new file mode 100644 index 0000000..3764c64 --- /dev/null +++ b/tests/integration/test_tcp_transform.py @@ -0,0 +1,137 @@ +"""TCP calibration configuration through real client and controller paths.""" + +import socket + +import msgspec +import numpy as np +import pytest +from waldoctl.setup import Pose + +from parol6.client.async_client import AsyncRobotClient +from parol6.protocol.wire import CmdType, MsgType + + +@pytest.mark.asyncio +async def test_unanswered_tcp_readback_cannot_clear_a_saved_calibration(): + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as silent: + silent.bind(("127.0.0.1", 0)) + client = AsyncRobotClient(port=silent.getsockname()[1], timeout=0.05, retries=0) + try: + with pytest.raises(TimeoutError): + await client.tcp_offset() + with pytest.raises(TimeoutError): + await client.tcp_transform() + finally: + await client.close() + + +@pytest.mark.asyncio +async def test_full_tcp_transform_agrees_across_wire_fk_preview_and_motion(ports): + from parol6.client.dry_run_client import DryRunRobotClient + from parol6.robot import Robot + + values = (5.0, -3.0, 20.0, 20.0, 25.0, -10.0) + async with AsyncRobotClient(host=ports.server_ip, port=ports.server_port) as client: + base = await client.pose() + angles = await client.angles() + assert base is not None and angles is not None + expected = Pose(tuple(base)).matrix() @ Pose(values).matrix() + index = await client.set_tcp_transform(*values) + assert index >= 0 and await client.wait_command(index, timeout=10) + assert await client.tcp_transform() == pytest.approx(values) + assert await client.tcp_offset() == pytest.approx(values[:3]) + assert await client.wait_status( + lambda s: np.allclose( + np.asarray(s.pose).reshape(4, 4), expected, atol=0.05 + ), + timeout=5, + ), "stationary STATUS did not adopt the full TCP transform" + + local = Robot() + local.set_active_tool( + "NONE", + tcp_offset_m=tuple(v / 1000 for v in values[:3]), + tcp_rotation_rad=tuple(np.radians(values[3:])), + ) + local_pose = local.fk(np.radians(angles), np.empty(6)) + local_matrix = Pose( + tuple([*(local_pose[:3] * 1000), *np.degrees(local_pose[3:])]) + ).matrix() + assert local_matrix == pytest.approx(expected, abs=0.05) + + preview = DryRunRobotClient(initial_joints_deg=angles) + preview.set_tcp_transform(*values) + predicted = preview.move_l([0, 0, 5, 0, 0, 0], frame="TRF", rel=True, speed=0.2) + assert predicted is not None and predicted.error is None + target = expected @ Pose((0, 0, 5, 0, 0, 0)).matrix() + predicted_pose = predicted.tcp_poses[-1] + predicted_matrix = Pose( + tuple([*(predicted_pose[:3] * 1000), *np.degrees(predicted_pose[3:])]) + ).matrix() + assert predicted_matrix[:3, 3] == pytest.approx(target[:3, 3], abs=1.0) + + index = await client.move_l( + [0, 0, 5, 0, 0, 0], frame="TRF", rel=True, speed=0.2 + ) + assert index >= 0 and await client.wait_command(index, timeout=15) + actual = await client.pose() + assert actual is not None + actual_matrix = Pose(tuple(actual)).matrix() + assert actual_matrix[:3, 3] == pytest.approx(target[:3, 3], abs=1.0) + assert actual_matrix[:3, :3] == pytest.approx(target[:3, :3], abs=0.02) + + delay = await client.delay(5) + assert await client.wait_status(lambda s: s.executing_index == delay, timeout=5) + pending = await client.set_tcp_transform(0, 0, 40, 0, 90, 0) + assert pending > delay + assert await client.tcp_transform() == pytest.approx(values) + assert await client.stop() > 0 + assert await client.tcp_transform() == pytest.approx(values) + index = await client.move_l(actual, speed=0.2) + assert index >= 0 and await client.wait_command(index, timeout=15), ( + "cancelled calibration leaked into the planner" + ) + + index = await client.select_tool("NONE") + assert await client.wait_command(index, timeout=10) + assert await client.tcp_transform() == pytest.approx(values) + index = await client.set_tcp_offset(1, 2, 3) + assert await client.wait_command(index, timeout=10) + assert await client.tcp_transform() == pytest.approx([1, 2, 3, 0, 0, 0]) + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as raw: + raw.settimeout(2) + for slot in range(6): + for invalid in (float("nan"), float("inf"), float("-inf")): + payload = [float(v) for v in values] + payload[slot] = invalid + raw.sendto( + msgspec.msgpack.encode( + [int(CmdType.SET_TCP_TRANSFORM), *payload] + ), + (ports.server_ip, ports.server_port), + ) + reply = msgspec.msgpack.decode(raw.recv(65535)) + assert reply[0] == int(MsgType.ERROR) + assert await client.tcp_transform() == pytest.approx([1, 2, 3, 0, 0, 0]) + index = await client.select_tool("SSG-48") + assert await client.wait_command(index, timeout=10) + assert await client.tcp_transform() == pytest.approx([0] * 6) + + +@pytest.mark.asyncio +async def test_tcp_calibration_binding_reports_the_selected_tool_variant(ports): + async with AsyncRobotClient(host=ports.server_ip, port=ports.server_port) as client: + for variant in ("vertical", "horizontal"): + index = await client.select_tool("PNEUMATIC", variant_key=variant) + assert await client.wait_command(index, timeout=10) + observed = [] + + def capture(status): + observed.append(status.tool_status.variant_key) + return len(observed) > 1 and status.tool_status.key == "PNEUMATIC" + + assert await client.wait_status(capture, timeout=3) + assert observed[-1] == variant + tool = await client.tool.status() + assert tool is not None and tool.variant_key == variant diff --git a/tests/unit/test_messages.py b/tests/unit/test_messages.py index e8f8b2a..03c282e 100644 --- a/tests/unit/test_messages.py +++ b/tests/unit/test_messages.py @@ -115,8 +115,7 @@ def test_pack_status_roundtrip(self): # action_params at index 16 assert unpacked[16] == "speed=50 acc=100" - # tool_status at index 17 is a 7-element tuple: - # (key, state, engaged, part_detected, fault_code, positions, channels) + # The optional variant follows the original seven tool-status fields. ts = unpacked[17] assert ts[0] == "ssg48" # key assert ts[1] == 2 # state (ToolState.ACTIVE) @@ -140,6 +139,7 @@ def test_pack_decode_status_bin_roundtrip(self): cart_en_trf = np.ones(12, dtype=np.uint8) tool_status = ToolStatus( key="electric_gripper", + variant_key="pinch", state=ToolState.IDLE, engaged=False, part_detected=True, @@ -175,6 +175,18 @@ def test_pack_decode_status_bin_roundtrip(self): assert ts.positions == (0.5,) assert ts.channels == (1.2, 3.4) assert buf.tcp_speed == pytest.approx(55.5) + assert ts.variant_key == "pinch" + assert buf.copy().tool_status.variant_key == "pinch" + legacy = decode(packed) + legacy[17] = legacy[17][:7] + assert decode_status_bin_into(encode(legacy), buf) + assert buf.tool_status.variant_key == "", ( + "legacy status retained a stale variant" + ) + for invalid in (False, 42, None, "x" * 129): + bad = decode(packed) + bad[17][7] = invalid + assert not decode_status_bin_into(encode(bad), buf) def test_invalid_data_raises(self): with pytest.raises(msgspec.ValidationError): diff --git a/tests/unit/test_tcp_offset.py b/tests/unit/test_tcp_offset.py index 239f29f..c997f03 100644 --- a/tests/unit/test_tcp_offset.py +++ b/tests/unit/test_tcp_offset.py @@ -69,8 +69,10 @@ def test_dry_run_select_tool_resets_tcp_offset(): client.set_tcp_offset(x=0, y=0, z=-190) assert client.tcp_offset() == [0.0, 0.0, -190.0] - # Selecting a tool resets offset + # Re-selecting the same tool preserves the applied calibration, as live does. client.select_tool("SSG-48") + assert client.tcp_offset() == [0.0, 0.0, -190.0] + client.select_tool("NONE") assert client.tcp_offset() == [0.0, 0.0, 0.0] From 9348393b2c76abd149bdc518b81639518694c1fa Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 05:59:58 +0000 Subject: [PATCH 2/5] Resync the planner when Stop or Estop cancels the queue The planner subprocess applies SET_TCP_TRANSFORM (and tool/shape changes) when it plans a command, while that command is still queued. A user STOP or software Estop cancelled the queue but left the planner holding the transform, so every later plan was solved against a TCP the controller never applied and reported. Only the hardware E-stop input and reset resynced; all three cancellation paths now share one resync. Co-Authored-By: Claude Fable 5.1 --- parol6/server/controller.py | 33 ++++++++++++++---------- tests/integration/test_stop_semantics.py | 28 ++++++++++++++++++++ 2 files changed, 47 insertions(+), 14 deletions(-) diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 2dda415..aecd433 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -345,13 +345,7 @@ def _handle_estop(self, state: ControllerState) -> None: logger.warning("E-STOP activated") self.estop_active = True self._segment_player.cancel(state) - self._planner.sync_tool( - state.current_tool, - variant_key=state.current_tool_variant, - tcp_offset_m=state.tcp_offset_m, - tcp_rotation_rad=state.tcp_rotation_rad, - ) - self._planner.sync_shapes(state.shapes) + self._resync_planner(state) if self._executor.active_command: self._executor.cancel_active_command("E-Stop activated") self._executor.clear_queue("E-Stop activated") @@ -812,6 +806,22 @@ def _handle_query( addr, make_error(ErrorCode.COMM_DECODE_ERROR, detail=str(e)) ) + def _resync_planner(self, state: ControllerState) -> None: + """Bring the planner subprocess back to the controller's tool and world. + + The planner applies SET_TCP_TRANSFORM / SELECT_TOOL / SET_SHAPES at + plan time, when the command is still queued. Cancelling that queue + leaves the planner holding a change the controller never applied, + and every later plan would be solved against it. + """ + self._planner.sync_tool( + state.current_tool, + variant_key=state.current_tool_variant, + tcp_offset_m=state.tcp_offset_m, + tcp_rotation_rad=state.tcp_rotation_rad, + ) + self._planner.sync_shapes(state.shapes) + def _handle_system_command( self, command: SystemCommand, @@ -842,6 +852,7 @@ def _handle_system_command( self._segment_player.cancel(state) self._executor.cancel_active_command(reason) self._executor.clear_queue(reason) + self._resync_planner(state) # Reset-state: cancel motion pipeline so stale segments don't play. # Also sync the (now-cleared) tool state to the planner subprocess @@ -850,13 +861,7 @@ def _handle_system_command( self._segment_player.cancel(state) self._executor.cancel_active_command("Reset") self._executor.clear_queue("Reset") - self._planner.sync_tool( - state.current_tool, - variant_key=state.current_tool_variant, - tcp_offset_m=state.tcp_offset_m, - tcp_rotation_rad=state.tcp_rotation_rad, - ) - self._planner.sync_shapes(state.shapes) + self._resync_planner(state) # Infrastructure side effects (only 2-3 commands trigger these) if command._switch_simulator is not None: diff --git a/tests/integration/test_stop_semantics.py b/tests/integration/test_stop_semantics.py index 2db63dd..d35d710 100644 --- a/tests/integration/test_stop_semantics.py +++ b/tests/integration/test_stop_semantics.py @@ -89,3 +89,31 @@ def test_estop_latches_until_reset(client: RobotClient, server_proc): "canceled motion resurfaced after reset" ) assert client.home(wait=True, timeout=30.0) >= 0 + + +def test_stop_discards_a_queued_tcp_transform_from_the_planner_too( + client: RobotClient, server_proc +): + """The planner applies SET_TCP_TRANSFORM when it plans, while the command + is still queued; a Stop that cancels the queue must take that transform + back from the planner, or every later plan is solved against a TCP the + controller never applied.""" + assert client.home(wait=True, timeout=30.0) >= 0 + index = client.set_tcp_transform(0, 0, 0, 0, 0, 0) + assert index >= 0 and client.wait_command(index, timeout=5.0) + start = client.angles() + pose = client.pose() + assert start is not None and pose is not None + assert client.delay(3.0) >= 0 + assert client.set_tcp_transform(0, 0, 40, 0, 90, 0) >= 0 + assert client.stop() == 1 + assert client.tcp_transform() == pytest.approx([0] * 6) + # A move to the pose the arm is already at is planned against the + # controller's TCP and therefore goes nowhere. + index = client.move_l(pose, duration=1.0, wait=False) + assert index >= 0 and client.wait_command(index, timeout=10.0) + after = client.angles() + assert after is not None + assert np.allclose(after, start, atol=0.5), ( + f"the planner kept the cancelled TCP: {start} -> {after}" + ) From 79a891cdb147dac7cb7505596277a4e7a950a9a7 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 18 Sep 2026 01:24:48 +0000 Subject: [PATCH 3/5] Drop the has_tcp_transform flag A full user TCP transform is part of every client contract now, so there is no flag to advertise. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01N8zt66KjkzgZLtQuSfJd9r --- parol6/robot.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/parol6/robot.py b/parol6/robot.py index e7ae130..51ffaf1 100644 --- a/parol6/robot.py +++ b/parol6/robot.py @@ -654,10 +654,6 @@ def async_client_class(self) -> type: # -- Kinematics --------------------------------------------------------- - @property - def has_tcp_transform(self) -> bool: - return True - def _load_q_buf(self, q_rad: NDArray[np.float64]) -> None: """Copy joint radians into the padded pinokin q buffer.""" n = min(len(q_rad), self._pinokin.nq) From 571f6be78cc24ea48bd1618a575c6d8cc0f85320 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 18 Sep 2026 03:04:18 +0000 Subject: [PATCH 4/5] Pin waldoctl v0.14.0 Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01N8zt66KjkzgZLtQuSfJd9r --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 0bc318d..dcc440e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -46,7 +46,7 @@ dependencies = [ "psutil>=5.9", "msgspec>=0.18", "ormsgpack>=1.4.0", - "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.15.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.14.0", ] [tool.setuptools.packages.find] From 46b303e33d7e152d4bbcf269afcac1dd4a10ddf2 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 18 Sep 2026 14:08:38 +0000 Subject: [PATCH 5/5] Import the unit helpers at module level The math conversions and the gripper config classes were imported inside the methods that use them although their modules are already imported at the top; nothing here cycles. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01N8zt66KjkzgZLtQuSfJd9r --- parol6/client/async_client.py | 3 +-- parol6/client/dry_run_client.py | 11 +++-------- parol6/commands/query_commands.py | 3 +-- parol6/commands/system_commands.py | 3 +-- parol6/server/motion_planner.py | 3 +-- 5 files changed, 7 insertions(+), 16 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 7a4fdfc..5d1e60b 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -11,6 +11,7 @@ import time from collections.abc import AsyncIterator, Callable from typing import TYPE_CHECKING, Any, cast +from math import isfinite import msgspec import numpy as np @@ -1041,8 +1042,6 @@ async def set_tcp_transform( ) async def tcp_transform(self) -> list[float]: - from math import isfinite - resp = await self._request(TcpTransformCmd()) if not isinstance(resp, TcpTransformResultStruct): raise TimeoutError("Controller did not return a TCP transform") diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 45a3167..7de0b8b 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -36,6 +36,7 @@ from ..utils.ik import solve_ik from pinokin import se3_from_rpy, se3_rpy import re as _re +from math import degrees, radians import parol6.protocol.wire as _wire from waldoctl.commands import CommandKind, command_table @@ -58,7 +59,8 @@ from ..server.state import ControllerState, get_fkine_se3 from ..utils.error_catalog import RobotError, make_error from ..utils.error_codes import ErrorCode -from parol6.tools import get_registry +from parol6.tools import ElectricGripperConfig, PneumaticGripperConfig, get_registry +from waldoctl.tools import ToolType if TYPE_CHECKING: from parol6.robot import Robot @@ -156,9 +158,6 @@ def key(self) -> str: @property def tool_type(self) -> str: - from waldoctl.tools import ToolType - from parol6.tools import ElectricGripperConfig, PneumaticGripperConfig - spec = get_registry().get(self.key) return ( ToolType.GRIPPER @@ -266,8 +265,6 @@ def tcp_offset(self) -> list[float]: ] def tcp_transform(self) -> list[float]: - from math import degrees - return self.tcp_offset() + [degrees(v) for v in self._state.tcp_rotation_rad] def flush(self) -> list[DryRunResult]: @@ -320,8 +317,6 @@ def _dispatch(self, params: Any) -> DryRunResult | None: self._active_variant_key = params.variant_key self._state.set_tool(self._active_tool_key, params.variant_key) if isinstance(params, (SetTcpOffsetCmd, SetTcpTransformCmd)): - from math import radians - rotation = ( (radians(params.roll), radians(params.pitch), radians(params.yaw)) if isinstance(params, SetTcpTransformCmd) diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index dd605b4..67ff11a 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -3,6 +3,7 @@ """ from typing import TYPE_CHECKING +from math import degrees import numpy as np @@ -443,8 +444,6 @@ class TcpTransformCommand(QueryCommand[TcpTransformCmd]): __slots__ = () def compute(self, state: "ControllerState") -> bytes: - from math import degrees - xyz = state.tcp_offset_m rpy = state.tcp_rotation_rad return pack_response( diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index 86c4a22..0589c5b 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -10,6 +10,7 @@ import logging import os from typing import TYPE_CHECKING +from math import radians from parol6.commands.base import ( CommandBase, @@ -242,8 +243,6 @@ class SetTcpTransformCommand(MotionCommand[SetTcpTransformCmd]): __slots__ = () def do_setup(self, state: ControllerState) -> None: - from math import radians - state.set_tcp_transform( (self.p.x / 1000, self.p.y / 1000, self.p.z / 1000), (radians(self.p.roll), radians(self.p.pitch), radians(self.p.yaw)), diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 4d1e2ba..c6f5ffc 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -20,6 +20,7 @@ import signal from dataclasses import dataclass, field from typing import TYPE_CHECKING, Union, cast +from math import radians import numpy as np @@ -514,8 +515,6 @@ def _handle_inline(self, command_index: int, params: object) -> None: tcp_rotation_rad=self.state.tcp_rotation_rad, ) elif isinstance(params, (SetTcpOffsetCmd, SetTcpTransformCmd)): - from math import radians - offset_m = (params.x / 1000.0, params.y / 1000.0, params.z / 1000.0) rotation_rad = ( (radians(params.roll), radians(params.pitch), radians(params.yaw))