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

@@ -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,

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:

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:

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

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]),
+            )
+        )

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