devagent-physical-engine 0.12.4__tar.gz → 0.12.5__tar.gz

{devagent_physical_engine-0.12.4/src/devagent_physical_engine.egg-info → devagent_physical_engine-0.12.5}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: devagent-physical-engine
-Version: 0.12.4
+Version: 0.12.5
 Summary: Verification-first agentic engineering, simulation, and evidence runtime for robotics and industrial automation.
 Author: Tom Ha
 License: All Rights Reserved
@@ -424,7 +424,7 @@ Detailed engineering documentation lives in the repository:
 
 ## Project status
 
-**v0.12.4 — Beta software / evidence-gated task-specific physical simulation.**
+**v0.12.5 — Beta software / evidence-gated task-specific physical simulation.**
 
 The software and release pipeline are production-oriented, but physical readiness is scope-specific and evidence-driven. The UR5e task-specific reference-workcell path must pass the executable v0.12 ROS/Gazebo/MoveIt campaign on the target workstation before that scope is considered simulation-qualified there. Real robot execution remains locked. Functional-safety certification and site qualification are not claimed.
 

{devagent_physical_engine-0.12.4 → devagent_physical_engine-0.12.5}/README.md

@@ -383,7 +383,7 @@ Detailed engineering documentation lives in the repository:
 
 ## Project status
 
-**v0.12.4 — Beta software / evidence-gated task-specific physical simulation.**
+**v0.12.5 — Beta software / evidence-gated task-specific physical simulation.**
 
 The software and release pipeline are production-oriented, but physical readiness is scope-specific and evidence-driven. The UR5e task-specific reference-workcell path must pass the executable v0.12 ROS/Gazebo/MoveIt campaign on the target workstation before that scope is considered simulation-qualified there. Real robot execution remains locked. Functional-safety certification and site qualification are not claimed.
 

{devagent_physical_engine-0.12.4 → devagent_physical_engine-0.12.5}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "devagent-physical-engine"
-version = "0.12.4"
+version = "0.12.5"
 description = "Verification-first agentic engineering, simulation, and evidence runtime for robotics and industrial automation."
 readme = { file = "README.md", content-type = "text/markdown" }
 requires-python = ">=3.11"

{devagent_physical_engine-0.12.4 → devagent_physical_engine-0.12.5}/src/devagent_physical_engine/__init__.py

@@ -41,4 +41,4 @@ __all__ = [
     "TwinValidator",
 ]
 
-__version__ = "0.12.4"
+__version__ = "0.12.5"

{devagent_physical_engine-0.12.4 → devagent_physical_engine-0.12.5}/src/devagent_physical_engine/physical_motion.py

@@ -160,6 +160,12 @@ class MotionExecutionMetrics:
     min_clearance_m: float | None = None
     final_tcp_error_m: float | None = None
     energy_proxy: float | None = None
+    joint_state_sample_rate_hz: float | None = None
+    max_sample_gap_s: float | None = None
+    trajectory_duration_error_s: float | None = None
+    tracking_sample_count: int | None = None
+    tracking_scope: str = "endpoint_only"
+    tracking_interpolation: str = "unspecified"
     planner_retries: int | None = None
     trajectory_retries: int | None = None
     failure_codes: tuple[str, ...] = ()
@@ -168,6 +174,10 @@ class MotionExecutionMetrics:
     def __post_init__(self) -> None:
         if not self.metrics_origin.strip():
             raise PhysicalMotionError("metrics_origin_required")
+        if not self.tracking_scope.strip():
+            raise PhysicalMotionError("tracking_scope_required")
+        if not self.tracking_interpolation.strip():
+            raise PhysicalMotionError("tracking_interpolation_required")
         for name in (
             "observed_duration_s",
             "planned_duration_s",
@@ -178,6 +188,9 @@ class MotionExecutionMetrics:
             "min_clearance_m",
             "final_tcp_error_m",
             "energy_proxy",
+            "joint_state_sample_rate_hz",
+            "max_sample_gap_s",
+            "trajectory_duration_error_s",
         ):
             value = getattr(self, name)
             if value is None:
@@ -185,7 +198,7 @@ class MotionExecutionMetrics:
             numeric = float(value)
             if not isfinite(numeric) or numeric < 0:
                 raise PhysicalMotionError(f"metric_invalid:{name}")
