Apply full TCP transforms through planning and readback

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.

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)):

parol6/ack_policy.py

@@ -11,7 +11,6 @@
     CmdType.SIMULATOR,
     CmdType.SELECT_PROFILE,
     CmdType.RESET_STATE,
-    CmdType.SET_TCP_OFFSET,
     CmdType.SET_SHAPES,
     CmdType.SET_STATUS_RATE,
 }
@@ -35,6 +34,7 @@
     CmdType.PING,
     CmdType.IS_SIMULATOR,
     CmdType.TCP_OFFSET,
+    CmdType.TCP_TRANSFORM,
     CmdType.SHAPES,
     CmdType.STATUS_RATE,
 }
@@ -52,6 +52,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,

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
@@ -84,6 +85,7 @@
     ShapesCmd,
     ShapesResultStruct,
     SetTcpOffsetCmd,
+    SetTcpTransformCmd,
     ShapeWire,
     ServoJCmd,
     ServoJPoseCmd,
@@ -94,6 +96,8 @@
     StatusCmd,
     TcpOffsetCmd,
     TcpOffsetResultStruct,
+    TcpTransformCmd,
+    TcpTransformResultStruct,
     TcpSpeedCmd,
     TeleportCmd,
     SpeedsResultStruct,
@@ -1013,6 +1017,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
@@ -1022,6 +1028,28 @@ 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]:
+        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).
 
@@ -1083,7 +1111,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"``).
@@ -1172,6 +1200,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:

parol6/client/dry_run_client.py

@@ -36,13 +36,15 @@
 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
 from ..protocol.wire import (
     HomeCmd,
     SelectToolCmd,
     SetTcpOffsetCmd,
+    SetTcpTransformCmd,
     TeleportCmd,
     ToolActionCmd,
 )
@@ -57,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
@@ -155,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
@@ -244,7 +244,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
@@ -260,11 +259,14 @@ 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]:
+        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()
@@ -306,21 +308,22 @@ 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)):
+            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
@@ -339,7 +342,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:

parol6/client/sync_client.py

@@ -374,6 +374,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).
 

parol6/commands/query_commands.py

@@ -3,6 +3,7 @@
 """
 
 from typing import TYPE_CHECKING
+from math import degrees
 
 import numpy as np
 
@@ -42,6 +43,8 @@
     SpeedsResultStruct,
     TcpOffsetCmd,
     TcpOffsetResultStruct,
+    TcpTransformCmd,
+    TcpTransformResultStruct,
     StatusCmd,
     StatusResultStruct,
     TcpSpeedCmd,
@@ -149,6 +152,7 @@ def compute(self, state: "ControllerState") -> bytes:
                     ts.fault_code,
                     list(ts.positions),
                     list(ts.channels),
+                    ts.variant_key,
                 ],
             )
         )
@@ -258,6 +262,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,
             )
         )
 
@@ -430,3 +435,24 @@ 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:
+        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]),
+            )
+        )

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,
@@ -25,6 +26,7 @@
     ResetCmd,
     SelectProfileCmd,
     SetTcpOffsetCmd,
+    SetTcpTransformCmd,
     SimulatorCmd,
     StopCmd,
     WriteIOCmd,
@@ -231,3 +233,21 @@ 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:
+        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