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]