-        for name in ("planner_retries", "trajectory_retries"):
+        for name in ("tracking_sample_count", "planner_retries", "trajectory_retries"):
             value = getattr(self, name)
             if value is not None and value < 0:
                 raise PhysicalMotionError(f"metric_invalid:{name}")

devagent_physical_engine-0.12.5/src/devagent_physical_engine/pose_math.py

@@ -0,0 +1,134 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from math import acos, isfinite, sqrt
+
+
+class PoseMathError(ValueError):
+    pass
+
+
+def _finite(values: tuple[float, ...], *, code: str) -> tuple[float, ...]:
+    converted = tuple(float(value) for value in values)
+    if any(not isfinite(value) for value in converted):
+        raise PoseMathError(code)
+    return converted
+
+
+def normalize_quaternion(
+    quaternion_xyzw: tuple[float, float, float, float],
+) -> tuple[float, float, float, float]:
+    values = _finite(tuple(quaternion_xyzw), code="quaternion_non_finite")
+    if len(values) != 4:
+        raise PoseMathError("quaternion_size_invalid")
+    norm = sqrt(sum(value * value for value in values))
+    if norm <= 1e-12:
+        raise PoseMathError("quaternion_zero")
+    normalized = tuple(value / norm for value in values)
+    # q and -q encode the same rotation. Keep one deterministic representation.
+    for component in (normalized[3], normalized[0], normalized[1], normalized[2]):
+        if abs(component) <= 1e-15:
+            continue
+        return tuple(-value for value in normalized) if component < 0.0 else normalized
+    raise PoseMathError("quaternion_zero")
+
+
+def quaternion_multiply(
+    first: tuple[float, float, float, float],
+    second: tuple[float, float, float, float],
+) -> tuple[float, float, float, float]:
+    ax, ay, az, aw = normalize_quaternion(first)
+    bx, by, bz, bw = normalize_quaternion(second)
+    return normalize_quaternion(
+        (
+            aw * bx + ax * bw + ay * bz - az * by,
+            aw * by - ax * bz + ay * bw + az * bx,
+            aw * bz + ax * by - ay * bx + az * bw,
+            aw * bw - ax * bx - ay * by - az * bz,
+        )
+    )
+
+
+def quaternion_conjugate(
+    quaternion_xyzw: tuple[float, float, float, float],
+) -> tuple[float, float, float, float]:
+    x, y, z, w = normalize_quaternion(quaternion_xyzw)
+    return (-x, -y, -z, w)
+
+
+def rotate_vector(
+    quaternion_xyzw: tuple[float, float, float, float],
+    vector_xyz: tuple[float, float, float],
+) -> tuple[float, float, float]:
+    qx, qy, qz, qw = normalize_quaternion(quaternion_xyzw)
+    vx, vy, vz = _finite(tuple(vector_xyz), code="vector_non_finite")
+    if len((vx, vy, vz)) != 3:
+        raise PoseMathError("vector_size_invalid")
+    # Unit-quaternion vector rotation without constructing temporary quaternions.
+    tx = 2.0 * (qy * vz - qz * vy)
+    ty = 2.0 * (qz * vx - qx * vz)
+    tz = 2.0 * (qx * vy - qy * vx)
+    return (
+        vx + qw * tx + (qy * tz - qz * ty),
+        vy + qw * ty + (qz * tx - qx * tz),
+        vz + qw * tz + (qx * ty - qy * tx),
+    )
+
+
+@dataclass(frozen=True, slots=True)
+class RigidPose:
+    position_m: tuple[float, float, float]
+    quaternion_xyzw: tuple[float, float, float, float]
+
+    def __post_init__(self) -> None:
+        position = _finite(tuple(self.position_m), code="pose_position_non_finite")
+        if len(position) != 3:
+            raise PoseMathError("pose_position_size_invalid")
+        object.__setattr__(self, "position_m", position)
+        object.__setattr__(
+            self,
+            "quaternion_xyzw",
+            normalize_quaternion(tuple(self.quaternion_xyzw)),
+        )
+
+    @classmethod
+    def identity(cls) -> "RigidPose":
+        return cls((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))
+
+    def compose(self, child: "RigidPose") -> "RigidPose":
+        offset = rotate_vector(self.quaternion_xyzw, child.position_m)
+        return RigidPose(
+            tuple(self.position_m[index] + offset[index] for index in range(3)),
+            quaternion_multiply(self.quaternion_xyzw, child.quaternion_xyzw),
+        )
+
+    def inverse(self) -> "RigidPose":
+        inverse_q = quaternion_conjugate(self.quaternion_xyzw)
+        inverse_position = rotate_vector(
+            inverse_q,
+            tuple(-value for value in self.position_m),
+        )
+        return RigidPose(inverse_position, inverse_q)
+
+    def relative_to(self, parent: "RigidPose") -> "RigidPose":
+        return parent.inverse().compose(self)
+
+    def position_error_m(self, other: "RigidPose") -> float:
+        return sqrt(
+            sum(
+                (self.position_m[index] - other.position_m[index]) ** 2
+                for index in range(3)
+            )
+        )
+
+    def orientation_error_rad(self, other: "RigidPose") -> float:
+        first = normalize_quaternion(self.quaternion_xyzw)
+        second = normalize_quaternion(other.quaternion_xyzw)
+        dot = abs(sum(a * b for a, b in zip(first, second)))
+        return 2.0 * acos(max(-1.0, min(1.0, dot)))
+
+    def to_dict(self) -> dict[str, list[float]]:
+        return {
+            "position_m": list(self.position_m),
+            "quaternion_xyzw": list(self.quaternion_xyzw),
+        }

