Enforce held-object geometry and explicit reconciliation

README.md

@@ -330,6 +330,14 @@ the pause request can be acknowledged while still decelerating. Queued delays
 retain their remaining time while paused; positive speed changes do not retime
 delays, tool actuators or homing routines already in progress.
 
+Completion waits query the requested command's exact success. Tool actions run
+concurrently with arm motion, so the highest completed index alone cannot prove
+that an earlier command finished. The controller retains its latest 1024
+successful completions; an unknown, cancelled, or expired result remains
+unconfirmed. A controller-session change during a wait raises `ConnectionError`.
+This requires matching client and controller versions supporting the completion
+query.
+
 Standalone `wait_command()` keeps its wall-clock timeout and returns false if
 completion is unconfirmed. Blocking motion calls raise `TimeoutError` in that
 case. A timed-out wait leaves the motion queued; `stop()` cancels it. Planning
@@ -508,3 +516,30 @@ The client advertises `io.digital` for typed named-signal skills, which can be
 imported from `waldo_commander.skills`; mappings are `waldoctl.signals.DigitalSignal`
 values stored in a setup snapshot. Dry-run clients advertise `execution.preview`
 so those skills require explicit observation fixtures during preview.
+
+## Held-object collision geometry
+
+Program shapes can be attached to the `L6` flange. `shape.attach(flange_pose=...,
+epoch=world.attachment_epoch, allowed_contacts=(...))` creates a declaration from
+a fresh `world = rbt.shapes()` readback; apply the complete program layer with
+`rbt.set_shapes(...)`. Poses use metres and extrinsic XYZ radians (`Rz @ Ry @ Rx`)
+relative to the flange, independently of the tool/TCP correction. A detachment
+uses `shape.detach(world_pose=...)` and removes its contact exemptions.
+
+Only collision-enabled, nonphysical program shapes can attach. Changes require
+idle motion and a fresh position reference. Exact allowed-contact names exempt
+only pairs involving their declaring shape: URDF links, `tool:name`,
+`shape:name`, or `install:name`, with at most 32 unique partners. Unknown names,
+wildcards and self names are refused without changing the applied world.
+Unrelated checks stay active during planned and streamed motion.
+
+Readback includes `attachment_epoch` and `attachments_valid`. Controller/session,
+reference, source and selected-tool changes invalidate the old assumptions;
+arm motion remains blocked until the declarations are removed or explicitly
+reconciled against fresh state. For multiple stale attachments, reapply all
+verified declarations together in one `set_shapes` call. Stored world files
+do not restore a fresh context. Dry-run clients preserve these context gates.
+
+These declarations do not actuate a gripper, confirm a grasp or estimate payload.
+Waldo Commander supplies `attach_object` / `detach_object` Python skills and
+shape-menu controls that use this API and verify controller readback.

parol6/PAROL6_ROBOT.py

@@ -313,12 +313,35 @@ def apply_shapes(shapes: "Iterable[Any]") -> None:
     """
     global _active_shape_names, _program_shapes
     shapes = _validate_shapes(shapes)
-    _program_shapes = shapes
     if collision is None:
+        if any(s.attachment is not None for s in shapes):
+            raise ValueError("attachments require an active collision checker")
+        _program_shapes = shapes
         return
+    names = {
+        reported
+        for name, reported in collision.geometry_link_names
+        if not name.startswith("shape:")
+    } | {f"shape:{s.name}" for s in shapes if s.collision}
+    for shape in shapes:
+        if shape.attachment is not None:
+            unknown = set(shape.attachment.allowed_contacts) - names
+            if unknown:
+                raise ValueError(f"unknown contact partners: {sorted(unknown)}")
+    previous = _program_shapes
+    try:
+        _replace_program_geometry(shapes)
+    except Exception:
+        _replace_program_geometry(previous)
+        raise
+    _program_shapes = shapes
+
+
+def _replace_program_geometry(shapes: list) -> None:
+    assert collision is not None
     for name in _active_shape_names:
         collision.remove_geometry_by_name(name)
-    _active_shape_names = []
+    _active_shape_names.clear()
     for s in shapes:
         if not s.collision:
             continue
@@ -327,6 +350,27 @@ def apply_shapes(shapes: "Iterable[Any]") -> None:
             name, s.kind, s.params(), _pose_to_matrix(s.pose), margin=s.margin
         )
         _active_shape_names.append(name)
+        if s.attachment is not None:
+            collision.reparent_geometry_by_name(name, "L6", _pose_to_matrix(s.pose))
+    geom_names = collision.geometry_names
+    reports = dict(collision.geometry_link_names)
+    attached = {
+        f"shape:{s.name}": s.attachment for s in shapes if s.attachment is not None
+    }
+    for name, attachment in attached.items():
+        index = geom_names.index(name)
+        for other_index, other_name in enumerate(geom_names):
+            if other_index == index:
+                continue
+            other_attachment = attached.get(other_name)
+            allowed = reports[other_name] in attachment.allowed_contacts or (
+                other_attachment is not None
+                and reports[name] in other_attachment.allowed_contacts
+            )
+            if allowed:
+                collision.remove_collision_pair(index, other_index)
+            else:
+                collision.add_collision_pair(index, other_index)
 
 
 def apply_installation_shapes(shapes: "Iterable[Any]") -> None:
@@ -339,6 +383,8 @@ def apply_installation_shapes(shapes: "Iterable[Any]") -> None:
     """
     global _installation_shapes
     shapes = _validate_shapes(shapes)
