devagent-physical-engine 0.10.0__py3-none-any.whl

devagent_physical_engine/twin_materialization.py

@@ -0,0 +1,404 @@
+from __future__ import annotations
+
+from dataclasses import asdict, dataclass
+from hashlib import sha256
+import json
+from math import asin, atan2, cos, isfinite, sin
+from pathlib import Path
+import re
+from typing import Any
+
+from .twin import GeometryKind, Pose3D, TwinSpec
+
+
+class TwinMaterializationError(ValueError):
+    pass
+
+
+@dataclass(frozen=True, slots=True)
+class MaterializationIssue:
+    code: str
+    detail: str
+    entity_id: str | None = None
+
+
+@dataclass(frozen=True, slots=True)
+class MaterializedObject:
+    entity_id: str
+    backend_id: str
+    kind: str
+    dimensions_m: tuple[float, ...]
+    position_m: tuple[float, float, float]
+    quaternion_xyzw: tuple[float, float, float, float]
+    static: bool
+    mass_kg: float | None = None
+    friction: float | None = None
+    restitution: float | None = None
+
+    def to_dict(self) -> dict[str, Any]:
+        return asdict(self)
+
+
+@dataclass(frozen=True, slots=True)
+class CanonicalTwinMaterialization:
+    """One canonical geometry set consumed by both Gazebo and MoveIt."""
+
+    twin_id: str
+    twin_hash: str
+    robot_profile_key: str
+    planning_frame: str
+    objects: tuple[MaterializedObject, ...]
+    materializer_version: str = "canonical-twin-v1"
+
+    def to_dict(self) -> dict[str, Any]:
+        return {
+            "twin_id": self.twin_id,
+            "twin_hash": self.twin_hash,
+            "robot_profile_key": self.robot_profile_key,
+            "planning_frame": self.planning_frame,
+            "objects": [item.to_dict() for item in self.objects],
+            "materializer_version": self.materializer_version,
+        }
+
+    @property
+    def fingerprint(self) -> str:
+        return _hash(self.to_dict())
+
+    def canonical_scene(self) -> dict[str, Any]:
+        return {
+            "planning_frame": self.planning_frame,
+            "objects": [
+                {
+                    "id": item.backend_id,
+                    "geometry_kind": item.kind,
+                    "dimensions_m": [_canon_float(v) for v in item.dimensions_m],
+                    "position_m": [_canon_float(v) for v in item.position_m],
+                    "quaternion_xyzw": [_canon_float(v) for v in item.quaternion_xyzw],
+                }
+                for item in self.objects
+            ],
+        }
+
+    @property
+    def scene_hash(self) -> str:
+        return _hash(self.canonical_scene())
+
+    def moveit_scene_payload(self) -> dict[str, Any]:
+        return {
+            "materialization_hash": self.fingerprint,
+            "twin_hash": self.twin_hash,
+            "planning_frame": self.planning_frame,
+            "scene_hash": self.scene_hash,
+            "objects": [
+                {
+                    "id": item.backend_id,
+                    "geometry_kind": item.kind,
+                    "dimensions_m": list(item.dimensions_m),
+                    "pose": {
+                        "position_m": list(item.position_m),
+                        "quaternion_xyzw": list(item.quaternion_xyzw),
+                    },
+                }
+                for item in self.objects
+            ],
+        }
+
+    def to_sdf(self) -> str:
+        lines = [
+            "<sdf version='1.10'>",
+            "  <world name='default'>",
+            "    <gravity>0 0 -9.8</gravity>",
+            "    <physics type='ode'><max_step_size>0.001</max_step_size><real_time_factor>1</real_time_factor><real_time_update_rate>1000</real_time_update_rate></physics>",
+        ]
+        for item in self.objects:
+            x, y, z = item.position_m
+            roll, pitch, yaw = _rpy(item.quaternion_xyzw)
+            lines.extend([
+                f"    <model name='{item.backend_id}'>",
+                f"      <pose>{_f(x)} {_f(y)} {_f(z)} {_f(roll)} {_f(pitch)} {_f(yaw)}</pose>",
+                "      <link name='body'>",
+            ])
+            if not item.static:
+                if item.mass_kg is None:
+                    raise TwinMaterializationError(f"dynamic_mass_missing:{item.entity_id}")
+                ixx, iyy, izz = _inertia(item, item.mass_kg)
+                lines.extend([
+                    "        <inertial>",
+                    f"          <mass>{_f(item.mass_kg)}</mass>",
+                    "          <inertia>",
+                    f"            <ixx>{_f(ixx)}</ixx><ixy>0</ixy><ixz>0</ixz>",
+                    f"            <iyy>{_f(iyy)}</iyy><iyz>0</iyz><izz>{_f(izz)}</izz>",
+                    "          </inertia>",
+                    "        </inertial>",
+                ])
+            geometry = _sdf_geometry(item)
+            lines.extend([
+                "        <collision name='collision'><geometry>",
+                *["          " + value for value in geometry],
+                "        </geometry>",
+            ])
+            if item.friction is not None:
+                lines.append(
+                    "        <surface><friction><ode>"
+                    f"<mu>{_f(item.friction)}</mu><mu2>{_f(item.friction)}</mu2>"
+                    "</ode></friction></surface>"
+                )
+            lines.extend([
+                "        </collision>",
+                "        <visual name='visual'><geometry>",
+                *["          " + value for value in geometry],
+                "        </geometry></visual>",
+                "      </link>",
+                f"      <static>{str(item.static).lower()}</static>",
+                "    </model>",
+            ])
+        lines.extend(["  </world>", "</sdf>"])
+        return "\n".join(lines) + "\n"
+
+    def write_sdf(self, path: Path | str) -> Path:
+        target = Path(path).expanduser().resolve()
+        target.parent.mkdir(parents=True, exist_ok=True)
+        temporary = target.with_suffix(target.suffix + ".tmp")
+        temporary.write_text(self.to_sdf(), encoding="utf-8")
+        temporary.replace(target)
+        return target
+
+
+@dataclass(frozen=True, slots=True)
+class TwinMaterializationResult:
+    twin_hash: str
+    materialization: CanonicalTwinMaterialization | None
+    issues: tuple[MaterializationIssue, ...]
+
+    @property
+    def ready(self) -> bool:
+        return self.materialization is not None and not self.issues
+
+
+@dataclass(frozen=True, slots=True)
+class _Transform:
+    translation: tuple[float, float, float]
+    quaternion: tuple[float, float, float, float]
+
+
+def _canon_float(value: float) -> float:
+    value = float(value)
+    if abs(value) < 1e-12:
+        return 0.0
+    return float(format(value, ".12g"))
+
+
+def _hash(value: object) -> str:
+    text = json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False)
+    return sha256(text.encode("utf-8")).hexdigest()
+
+
+def _qmul(first, second):
+    ax, ay, az, aw = first
+    bx, by, bz, bw = second
+    return (
+        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 _qconj(quaternion):
+    return (-quaternion[0], -quaternion[1], -quaternion[2], quaternion[3])
+
+
+def _qrot(quaternion, vector):
+    output = _qmul(_qmul(quaternion, (vector[0], vector[1], vector[2], 0.0)), _qconj(quaternion))
+    return output[:3]
+
+
+def _compose(first: _Transform, second: _Transform) -> _Transform:
+    rotated = _qrot(first.quaternion, second.translation)
+    return _Transform(
+        tuple(first.translation[index] + rotated[index] for index in range(3)),
+        _qmul(first.quaternion, second.quaternion),
+    )
+
+
+def _inverse(value: _Transform) -> _Transform:
+    quaternion = _qconj(value.quaternion)
+    return _Transform(_qrot(quaternion, tuple(-component for component in value.translation)), quaternion)
+
+
+def _pose(value: Pose3D) -> _Transform:
+    cr, sr = cos(value.roll_rad / 2), sin(value.roll_rad / 2)
+    cp, sp = cos(value.pitch_rad / 2), sin(value.pitch_rad / 2)
+    cy, sy = cos(value.yaw_rad / 2), sin(value.yaw_rad / 2)
+    quaternion = (
+        sr * cp * cy - cr * sp * sy,
+        cr * sp * cy + sr * cp * sy,
+        cr * cp * sy - sr * sp * cy,
+        cr * cp * cy + sr * sp * sy,
+    )
+    return _Transform((float(value.x_m), float(value.y_m), float(value.z_m)), quaternion)
+
+
+def _canonical_quaternion(quaternion):
+    canonical = tuple(_canon_float(value) for value in quaternion)
+    return tuple(-value for value in canonical) if canonical[3] < 0 else canonical
+
+
+def _rpy(quaternion):
+    x, y, z, w = quaternion
+    return (
+        atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
+        asin(max(-1.0, min(1.0, 2 * (w * y - z * x)))),
+        atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)),
+    )
+
+
+def _backend_id(entity_id: str) -> str:
+    slug = re.sub(r"[^A-Za-z0-9_]+", "_", entity_id).strip("_").lower()[:32] or "entity"
+    return f"devagent_{slug}_{sha256(entity_id.encode('utf-8')).hexdigest()[:10]}"
+
+
+def _numeric(value: object) -> float | None:
+    if value is None or isinstance(value, bool) or not isinstance(value, (int, float)):
+        return None
+    output = float(value)
+    return output if isfinite(output) else None
+
+
+def _f(value: float) -> str:
+    return format(float(value), ".12g")
+
+
+def _sdf_geometry(item: MaterializedObject) -> list[str]:
+    if item.kind == GeometryKind.BOX.value:
+        x, y, z = item.dimensions_m
+        return [f"<box><size>{_f(x)} {_f(y)} {_f(z)}</size></box>"]
+    radius, height = item.dimensions_m
+    return [f"<cylinder><radius>{_f(radius)}</radius><length>{_f(height)}</length></cylinder>"]
+
+
+def _inertia(item: MaterializedObject, mass: float):
+    if item.kind == GeometryKind.BOX.value:
+        x, y, z = item.dimensions_m
+        return (
+            mass * (y * y + z * z) / 12,
+            mass * (x * x + z * z) / 12,
+            mass * (x * x + y * y) / 12,
+        )
+    radius, height = item.dimensions_m
+    transverse = mass * (3 * radius * radius + height * height) / 12
+    return transverse, transverse, mass * radius * radius / 2
+
+
+class TwinMaterializer:
+    """Resolve the engineering Twin once for both physics and planning backends."""
+
+    def __init__(self, *, planning_frame: str = "base_link") -> None:
+        if not planning_frame.strip():
+            raise ValueError("planning_frame_required")
+        self.planning_frame = planning_frame
+
+    def compile(self, twin: TwinSpec) -> TwinMaterializationResult:
+        if twin.robot_base_pose is None:
+            return TwinMaterializationResult(
+                twin.fingerprint,
+                None,
+                (MaterializationIssue("robot_base_pose_missing", "Resolved robot base pose is required."),),
+            )
+
+        issues: list[MaterializationIssue] = []
+        frame_specs = {frame.child_frame_id: frame for frame in twin.frames}
+        cache: dict[str, _Transform] = {
+            "world": _Transform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))
+        }
+        visiting: set[str] = set()
+
+        def resolve_frame(frame_id: str) -> _Transform:
+            if frame_id in cache:
+                return cache[frame_id]
+            if frame_id in visiting:
+                raise TwinMaterializationError(f"frame_cycle:{frame_id}")
+            spec = frame_specs.get(frame_id)
+            if spec is None:
+                raise TwinMaterializationError(f"unknown_frame:{frame_id}")
+            visiting.add(frame_id)
+            try:
+                output = _compose(resolve_frame(spec.parent_frame_id), _pose(spec.transform))
+            finally:
+                visiting.remove(frame_id)
+            cache[frame_id] = output
+            return output
+
+        def world_pose(pose: Pose3D) -> _Transform:
+            return _compose(resolve_frame(pose.frame_id), _pose(pose))
+
+        try:
+            base_from_world = _inverse(world_pose(twin.robot_base_pose))
+        except TwinMaterializationError as exc:
+            return TwinMaterializationResult(
+                twin.fingerprint,
+                None,
+                (MaterializationIssue(str(exc).split(":", 1)[0], str(exc)),),
+            )
+
+        objects: list[MaterializedObject] = []
+        for entity in twin.entities:
+            if not entity.enabled or not entity.geometry.collision_geometry:
+                continue
+            if entity.pose is None:
+                issues.append(MaterializationIssue("entity_pose_missing", "Collision entity has no pose.", entity.entity_id))
+                continue
+            if entity.geometry.kind not in {GeometryKind.BOX, GeometryKind.CYLINDER}:
+                issues.append(MaterializationIssue(
+                    "geometry_not_materializable_v1",
+                    "Only verified box/cylinder primitives are materialized; mesh/CAD requires asset qualification.",
+                    entity.entity_id,
+                ))
+                continue
+            try:
+                backend_pose = _compose(base_from_world, world_pose(entity.pose))
+            except TwinMaterializationError as exc:
+                issues.append(MaterializationIssue(str(exc).split(":", 1)[0], str(exc), entity.entity_id))
+                continue
+
+            is_workpiece = entity.entity_id == twin.workpiece_entity_id
+            mass = _numeric(entity.physics.mass_kg.value)
+            friction = _numeric(entity.physics.friction_coefficient.value)
+            if is_workpiece and mass is None:
+                issues.append(MaterializationIssue("dynamic_workpiece_mass_missing", "Dynamic workpiece mass evidence is required.", entity.entity_id))
+                continue
+            if is_workpiece and friction is None:
+                issues.append(MaterializationIssue("dynamic_workpiece_friction_missing", "Dynamic workpiece friction evidence is required.", entity.entity_id))
+                continue
+
+            dimensions = tuple(float(value) for value in entity.geometry.dimensions_m)
+            if any(not isfinite(value) or value <= 0 for value in dimensions):
+                issues.append(MaterializationIssue("invalid_geometry_dimensions", "Geometry dimensions must be finite and positive.", entity.entity_id))
+                continue
+
+            objects.append(MaterializedObject(
+                entity_id=entity.entity_id,
+                backend_id=_backend_id(entity.entity_id),
+                kind=entity.geometry.kind.value,
+                dimensions_m=dimensions,
+                position_m=tuple(_canon_float(value) for value in backend_pose.translation),
+                quaternion_xyzw=_canonical_quaternion(backend_pose.quaternion),
+                static=not is_workpiece,
+                mass_kg=mass,
+                friction=friction,
+                restitution=_numeric(entity.physics.restitution.value),
+            ))
+
+        if not objects:
+            issues.append(MaterializationIssue("collision_environment_empty", "At least one collision object must materialize."))
+        if issues:
+            return TwinMaterializationResult(twin.fingerprint, None, tuple(issues))
+
+        materialization = CanonicalTwinMaterialization(
+            twin_id=twin.twin_id,
+            twin_hash=twin.fingerprint,
+            robot_profile_key=twin.robot_profile_key,
+            planning_frame=self.planning_frame,
+            objects=tuple(sorted(objects, key=lambda item: item.backend_id)),
+        )
+        return TwinMaterializationResult(twin.fingerprint, materialization, ())