{devagent_physical_engine-0.12.4 → devagent_physical_engine-0.12.5}/src/devagent_physical_engine/reference_workcell.py

@@ -34,11 +34,21 @@ _PLATFORM_DIMS_M = (0.36, 0.36, 0.18)
 _TOOL_DIMS_M = (0.04, 0.04, 0.03)
 _TOOL_COLLISION_CENTER_LOCAL_M = (0.0, 0.0, 0.015)
 _WORKPIECE_CENTER_FROM_TOOL_LOCAL_M = (0.0, 0.0, 0.07)
-
-# Universal Robots published UR5e nominal DH dimensions in metres. These values are
-# used only to build the packaged reference workcell. Runtime MoveIt FK is required
-# to agree before a BOX is attached, so this template cannot silently override the
-# robot model installed on the workstation.
+# tool0 and flange are fixed UR end-effector frames without collision geometry;
+# wrist_3_link owns the physical wrist collision mesh at the mounting interface.
+# The packaged reference gripper starts at that interface, so those three links are
+# the only robot links it may touch. All other robot/environment collisions remain
+# authoritative in MoveIt.
+_REFERENCE_TOOL_TOUCH_LINKS = ("tool0", "flange", "wrist_3_link")
+
+# Universal Robots published UR5e nominal DH dimensions in metres. Standard UR DH
+# kinematics are expressed in the controller ``base`` convention. The ROS 2 Jazzy
+# UR description defines ``base_link`` as REP-103 and ``base`` at the same origin
+# rotated by pi around Z. DevAgent's canonical Twin and MoveIt FK both use
+# ``base_link``, so the reference FK applies that documented fixed transform before
+# the DH chain. Runtime MoveIt FK still has authority and must agree before a BOX is
+# attached, so this template cannot silently override the robot model installed on
+# the workstation.
 _UR5E_DH_A = (0.0, -0.425, -0.3922, 0.0, 0.0, 0.0)
 _UR5E_DH_D = (0.1625, 0.0, 0.0, 0.1333, 0.0997, 0.0996)
 _UR5E_DH_ALPHA = (pi / 2, 0.0, 0.0, pi / 2, -pi / 2, 0.0)
@@ -58,46 +68,88 @@ class LoadPhaseTarget:
     synchronize_workpiece_pose: bool = False
 
 
-def _matmul(first: tuple[tuple[float, ...], ...], second: tuple[tuple[float, ...], ...]):
+def _matmul(
+    first: tuple[tuple[float, ...], ...],
+    second: tuple[tuple[float, ...], ...],
+):
     return tuple(
-        tuple(sum(first[row][k] * second[k][col] for k in range(4)) for col in range(4))
+        tuple(
+            sum(first[row][k] * second[k][col] for k in range(4))
+            for col in range(4)
+        )
         for row in range(4)
     )
 
 