+    if any(s.attachment is not None for s in shapes):
+        raise ValueError("installation shapes cannot declare attachments")
     _installation_shapes = shapes
     if collision is None:
         return

parol6/ack_policy.py

@@ -40,6 +40,7 @@
     CmdType.SHAPES,
     CmdType.STATUS_RATE,
     CmdType.EXECUTION_SPEED,
+    CmdType.COMMAND_COMPLETION,
 }
 
 # Streaming commands are fire-and-forget (no ACK needed)
@@ -71,6 +72,25 @@
     CmdType.TOOL_ACTION,
 }
 
+# Commands that move the arm: refused while the attachment context is stale
+ARM_MOTION_CMD_TYPES: frozenset[CmdType] = frozenset(
+    {
+        CmdType.HOME,
+        CmdType.MOVEJ,
+        CmdType.MOVEJ_POSE,
+        CmdType.MOVEL,
+        CmdType.MOVEC,
+        CmdType.MOVES,
+        CmdType.MOVEP,
+        CmdType.JOGJ,
+        CmdType.JOGL,
+        CmdType.SERVOJ,
+        CmdType.SERVOJ_POSE,
+        CmdType.SERVOL,
+        CmdType.TELEPORT,
+    }
+)
+
 
 class AckPolicy:
     """

parol6/client/async_client.py

@@ -31,7 +31,11 @@
 from waldoctl.execution import ExecutionSpeed, validate_execution_scale
 
 from .. import config as cfg
-from ..ack_policy import QUERY_CMD_TYPES, SYSTEM_CMD_TYPES, AckPolicy
+from ..ack_policy import (
+    QUERY_CMD_TYPES,
+    SYSTEM_CMD_TYPES,
+    AckPolicy,
+)
 from ..utils.error_catalog import RobotError
 from ..utils.errors import MotionError
 from ..protocol.wire import (
@@ -42,6 +46,8 @@
     decode_status_bin_into,
     CheckpointCmd,
     ConnectHardwareCmd,
+    CommandCompletionCmd,
+    CommandCompletionResultStruct,
     CurrentActionResultStruct,
     DelayCmd,
     EnablementResultStruct,
@@ -677,10 +683,13 @@ async def _request(
                     end_time = time.monotonic() + wait
                     while time.monotonic() < end_time:
                         try:
-                            resp_data, _ = await asyncio.wait_for(
-                                self._rx_queue.get(),
-                                timeout=max(0.0, end_time - time.monotonic()),
-                            )
+                            # Keep the receive in this task: Python 3.11's
+                            # wait_for can swallow an outer cancellation when
+                            # its child receives a reply in the same turn.
+                            async with asyncio.timeout(
+                                max(0.0, end_time - time.monotonic())
+                            ):
+                                resp_data, _ = await self._rx_queue.get()
                             try:
                                 parsed = decode_message(resp_data)
                                 if parsed.req_id != req_id:
@@ -732,10 +741,8 @@ async def _request_ok_raw(self, data: bytes, timeout: float, req_id: int) -> OkM
             self._transport.sendto(data)
             while time.monotonic() < end_time:
                 try:
-                    resp_data, _addr = await asyncio.wait_for(
-                        self._rx_queue.get(),
-                        timeout=max(0.0, end_time - time.monotonic()),
-                    )
+                    async with asyncio.timeout(max(0.0, end_time - time.monotonic())):
+                        resp_data, _addr = await self._rx_queue.get()
                     try:
                         match decode_message(resp_data):
                             case OkMsg(reply_id) as ok if reply_id == req_id:
@@ -1245,15 +1252,30 @@ async def shapes(self) -> ShapeWorld | None:
         if not isinstance(resp, ShapesResultStruct):
             return None
         return ShapeWorld(
+            attachment_epoch=resp.attachment_epoch,
             installation=tuple(
                 shape_from_wire(
-                    w.kind, w.params, w.pose, w.collision, w.margin, w.name, w.physics
+                    w.kind,
+                    w.params,
+                    w.pose,
+                    w.collision,
+                    w.margin,
+                    w.name,
+                    w.physics,
+                    w.attachment,
                 )
                 for w in resp.installation
             ),
             program=tuple(
                 shape_from_wire(
-                    w.kind, w.params, w.pose, w.collision, w.margin, w.name, w.physics
+                    w.kind,
+                    w.params,
+                    w.pose,
+                    w.collision,
+                    w.margin,
+                    w.name,
+                    w.physics,
+                    w.attachment,
                 )
                 for w in resp.program
             ),
@@ -1585,9 +1607,10 @@ async def wait_status(
     async def wait_command(self, command_index: int, timeout: float = 10.0) -> bool:
         """Wait until a specific command index has been completed.
 
