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

devagent_physical_engine/ros2/state_validity_probe.py

@@ -0,0 +1,125 @@
+from __future__ import annotations
+
+import argparse
+from hashlib import sha256
+import json
+from math import ceil
+from pathlib import Path
+from typing import Any
+
+
+def _hash(payload: object) -> str:
+    text = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False)
+    return sha256(text.encode("utf-8")).hexdigest()
+
+
+def _sample_motion(motion: dict[str, Any], *, max_joint_step_rad: float, max_samples: int) -> list[tuple[float, ...]]:
+    points = motion["points"]
+    samples: list[tuple[float, ...]] = []
+    for index in range(len(points) - 1):
+        start = tuple(float(value) for value in points[index]["positions_rad"])
+        end = tuple(float(value) for value in points[index + 1]["positions_rad"])
+        if len(start) != len(end):
+            raise ValueError("trajectory_joint_shape_mismatch")
+        maximum = max(abs(first - second) for first, second in zip(start, end))
+        steps = max(1, int(ceil(maximum / max_joint_step_rad)))
+        for step in range(steps):
+            ratio = step / steps
+            sample = tuple(first + (second - first) * ratio for first, second in zip(start, end))
+            if not samples or sample != samples[-1]:
+                samples.append(sample)
+                if len(samples) > max_samples:
+                    raise ValueError("preexecution_sample_limit_exceeded")
+    final = tuple(float(value) for value in points[-1]["positions_rad"])
+    if not samples or final != samples[-1]:
+        samples.append(final)
+    if len(samples) > max_samples:
+        raise ValueError("preexecution_sample_limit_exceeded")
+    return samples
+
+
+def check(path: Path, *, timeout_s: float, max_joint_step_rad: float, max_samples: int, group_name: str) -> dict[str, Any]:
+    payload = json.loads(path.read_text(encoding="utf-8"))
+    motion = payload["motion"]
+    joint_names = tuple(str(value) for value in motion["joint_names"])
+    samples = _sample_motion(motion, max_joint_step_rad=max_joint_step_rad, max_samples=max_samples)
+
+    import rclpy
+    from moveit_msgs.srv import GetStateValidity
+
+    rclpy.init()
+    node = rclpy.create_node("devagent_state_validity_probe")
+    client = node.create_client(GetStateValidity, "/check_state_validity")
+    try:
+        if not client.wait_for_service(timeout_sec=timeout_s):
+            return {"success": False, "code": "state_validity_service_unavailable", "sample_count": 0}
+        checked = 0
+        for index, positions in enumerate(samples):
+            request = GetStateValidity.Request()
+            request.group_name = group_name
+            request.robot_state.is_diff = True
+            request.robot_state.joint_state.name = list(joint_names)
+            request.robot_state.joint_state.position = list(positions)
+            future = client.call_async(request)
+            rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_s)
+            if not future.done() or future.result() is None:
+                return {"success": False, "code": "state_validity_timeout", "sample_count": checked, "first_invalid": index}
+            response = future.result()
+            checked += 1
+            if not response.valid:
+                evidence = {"index": index, "positions_rad": list(positions), "contact_count": len(response.contacts)}
+                return {
+                    "success": False,
+                    "code": "collision_or_invalid_state",
+                    "sample_count": checked,
+                    "first_invalid": evidence,
+                    "group_name": group_name,
+                    "max_joint_step_rad": max_joint_step_rad,
+                    "evidence_hash": _hash(evidence),
+                }
+        evidence = {
+            "motion_hash": payload["motion_hash"],
+            "materialization_hash": payload["materialization_hash"],
+            "sample_count": checked,
+            "group_name": group_name,
+            "max_joint_step_rad": max_joint_step_rad,
+        }
+        return {
+            "success": True,
+            "code": "sampled_state_validity_pass",
+            "sample_count": checked,
+            "group_name": group_name,
+            "max_joint_step_rad": max_joint_step_rad,
+            "evidence_hash": _hash(evidence),
+        }
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    sub = parser.add_subparsers(dest="command", required=True)
+    command = sub.add_parser("check")
+    command.add_argument("--input", required=True)
+    command.add_argument("--timeout", type=float, default=10.0)
+    command.add_argument("--max-joint-step", type=float, default=0.03)
+    command.add_argument("--max-samples", type=int, default=4000)
+    command.add_argument("--group", default="ur_manipulator")
+    args = parser.parse_args()
+    try:
+        result = check(
+            Path(args.input).expanduser().resolve(),
+            timeout_s=float(args.timeout),
+            max_joint_step_rad=float(args.max_joint_step),
+            max_samples=int(args.max_samples),
+            group_name=str(args.group),
+        )
+    except Exception as exc:
+        result = {"success": False, "code": f"state_validity_probe_error:{type(exc).__name__}", "sample_count": 0}
+    print(json.dumps(result, sort_keys=True))
+    return 0 if result.get("success") else 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

devagent_physical_engine/ros2/tf_probe.py