-def _rotation_to_quaternion(rotation: tuple[tuple[float, ...], ...]) -> tuple[float, float, float, float]:
+def _rotation_to_quaternion(
+    rotation: tuple[tuple[float, ...], ...],
+) -> tuple[float, float, float, float]:
     r00, r01, r02 = rotation[0]
     r10, r11, r12 = rotation[1]
     r20, r21, r22 = rotation[2]
     trace = r00 + r11 + r22
     if trace > 0.0:
         s = sqrt(trace + 1.0) * 2.0
-        q = ((r21 - r12) / s, (r02 - r20) / s, (r10 - r01) / s, 0.25 * s)
+        q = (
+            (r21 - r12) / s,
+            (r02 - r20) / s,
+            (r10 - r01) / s,
+            0.25 * s,
+        )
     elif r00 > r11 and r00 > r22:
         s = sqrt(1.0 + r00 - r11 - r22) * 2.0
-        q = (0.25 * s, (r01 + r10) / s, (r02 + r20) / s, (r21 - r12) / s)
+        q = (
+            0.25 * s,
+            (r01 + r10) / s,
+            (r02 + r20) / s,
+            (r21 - r12) / s,
+        )
     elif r11 > r22:
         s = sqrt(1.0 + r11 - r00 - r22) * 2.0
-        q = ((r01 + r10) / s, 0.25 * s, (r12 + r21) / s, (r02 - r20) / s)
+        q = (
+            (r01 + r10) / s,
+            0.25 * s,
+            (r12 + r21) / s,
+            (r02 - r20) / s,
+        )
     else:
         s = sqrt(1.0 + r22 - r00 - r11) * 2.0
-        q = ((r02 + r20) / s, (r12 + r21) / s, 0.25 * s, (r10 - r01) / s)
+        q = (
+            (r02 + r20) / s,
+            (r12 + r21) / s,
+            0.25 * s,
+            (r10 - r01) / s,
+        )
     norm = sqrt(sum(value * value for value in q))
     normalized = tuple(value / norm for value in q)
-    return tuple(-value for value in normalized) if normalized[3] < 0.0 else normalized
+    return (
+        tuple(-value for value in normalized)
+        if normalized[3] < 0.0
+        else normalized
+    )
 
 
 def ur5e_reference_fk(joints_rad: Iterable[float]) -> ReferenceToolPose:
     joints = tuple(float(value) for value in joints_rad)
     if len(joints) != 6:
         raise ValueError("ur5e_reference_fk_requires_six_joints")
+
+    # base_link_T_base: same origin, 180 deg around Z. This is the fixed transform
+    # documented by Universal Robots for the ROS ``base_link`` -> controller ``base``
+    # convention. Multiplying it before the standard DH chain expresses tool0 in
+    # ``base_link`` exactly like MoveIt / robot_state_publisher.
     transform: tuple[tuple[float, ...], ...] = (
-        (1.0, 0.0, 0.0, 0.0),
-        (0.0, 1.0, 0.0, 0.0),
+        (-1.0, 0.0, 0.0, 0.0),
+        (0.0, -1.0, 0.0, 0.0),
         (0.0, 0.0, 1.0, 0.0),
         (0.0, 0.0, 0.0, 1.0),
     )
-    for theta, a, d, alpha in zip(joints, _UR5E_DH_A, _UR5E_DH_D, _UR5E_DH_ALPHA):
+    for theta, a, d, alpha in zip(
+        joints,
+        _UR5E_DH_A,
+        _UR5E_DH_D,
+        _UR5E_DH_ALPHA,
+    ):
         ct, st = cos(theta), sin(theta)
         ca, sa = cos(alpha), sin(alpha)
         local = (
@@ -107,17 +159,22 @@ def ur5e_reference_fk(joints_rad: Iterable[float]) -> ReferenceToolPose:
             (0.0, 0.0, 0.0, 1.0),
         )
         transform = _matmul(transform, local)
-    rotation = tuple(tuple(transform[row][col] for col in range(3)) for row in range(3))
+    rotation = tuple(
+        tuple(transform[row][col] for col in range(3))
+        for row in range(3)
+    )
     return ReferenceToolPose(
         position_m=tuple(float(transform[row][3]) for row in range(3)),
         quaternion_xyzw=_rotation_to_quaternion(rotation),
     )
 
 
-def _rotate(quaternion: tuple[float, float, float, float], vector: tuple[float, float, float]):
+def _rotate(
+    quaternion: tuple[float, float, float, float],
+    vector: tuple[float, float, float],
+):
     x, y, z, w = quaternion
     vx, vy, vz = vector