devagent_physical_engine/verification.py

@@ -0,0 +1,46 @@
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+from .models import Resource, TaskGraph, TaskNode, WorldState
+
+
+@dataclass(frozen=True, slots=True)
+class VerificationIssue:
+    code: str
+    task_id: str | None = None
+    detail: str = ""
+
+
+@dataclass(slots=True)
+class VerificationResult:
+    passed: bool
+    issues: list[VerificationIssue] = field(default_factory=list)
+
+
+class DeterministicVerifier:
+    def verify_graph(self, graph: TaskGraph, resources: list[Resource]) -> VerificationResult:
+        issues = [VerificationIssue(error) for error in graph.validate_structure()]
+        resource_map = {resource.resource_id: resource for resource in resources}
+        try:
+            ordered = graph.topological_order()
+        except ValueError as exc:
+            issues.append(VerificationIssue("invalid_task_graph", detail=str(exc)))
+            return VerificationResult(False, issues)
+        for task in ordered:
+            resource = resource_map.get(task.contract.resource_id)
+            if resource is None:
+                issues.append(VerificationIssue("unknown_resource", task.task_id, task.contract.resource_id))
+            elif not resource.supports(task.contract.action):
+                issues.append(VerificationIssue("unsupported_capability", task.task_id, task.contract.action.value))
+        return VerificationResult(not issues, issues)
+
+    def verify_runtime_preconditions(self, task: TaskNode, world: WorldState) -> VerificationResult:
+        missing = [item for item in task.contract.preconditions if item not in world.facts]
+        issues = [VerificationIssue("missing_precondition", task.task_id, item) for item in missing]
+        return VerificationResult(not issues, issues)
+
+    def verify_simulation_constraints(self, task: TaskNode, satisfied_constraints: set[str]) -> VerificationResult:
+        missing = [item for item in task.contract.constraints if item not in satisfied_constraints]
+        issues = [VerificationIssue("unsatisfied_constraint", task.task_id, item) for item in missing]
+        return VerificationResult(not issues, issues)