@@ -0,0 +1,51 @@
+from __future__ import annotations
+
+import argparse
+import json
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--parent", required=True)
+    parser.add_argument("--child", required=True)
+    parser.add_argument("--timeout", type=float, default=10.0)
+    args = parser.parse_args()
+    payload: dict[str, object]
+    try:
+        import rclpy
+        from rclpy.duration import Duration
+        from rclpy.time import Time
+        from tf2_ros import Buffer, TransformListener
+
+        rclpy.init()
+        node = rclpy.create_node("devagent_tf_probe")
+        buffer = Buffer()
+        listener = TransformListener(buffer, node, spin_thread=True)
+        try:
+            transform = buffer.lookup_transform(
+                args.parent,
+                args.child,
+                Time(),
+                timeout=Duration(seconds=float(args.timeout)),
+            )
+            translation = transform.transform.translation
+            rotation = transform.transform.rotation
+            payload = {
+                "success": True,
+                "code": "tf_verified",
+                "parent_frame": args.parent,
+                "child_frame": args.child,
+                "translation_m": [translation.x, translation.y, translation.z],
+                "quaternion_xyzw": [rotation.x, rotation.y, rotation.z, rotation.w],
+            }
+        finally:
+            node.destroy_node()
+            rclpy.shutdown()
+    except Exception as exc:
+        payload = {"success": False, "code": f"tf_probe_error:{type(exc).__name__}"}
+    print(json.dumps(payload, sort_keys=True))
+    return 0 if payload.get("success") else 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

devagent_physical_engine/ros2/trajectory.py

@@ -0,0 +1,188 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+import json
+from math import isfinite
+import re
+from typing import Mapping
+
+from ..physical_motion import JointTrajectoryPoint, PhysicalMotionPlan
+from .commands import CommandResult, RosCommandRunner
+
+
+class RosTrajectoryError(RuntimeError):
+    pass
+
+
+@dataclass(frozen=True, slots=True)
+class JointStateSnapshot:
+    names: tuple[str, ...]
+    positions_rad: tuple[float, ...]
+
+    def __post_init__(self) -> None:
+        if not self.names or len(self.names) != len(self.positions_rad):
+            raise RosTrajectoryError("joint_state_shape_invalid")
+        if len(set(self.names)) != len(self.names):
+            raise RosTrajectoryError("joint_state_duplicate_name")
+        if any(not name.strip() for name in self.names):
+            raise RosTrajectoryError("joint_state_empty_name")
+        if any(not isfinite(value) for value in self.positions_rad):
+            raise RosTrajectoryError("joint_state_non_finite")
+
+    def ordered(self, joint_names: tuple[str, ...]) -> tuple[float, ...]:
+        values = dict(zip(self.names, self.positions_rad))
+        missing = [name for name in joint_names if name not in values]
+        if missing:
+            raise RosTrajectoryError(
+                "joint_state_missing:" + ",".join(sorted(missing))
+            )
+        return tuple(values[name] for name in joint_names)
+
+
+def read_joint_state(
+    runner: RosCommandRunner,
+    *,
+    topic: str = "/joint_states",
+    timeout_s: float = 8.0,
+) -> JointStateSnapshot:
+    result = runner.run(
+        ["ros2", "topic", "echo", topic, "--once"],
+        timeout_s=timeout_s,
+    )
+    if not result.ok:
+        raise RosTrajectoryError(
+            "joint_state_snapshot_failed:"
+            + (result.stderr.strip()[-300:] or "ros2_topic_echo_failed")
+        )
+    try:
+        import yaml
+
+        payload = yaml.safe_load(result.stdout)
+        if not isinstance(payload, Mapping):
+            raise TypeError("joint state payload is not a mapping")
+        names_raw = payload.get("name")
+        positions_raw = payload.get("position")
+        if not isinstance(names_raw, list) or not isinstance(positions_raw, list):
+            raise TypeError("name/position fields are not arrays")
+        return JointStateSnapshot(
+            tuple(str(item) for item in names_raw),
+            tuple(float(item) for item in positions_raw),
+        )
+    except Exception as exc:
+        raise RosTrajectoryError("joint_state_snapshot_invalid") from exc
+
+
+def _duration_payload(seconds: float) -> dict[str, int]:
+    whole = int(seconds)
+    nanos = int(round((seconds - whole) * 1_000_000_000))
+    if nanos >= 1_000_000_000:
+        whole += 1
+        nanos -= 1_000_000_000
+    return {"sec": whole, "nanosec": nanos}
+
+
+def _point_payload(point: JointTrajectoryPoint) -> dict:
+    payload = {
+        "positions": list(point.positions_rad),
+        "time_from_start": _duration_payload(point.time_from_start_s),
+    }
+    if point.velocities_rad_s:
+        payload["velocities"] = list(point.velocities_rad_s)
+    if point.accelerations_rad_s2:
+        payload["accelerations"] = list(point.accelerations_rad_s2)
+    return payload
+
+
+def follow_joint_trajectory_command(
+    motion: PhysicalMotionPlan,
+    *,
+    action_name: str = "/joint_trajectory_controller/follow_joint_trajectory",
+) -> tuple[str, ...]:
+    if not action_name.strip().startswith("/"):
+        raise RosTrajectoryError("trajectory_action_must_be_absolute")
+    points = motion.points[1:]
+    goal = {
+        "trajectory": {
+            "joint_names": list(motion.joint_names),
+            "points": [_point_payload(point) for point in points],
+        }
+    }
+    return (
+        "ros2",
+        "action",
+        "send_goal",
+        action_name,
+        "control_msgs/action/FollowJointTrajectory",
+        json.dumps(goal, separators=(",", ":"), allow_nan=False),
+    )
+
+
+def single_point_motion(
+    *,
+    motion_id: str,
+    robot_profile_key: str,
+    joint_names: tuple[str, ...],
+    current_positions_rad: tuple[float, ...],
+    target_positions_rad: tuple[float, ...],
+    duration_s: float,
+    source_graph_hash: str,
+    twin_hash: str,
+    planner_id: str = "devagent-reset",
+) -> PhysicalMotionPlan:
+    if duration_s <= 0:
+        raise RosTrajectoryError("reset_duration_must_be_positive")
+    return PhysicalMotionPlan(
+        motion_id=motion_id,
+        robot_profile_key=robot_profile_key,
+        joint_names=joint_names,
+        points=(
+            JointTrajectoryPoint(current_positions_rad, 0.0),
+            JointTrajectoryPoint(target_positions_rad, duration_s),
+        ),
+        source_graph_hash=source_graph_hash,
+        planner_id=planner_id,
+        twin_hash=twin_hash,
+        constraints=("simulation_reset_only",),
+    )
+
+
+def _confirm_successful_action_output(output: str) -> None:
+    normalized = output.lower()
+    if "goal rejected" in normalized:
+        raise RosTrajectoryError("trajectory_goal_rejected")
+    for failure in ("status: aborted", "status: canceled", "status: cancelled"):
+        if failure in normalized:
+            raise RosTrajectoryError("trajectory_action_not_succeeded")
+
+    error_codes = [int(value) for value in re.findall(r"error_code\s*[=:]\s*(-?\d+)", normalized)]
+    if any(code != 0 for code in error_codes):
+        raise RosTrajectoryError("trajectory_result_error_code")
+
+    success_markers = (
+        "status: succeeded",
+        "goal finished with status: succeeded",
+        "successful",
+    )
+    if error_codes and all(code == 0 for code in error_codes):
+        return
+    if any(marker in normalized for marker in success_markers):
+        return
+    raise RosTrajectoryError("trajectory_result_unconfirmed")
+
+
+def execute_joint_trajectory(
+    runner: RosCommandRunner,
+    motion: PhysicalMotionPlan,
+    *,
+    action_name: str = "/joint_trajectory_controller/follow_joint_trajectory",
+    timeout_s: float | None = None,
+) -> CommandResult:
+    timeout = timeout_s or max(15.0, motion.planned_duration_s + 15.0)
+    result = runner.run(
+        follow_joint_trajectory_command(motion, action_name=action_name),
+        timeout_s=timeout,
+    )
+    if not result.ok:
+        raise RosTrajectoryError("trajectory_action_failed")
+    _confirm_successful_action_output(result.stdout + "\n" + result.stderr)
+    return result