-    # Optimized unit-quaternion vector rotation.
     tx = 2.0 * (y * vz - z * vy)
     ty = 2.0 * (z * vx - x * vz)
     tz = 2.0 * (x * vy - y * vx)
@@ -128,12 +185,19 @@ def _rotate(quaternion: tuple[float, float, float, float], vector: tuple[float,
     )
 
 
-def _translated_tool_pose(tool: ReferenceToolPose, local_offset_m: tuple[float, float, float]) -> tuple[float, float, float]:
+def _translated_tool_pose(
+    tool: ReferenceToolPose,
+    local_offset_m: tuple[float, float, float],
+) -> tuple[float, float, float]:
     offset = _rotate(tool.quaternion_xyzw, local_offset_m)
     return tuple(tool.position_m[index] + offset[index] for index in range(3))
 
 
-def _box(dimensions_m: tuple[float, float, float], *, origin: EvidenceOrigin = EvidenceOrigin.DEFAULT) -> GeometrySpec:
+def _box(
+    dimensions_m: tuple[float, float, float],
+    *,
+    origin: EvidenceOrigin = EvidenceOrigin.DEFAULT,
+) -> GeometrySpec:
     return GeometrySpec(
         GeometryKind.BOX,
         origin,
@@ -156,7 +220,9 @@ def ur5e_load_phase_targets() -> tuple[LoadPhaseTarget, ...]:
     )
 
 
-def ur5e_reference_load_twin(request: ValidatedEngineeringRequest) -> TwinSpec:
+def ur5e_reference_load_twin(
+    request: ValidatedEngineeringRequest,
+) -> TwinSpec:
     if request.robot_key != "ur5e":
         raise ValueError("reference_load_workcell_requires_ur5e")
     if request.operation != "load":
@@ -166,13 +232,25 @@ def ur5e_reference_load_twin(request: ValidatedEngineeringRequest) -> TwinSpec:
 
     pick_tool = ur5e_reference_fk(UR5E_PICK)
     place_tool = ur5e_reference_fk(UR5E_PLACE)
-    source_box_center = _translated_tool_pose(pick_tool, _WORKPIECE_CENTER_FROM_TOOL_LOCAL_M)
-    destination_box_center = _translated_tool_pose(place_tool, _WORKPIECE_CENTER_FROM_TOOL_LOCAL_M)
+    source_box_center = _translated_tool_pose(
+        pick_tool,
+        _WORKPIECE_CENTER_FROM_TOOL_LOCAL_M,
+    )
+    destination_box_center = _translated_tool_pose(
+        place_tool,
+        _WORKPIECE_CENTER_FROM_TOOL_LOCAL_M,
+    )
 
     workpiece_bottom_z = source_box_center[2] - _WORKPIECE_DIMS_M[2] / 2.0
-    source_platform_center_z = workpiece_bottom_z - _PLATFORM_DIMS_M[2] / 2.0
-    destination_bottom_z = destination_box_center[2] - _WORKPIECE_DIMS_M[2] / 2.0
-    destination_platform_center_z = destination_bottom_z - _PLATFORM_DIMS_M[2] / 2.0
+    source_platform_center_z = (
+        workpiece_bottom_z - _PLATFORM_DIMS_M[2] / 2.0
+    )
+    destination_bottom_z = (
+        destination_box_center[2] - _WORKPIECE_DIMS_M[2] / 2.0
+    )
+    destination_platform_center_z = (
+        destination_bottom_z - _PLATFORM_DIMS_M[2] / 2.0
+    )
 
     source = TwinEntity(
         request.source,
@@ -185,7 +263,10 @@ def ur5e_reference_load_twin(request: ValidatedEngineeringRequest) -> TwinSpec:
             source_ref=REFERENCE_SOURCE_REF,
         ),
         _box(_PLATFORM_DIMS_M),
-        metadata={"reference_role": "source", "template": REFERENCE_WORKCELL_ID},
+        metadata={
+            "reference_role": "source",
+            "template": REFERENCE_WORKCELL_ID,
+        },
     )
     destination = TwinEntity(
         request.destination,
@@ -198,7 +279,10 @@ def ur5e_reference_load_twin(request: ValidatedEngineeringRequest) -> TwinSpec:
             source_ref=REFERENCE_SOURCE_REF,
         ),
         _box(_PLATFORM_DIMS_M),
