Enforce held-object geometry and explicit reconciliation

README.md

@@ -323,6 +323,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
@@ -501,3 +509,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

@@ -1,6 +1,6 @@
 import os
 
-from parol6.protocol.wire import CmdType
+from parol6.protocol.wire import CmdType, QueryType
 
 # System command types (always require ACK)
 SYSTEM_CMD_TYPES: set[CmdType] = {
@@ -19,29 +19,31 @@
 }
 
 # Query command types (use request/response, not ACK)
-QUERY_CMD_TYPES: set[CmdType] = {
-    CmdType.POSE,
-    CmdType.ANGLES,
-    CmdType.IO,
-    CmdType.JOINT_SPEEDS,
-    CmdType.STATUS,
-    CmdType.LOOP_STATS,
-    CmdType.ACTIVITY,
-    CmdType.QUEUE,
-    CmdType.TOOLS,
-    CmdType.TOOL_STATUS,
-    CmdType.PROFILE,
-    CmdType.REACHABLE,
-    CmdType.ERROR,
-    CmdType.TCP_SPEED,
-    CmdType.PING,
-    CmdType.IS_SIMULATOR,
-    CmdType.TCP_OFFSET,
-    CmdType.TCP_TRANSFORM,
-    CmdType.SHAPES,
-    CmdType.STATUS_RATE,
-    CmdType.EXECUTION_SPEED,
+QUERY_RESPONSE_TYPES: dict[CmdType, QueryType] = {
+    CmdType.POSE: QueryType.POSE,
+    CmdType.ANGLES: QueryType.ANGLES,
+    CmdType.IO: QueryType.IO,
+    CmdType.JOINT_SPEEDS: QueryType.SPEEDS,
+    CmdType.STATUS: QueryType.STATUS,
+    CmdType.LOOP_STATS: QueryType.LOOP_STATS,
+    CmdType.ACTIVITY: QueryType.CURRENT_ACTION,
+    CmdType.QUEUE: QueryType.QUEUE,
+    CmdType.TOOLS: QueryType.TOOL,
+    CmdType.TOOL_STATUS: QueryType.TOOL_STATUS,
+    CmdType.PROFILE: QueryType.PROFILE,
+    CmdType.REACHABLE: QueryType.ENABLEMENT,
+    CmdType.ERROR: QueryType.ERROR,
+    CmdType.TCP_SPEED: QueryType.TCP_SPEED,
+    CmdType.PING: QueryType.PING,
+    CmdType.IS_SIMULATOR: QueryType.IS_SIMULATOR,
+    CmdType.TCP_OFFSET: QueryType.TCP_OFFSET,
+    CmdType.TCP_TRANSFORM: QueryType.TCP_TRANSFORM,
+    CmdType.SHAPES: QueryType.SHAPES,
+    CmdType.STATUS_RATE: QueryType.STATUS_RATE,
+    CmdType.EXECUTION_SPEED: QueryType.EXECUTION_SPEED,
+    CmdType.COMMAND_COMPLETION: QueryType.COMMAND_COMPLETION,
 }
+QUERY_CMD_TYPES: set[CmdType] = set(QUERY_RESPONSE_TYPES)
 
 # Streaming commands are fire-and-forget (no ACK needed)
 FIRE_AND_FORGET: set[CmdType] = {

parol6/client/async_client.py

@@ -30,7 +30,12 @@
 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,
+    QUERY_RESPONSE_TYPES,
+    SYSTEM_CMD_TYPES,
+    AckPolicy,
+)
 from ..utils.error_catalog import RobotError
 from ..utils.errors import MotionError
 from ..protocol.wire import (
@@ -41,6 +46,8 @@
     decode_status_bin_into,
     CheckpointCmd,
     ConnectHardwareCmd,
+    CommandCompletionCmd,
+    CommandCompletionResultStruct,
     CurrentActionResultStruct,
     DelayCmd,
     EnablementResultStruct,
@@ -268,6 +275,7 @@ def skill_capabilities(self) -> frozenset[str]:
             "backend.parol6",
             "execution.speed",
             "observation.timed",
+            "world.attachments",
             "tool.gripper",
             "io.digital",
         }
@@ -643,6 +651,7 @@ async def _request(
         await self._ensure_endpoint()
         assert self._transport is not None
         data = encode_command(cmd)
+        expected = QUERY_RESPONSE_TYPES[STRUCT_TO_CMDTYPE[type(cmd)]]
         wait = self.timeout if timeout is None else timeout
         attempts = self.retries + 1 if timeout is None else 1
         for attempt in range(attempts):
@@ -653,13 +662,20 @@ 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 isinstance(parsed, ResponseMsg):
+                                    # A timed-out query can reply after the next
+                                    # query starts on this same UDP endpoint.
+                                    if parsed.result.__struct_config__.tag != expected:
+                                        continue
                                     return parsed.result
                                 if isinstance(parsed, ErrorMsg):
                                     raise MotionError(
@@ -701,10 +717,8 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg:
             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() as ok:
@@ -1208,15 +1222,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
             ),
@@ -1547,9 +1576,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).
@@ -1577,17 +1607,42 @@ 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 asyncio.sleep(0.02)
+        except TimeoutError:
+            return False
+        return False
 
     # --------------- Move commands (queued, pre-computed trajectory) ---------------