devagent_physical_engine/ros2/ur5e.py

@@ -0,0 +1,59 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+
+from .commands import RosCommandRunner, RunningProcess
+
+
+UR5E = "ur5e"
+
+
+def official_ur5e_moveit_command(*, world_file: Path | str | None = None) -> tuple[str, ...]:
+    command = [
+        "ros2",
+        "launch",
+        "ur_simulation_gz",
+        "ur_sim_moveit.launch.py",
+        "ur_type:=ur5e",
+    ]
+    if world_file is not None:
+        world = Path(world_file).expanduser().resolve()
+        if not world.is_file():
+            raise FileNotFoundError(f"world_file_not_found:{world}")
+        command.append(f"world_file:={world}")
+    return tuple(command)
+
+
+def official_ur_motion_smoke_command() -> tuple[str, ...]:
+    return ("ros2", "run", "ur_robot_driver", "example_move.py")
+
+
+@dataclass(slots=True)
+class UR5eSimulation:
+    runner: RosCommandRunner
+    process: RunningProcess
+    log_path: Path
+    command: tuple[str, ...]
+
+    @property
+    def running(self) -> bool:
+        return self.process.poll() is None
+
+    def stop(self, *, timeout_s: float = 10.0) -> None:
+        self.runner.stop(self.process, timeout_s=timeout_s)
+
+
+class UR5eSimulationLauncher:
+    def __init__(self, runner: RosCommandRunner) -> None:
+        self.runner = runner
+
+    def launch(
+        self,
+        *,
+        log_path: Path,
+        world_file: Path | str | None = None,
+    ) -> UR5eSimulation:
+        command = official_ur5e_moveit_command(world_file=world_file)
+        process = self.runner.start(command, log_path=log_path)
+        return UR5eSimulation(self.runner, process, log_path, command)