-        metadata={"reference_role": "destination", "template": REFERENCE_WORKCELL_ID},
+        metadata={
+            "reference_role": "destination",
+            "template": REFERENCE_WORKCELL_ID,
+        },
     )
     workpiece = TwinEntity(
         request.object_id,
@@ -211,14 +295,29 @@ def ur5e_reference_load_twin(request: ValidatedEngineeringRequest) -> TwinSpec:
         _box(_WORKPIECE_DIMS_M),
         physics=PhysicsSpec(
             mass_kg=EvidenceValue(
-                float(request.payload_kg) if request.payload_kg is not None else 1.0,
-                EvidenceOrigin.USER_DECLARED if request.payload_kg is not None else EvidenceOrigin.DEFAULT,
+                float(request.payload_kg)
+                if request.payload_kg is not None
+                else 1.0,
+                EvidenceOrigin.USER_DECLARED
+                if request.payload_kg is not None
+                else EvidenceOrigin.DEFAULT,
+                source_ref=REFERENCE_SOURCE_REF,
+            ),
+            friction_coefficient=EvidenceValue(
+                0.6,
+                EvidenceOrigin.DEFAULT,
+                source_ref=REFERENCE_SOURCE_REF,
+            ),
+            restitution=EvidenceValue(
+                0.0,
+                EvidenceOrigin.DEFAULT,
                 source_ref=REFERENCE_SOURCE_REF,
             ),
-            friction_coefficient=EvidenceValue(0.6, EvidenceOrigin.DEFAULT, source_ref=REFERENCE_SOURCE_REF),
-            restitution=EvidenceValue(0.0, EvidenceOrigin.DEFAULT, source_ref=REFERENCE_SOURCE_REF),
         ),
-        metadata={"reference_role": "workpiece", "template": REFERENCE_WORKCELL_ID},
+        metadata={
+            "reference_role": "workpiece",
+            "template": REFERENCE_WORKCELL_ID,
+        },
     )
     return TwinSpec(
         twin_id=f"TWIN-{request.request_id}-{REFERENCE_WORKCELL_ID}",
@@ -244,18 +343,31 @@ def ur5e_reference_load_twin(request: ValidatedEngineeringRequest) -> TwinSpec:
                 source_ref=REFERENCE_SOURCE_REF,
             ),
             geometry=_box(_TOOL_DIMS_M, origin=EvidenceOrigin.DEFAULT),
-            max_payload_kg=EvidenceValue(5.0, EvidenceOrigin.ROBOT_PROFILE, source_ref=REFERENCE_SOURCE_REF),
+            max_payload_kg=EvidenceValue(
+                5.0,
+                EvidenceOrigin.ROBOT_PROFILE,
+                source_ref=REFERENCE_SOURCE_REF,
+            ),
         ),
         controller=ControllerModel(
-            speed_scale_pct=EvidenceValue(30.0, EvidenceOrigin.DEFAULT, source_ref=REFERENCE_SOURCE_REF)
+            speed_scale_pct=EvidenceValue(
+                30.0,
+                EvidenceOrigin.DEFAULT,
+                source_ref=REFERENCE_SOURCE_REF,
+            )
         ),
         metadata={
             "template": REFERENCE_WORKCELL_ID,
-            "template_scope": "packaged_reference_simulation_not_customer_site_geometry",
+            "template_scope": (
+                "packaged_reference_simulation_not_customer_site_geometry"
+            ),
             "request_id": request.request_id,
+            "kinematic_reference_frame": "base_link",
             "expected_pick_tcp_position_m": list(pick_tool.position_m),
             "expected_place_tcp_position_m": list(place_tool.position_m),
-            "workpiece_tool_offset_local_m": list(_WORKPIECE_CENTER_FROM_TOOL_LOCAL_M),
+            "workpiece_tool_offset_local_m": list(
+                _WORKPIECE_CENTER_FROM_TOOL_LOCAL_M
+            ),
             "physical_qualification": False,
             "site_qualification": False,
         },
@@ -267,5 +379,5 @@ def ur5e_reference_tool_binding() -> ToolCollisionBinding:
         mount_link="tool0",
         position_m=_TOOL_COLLISION_CENTER_LOCAL_M,
         quaternion_xyzw=(0.0, 0.0, 0.0, 1.0),