-        Uses status broadcasts to monitor the server's completed_command_index.
-        Raises MotionError if the pipeline reports a planning/execution failure
-        at or before the awaited command index.
+        Queries exact success in the controller's last 1024 completions.
+        A concurrent tool finishing does not complete an unfinished arm command.
+        Unknown, cancelled, or expired results are never inferred successful
+        from the status high-water mark. Pipeline failures raise MotionError.
 
         Args:
             command_index: The command index to wait for (returned by motion commands).
@@ -1615,17 +1638,60 @@ def _blocking_error(s: StatusBuffer) -> RobotError | None:
                 return err
             return None
 
-        def _done(s: StatusBuffer) -> bool:
-            if s.completed_index >= command_index:
-                return True
-            return _blocking_error(s) is not None
-
-        ok = await self.wait_status(_done, timeout=timeout)
-        if ok:
-            err = _blocking_error(self._shared_status)
-            if err is not None:
-                raise MotionError(err)
-        return ok
+        command = CommandCompletionCmd(command_index)
+        session_id = self._shared_status.session_id or None
+
+        def check_session(candidate: int) -> None:
+            nonlocal session_id
+            if not candidate:
+                return
+            if session_id is None:
+                session_id = candidate
+            elif candidate != session_id:
+                raise ConnectionError(
+                    "Controller session changed during completion wait"
+                )
+
+        try:
+            async with asyncio.timeout(timeout):
+                while not self._closed:
+                    check_session(self._shared_status.session_id)
+                    result = await self._request(command)
+                    # Status has its own socket and can survive a command
+                    # socket that stopped receiving after a peer restart.
+                    check_session(self._shared_status.session_id)
+                    if (
+                        isinstance(result, CommandCompletionResultStruct)
+                        and result.command_index == command_index
+                    ):
+                        check_session(result.session_id)
+                        if result.completed:
+                            return True
+                    err = _blocking_error(self._shared_status)
+                    if err is not None:
+                        raise MotionError(err)
+                    await self._await_completion_hint(command_index, 0.25)
+        except TimeoutError:
+            return False
+        return False
+
+    async def _await_completion_hint(self, command_index: int, timeout: float) -> None:
+        """Return once a status frame reports the command complete or an
+        error standing, or after ``timeout`` without one, so the completion
+        query is paced by the status stream and still re-asked without it."""
+        end_time = time.monotonic() + timeout
+        while True:
+            self._status_event.clear()
+            status = self._shared_status
+            if status.completed_index >= command_index or status.error is not None:
+                return
+            remaining = end_time - time.monotonic()
+            if remaining <= 0:
+                return
+            try:
+                await asyncio.wait_for(self._status_event.wait(), remaining)
+            except asyncio.TimeoutError:
+                return
 
     # --------------- Move commands (queued, pre-computed trajectory) ---------------