-        touch_links=("tool0",),
+        touch_links=_REFERENCE_TOOL_TOUCH_LINKS,
     )

devagent_physical_engine-0.12.5/src/devagent_physical_engine/ros2/joint_state_probe.py

@@ -0,0 +1,139 @@
+from __future__ import annotations
+
+import argparse
+import json
+from math import isfinite
+import os
+from time import monotonic
+from typing import Any, Sequence
+
+
+def _validated_payload(
+    names: Sequence[str],
+    positions: Sequence[float],
+    required_joint_names: Sequence[str],
+) -> tuple[dict[str, Any] | None, str]:
+    canonical_names = tuple(str(name) for name in names)
+    try:
+        canonical_positions = tuple(float(value) for value in positions)
+    except (TypeError, ValueError):
+        return None, "joint_state_values_invalid"
+
+    if not canonical_names or len(canonical_names) != len(canonical_positions):
+        return None, "joint_state_shape_invalid"
+    if any(not name.strip() for name in canonical_names):
+        return None, "joint_state_empty_name"
+    if len(set(canonical_names)) != len(canonical_names):
+        return None, "joint_state_duplicate_name"
+    if any(not isfinite(value) for value in canonical_positions):
+        return None, "joint_state_non_finite"
+
+    required = tuple(str(name).strip() for name in required_joint_names)
+    if any(not name for name in required) or len(set(required)) != len(required):
+        return None, "joint_state_required_names_invalid"
+    missing = tuple(sorted(set(required) - set(canonical_names)))
+    if missing:
+        return None, "joint_state_missing:" + ",".join(missing)
+
+    return (
+        {
+            "success": True,
+            "code": "joint_state_snapshot_verified",
+            "name": list(canonical_names),
+            "position": list(canonical_positions),
+        },
+        "joint_state_snapshot_verified",
+    )
+
+
+def capture_joint_state(
+    *,
+    topic: str,
+    timeout_s: float,
+    required_joint_names: Sequence[str],
+) -> dict[str, Any]:
+    if timeout_s <= 0:
+        raise ValueError("joint_state_timeout_must_be_positive")
+    if not topic.startswith("/") or any(character.isspace() for character in topic):
+        raise ValueError("joint_state_topic_invalid")
+
+    import rclpy
+    from rclpy.qos import qos_profile_sensor_data
+    from sensor_msgs.msg import JointState
+
+    rclpy.init(args=None)
+    node = rclpy.create_node(f"devagent_joint_state_probe_{os.getpid()}")
+    result: dict[str, Any] | None = None
+    received_samples = 0
+    last_invalid_code = "joint_state_timeout"
+
+    def receive(message: JointState) -> None:
+        nonlocal result, received_samples, last_invalid_code
+        received_samples += 1
+        payload, code = _validated_payload(
+            message.name,
+            message.position,
+            required_joint_names,
+        )
+        if payload is not None:
+            result = payload
+        else:
+            last_invalid_code = code
+
+    subscription = node.create_subscription(
+        JointState,
+        topic,
+        receive,
+        qos_profile_sensor_data,
+    )
+    deadline = monotonic() + float(timeout_s)
+    try:
+        while rclpy.ok() and result is None:
+            remaining = deadline - monotonic()
+            if remaining <= 0:
+                break
+            rclpy.spin_once(node, timeout_sec=min(0.2, remaining))
+
+        if result is not None:
+            return {
+                **result,
+                "received_samples": received_samples,
+            }
+        return {
+            "success": False,
+            "code": last_invalid_code,
+            "received_samples": received_samples,
+        }
+    finally:
+        node.destroy_subscription(subscription)
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--topic", default="/joint_states")
+    parser.add_argument("--timeout", type=float, default=8.0)
+    parser.add_argument("--required-joint", action="append", default=[])
+    args = parser.parse_args()
+
+    try:
+        result = capture_joint_state(
+            topic=str(args.topic),
+            timeout_s=float(args.timeout),
+            required_joint_names=tuple(str(value) for value in args.required_joint),
+        )
+    except Exception as exc:
+        result = {
+            "success": False,
+            "code": f"joint_state_probe_error:{type(exc).__name__}",
+            "received_samples": 0,
+        }
+
+    print(json.dumps(result, sort_keys=True, separators=(",", ":"), allow_nan=False))
+    return 0 if result.get("success") else 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())