Add IK types, validation, and tests for RM75 kinematics

ik_qp/src/rm75_ik/__init__.py

@@ -0,0 +1,34 @@
+from .dual_arm import DualArmAssembly, DualArmMounts, load_dual_arm_mounts
+from .kinematics import RM75Kinematics, default_urdf_path, pose_errors, validate_se3
+from .realman_reference import RealManFkReference
+from .solver import RM75IkSolver, deterministic_recovery_seeds
+from .types import (
+    IkOptions,
+    IkResult,
+    IkStatus,
+    JointLimits,
+    joint_limit_profile,
+    physical_joint_limits,
+    teleop_joint_limits,
+)
+
+__all__ = [
+    "DualArmAssembly",
+    "DualArmMounts",
+    "IkOptions",
+    "IkResult",
+    "IkStatus",
+    "JointLimits",
+    "RM75IkSolver",
+    "RM75Kinematics",
+    "RealManFkReference",
+    "default_urdf_path",
+    "deterministic_recovery_seeds",
+    "joint_limit_profile",
+    "load_dual_arm_mounts",
+    "physical_joint_limits",
+    "pose_errors",
+    "teleop_joint_limits",
+    "validate_se3",
+]
+

ik_qp/src/rm75_ik/cli.py

@@ -0,0 +1,114 @@
+from __future__ import annotations
+
+import argparse
+import sys
+from pathlib import Path
+from typing import Optional, Sequence
+
+from .realman_reference import RealManFkReference
+from .validation import (
+    Stage1Validator,
+    ValidationSettings,
+    load_project_tools,
+    write_validation_report,
+)
+
+
+def _source_project_root() -> Optional[Path]:
+    candidate = Path(__file__).resolve().parents[3]
+    if (candidate / "xr_rm_bringup").is_dir():
+        return candidate
+    return None
+
+
+def _default_output_dir() -> Path:
+    package_root = Path(__file__).resolve().parents[2]
+    if (package_root / "pyproject.toml").is_file():
+        return package_root / "artifacts" / "stage1"
+    return Path.cwd() / "stage1_artifacts"
+
+
+def _default_tools_config() -> Optional[Path]:
+    root = _source_project_root()
+    if root is None:
+        return None
+    candidate = root / "xr_rm_bringup" / "config" / "peripherals_rm75.yaml"
+    return candidate if candidate.is_file() else None
+
+
+def build_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(
+        description="Offline RM75-B stage-1 kinematics and QP IK validation"
+    )
+    parser.add_argument(
+        "--sdk-root",
+        type=Path,
+        help="directory containing the RealMan Robotic_Arm Python package",
+    )
+    parser.add_argument(
+        "--tools-config",
+        type=Path,
+        default=_default_tools_config(),
+        help="peripherals_rm75.yaml used for tool-frame FK checks",
+    )
+    parser.add_argument(
+        "--skip-tools",
+        action="store_true",
+        help="skip project tool-frame verification",
+    )
+    parser.add_argument(
+        "--output-dir",
+        type=Path,
+        default=_default_output_dir(),
+        help="directory for JSON, CSV and Markdown reports",
+    )
+    parser.add_argument("--seed", type=int, default=20260629)
+    parser.add_argument(
+        "--quick",
+        action="store_true",
+        help="run a small smoke-validation sample set",
+    )
+    parser.add_argument(
+        "--report-only",
+        action="store_true",
+        help="always return exit code zero while preserving failed checks in reports",
+    )
+    return parser
+
+
+def main(argv: Optional[Sequence[str]] = None) -> int:
+    args = build_parser().parse_args(argv)
+    settings = (
+        ValidationSettings.quick(seed=args.seed, strict=not args.report_only)
+        if args.quick
+        else ValidationSettings(seed=args.seed, strict=not args.report_only)
+    )
+    tools = {}
+    if not args.skip_tools:
+        if args.tools_config is None:
+            raise SystemExit(
+                "tool validation requested but peripherals_rm75.yaml was not found; "
+                "pass --tools-config or --skip-tools"
+            )
+        tools = load_project_tools(args.tools_config)
+
+    reference = RealManFkReference(args.sdk_root)
+    validator = Stage1Validator(reference, settings, tools)
+    summary = validator.run()
+    paths = write_validation_report(args.output_dir, summary, validator.failures)
+
+    result_text = "PASS" if summary["passed"] else "FAIL"
+    print(f"RM75-B stage-1 validation: {result_text}")
+    for name, check in summary["checks"].items():
+        print(f"  [{'PASS' if check['passed'] else 'FAIL'}] {name}")
+    print("Reports:")
+    for path in paths:
+        print(f"  {path}")
+    if args.report_only or summary["passed"]:
+        return 0
+    return 1
+
+
+if __name__ == "__main__":
+    sys.exit(main())
+

ik_qp/src/rm75_ik/dual_arm.py

@@ -0,0 +1,132 @@
+from __future__ import annotations
+
+import sysconfig
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Optional
+
+import numpy as np
+import pinocchio as pin
+
+from .kinematics import RM75Kinematics
+from .types import JointLimits, physical_joint_limits
+
+
+def default_dual_source_path() -> Path:
+    source_path = (
+        Path(__file__).resolve().parents[2]
+        / "models"
+        / "dual_arm_mujoco_fixed.urdf"
+    )
+    if source_path.is_file():
+        return source_path
+    installed_path = (
+        Path(sysconfig.get_path("data"))
+        / "share"
+        / "rm75_ik"
+        / "models"
+        / "dual_arm_mujoco_fixed.urdf"
+    )
+    if installed_path.is_file():
+        return installed_path
+    raise FileNotFoundError("dual_arm_mujoco_fixed.urdf was not found")
+
+
+def _origin_to_se3(element: ET.Element) -> pin.SE3:
+    origin = element.find("origin")
+    if origin is None:
+        return pin.SE3.Identity()
+    xyz = np.fromstring(origin.get("xyz", "0 0 0"), sep=" ", dtype=float)
+    rpy = np.fromstring(origin.get("rpy", "0 0 0"), sep=" ", dtype=float)
+    if xyz.shape != (3,) or rpy.shape != (3,):
+        raise ValueError(f"invalid URDF origin on element {element.get('name')!r}")
+    return pin.SE3(pin.rpy.rpyToMatrix(*rpy), xyz)
+
+
+@dataclass(frozen=True)
+class DualArmMounts:
+    left_base: pin.SE3
+    right_base: pin.SE3
+    right_visual_origin_delta_m: float
+
+
+def load_dual_arm_mounts(source_urdf: Optional[Path | str] = None) -> DualArmMounts:
+    path = Path(source_urdf) if source_urdf is not None else default_dual_source_path()
+    root = ET.parse(path).getroot()
+    joints = {joint.get("name"): joint for joint in root.findall("joint")}
+    try:
+        world_left_joint1 = _origin_to_se3(joints["joint1"])
+        world_right_joint1 = _origin_to_se3(joints["joint8"])
+    except KeyError as exc:
+        raise ValueError("dual-arm source URDF must contain joint1 and joint8") from exc
+
+    base_to_joint1 = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.2405]))
+    left_base = world_left_joint1 * base_to_joint1.inverse()
+    right_base = world_right_joint1 * base_to_joint1.inverse()
+
+    right_visual = root.find(
+        "./link[@name='robot_base']/visual[@name='base_link_right']"
+    )
+    if right_visual is None:
+        visual_delta = float("nan")
+    else:
+        visual_pose = _origin_to_se3(right_visual)
+        visual_delta = float(
+            np.linalg.norm(right_base.translation - visual_pose.translation)
+        )
+    return DualArmMounts(left_base, right_base, visual_delta)
+
+
+class DualArmAssembly:
+    """Two independent RM75-B chains placed in a common world frame."""
+
+    dof = 14
+
+    def __init__(
+        self,
+        mounts: DualArmMounts,
+        left: RM75Kinematics,
+        right: RM75Kinematics,
+    ) -> None:
+        self.mounts = mounts
+        self._kinematics = {"left": left, "right": right}
+
+    @classmethod
+    def from_source_urdf(
+        cls,
+        source_urdf: Optional[Path | str] = None,
+        limits: Optional[JointLimits] = None,
+    ) -> "DualArmAssembly":
+        selected_limits = limits or physical_joint_limits()
+        return cls(
+            load_dual_arm_mounts(source_urdf),
+            RM75Kinematics(limits=selected_limits),
+            RM75Kinematics(limits=selected_limits),
+        )
+
+    def local_forward(
+        self,
+        arm: str,
+        q_rad: np.ndarray,
+        tool: Optional[pin.SE3] = None,
+    ) -> pin.SE3:
+        try:
+            kinematics = self._kinematics[arm]
+        except KeyError as exc:
+            raise ValueError("arm must be 'left' or 'right'") from exc
+        return kinematics.forward(q_rad, tool)
+
+    def forward(
+        self,
+        arm: str,
+        q_rad: np.ndarray,
+        tool: Optional[pin.SE3] = None,
+    ) -> pin.SE3:
+        local = self.local_forward(arm, q_rad, tool)
+        if arm == "left":
+            return self.mounts.left_base * local
+        if arm == "right":
+            return self.mounts.right_base * local
+        raise ValueError("arm must be 'left' or 'right'")
+

ik_qp/src/rm75_ik/kinematics.py

@@ -0,0 +1,124 @@
+from __future__ import annotations
+
+import sysconfig
+from pathlib import Path
+from typing import Optional, Tuple
+
+import numpy as np
+import pinocchio as pin
+
+from .types import JointLimits, physical_joint_limits
+
+
+EXPECTED_JOINT_NAMES = tuple(f"joint_{index}" for index in range(1, 8))
+FLANGE_FRAME = "link_7"
+
+
+def default_urdf_path() -> Path:
+    source_path = (
+        Path(__file__).resolve().parents[2]
+        / "kine_ctrl"
+        / "urdf_rm75"
+        / "RM75-B.urdf"
+    )
+    if source_path.is_file():
+        return source_path
+    installed_path = (
+        Path(sysconfig.get_path("data"))
+        / "share"
+        / "rm75_ik"
+        / "models"
+        / "RM75-B.urdf"
+    )
+    if installed_path.is_file():
+        return installed_path
+    raise FileNotFoundError("RM75-B.urdf was not found in source or installed data")
+
+
+def validate_se3(value: pin.SE3, name: str = "pose") -> None:
+    if not isinstance(value, pin.SE3):
+        raise TypeError(f"{name} must be pinocchio.SE3")
+    rotation = np.asarray(value.rotation)
+    translation = np.asarray(value.translation)
+    if rotation.shape != (3, 3) or translation.shape != (3,):
+        raise ValueError(f"{name} has invalid dimensions")
+    if not np.all(np.isfinite(rotation)) or not np.all(np.isfinite(translation)):
+        raise ValueError(f"{name} must be finite")
+    if not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-7):
+        raise ValueError(f"{name} rotation must be orthonormal")
+    if not np.isclose(np.linalg.det(rotation), 1.0, atol=1e-7):
+        raise ValueError(f"{name} rotation determinant must be +1")
+
+
+def pose_errors(current: pin.SE3, target: pin.SE3) -> Tuple[float, float]:
+    validate_se3(current, "current")
+    validate_se3(target, "target")
+    position_error = float(np.linalg.norm(current.translation - target.translation))
+    rotation_delta = current.rotation.T @ target.rotation
+    orientation_error = float(np.linalg.norm(pin.log3(rotation_delta)))
+    return position_error, orientation_error
+
+
+class RM75Kinematics:
+    """Pinocchio kinematics for one RM75-B.
+
+    Instances own mutable Pinocchio data and are intentionally not thread-safe.
+    Use one instance per arm/control thread.
+    """
+
+    def __init__(
+        self,
+        urdf_path: Optional[Path | str] = None,
+        limits: Optional[JointLimits] = None,
+    ) -> None:
+        self.urdf_path = Path(urdf_path) if urdf_path is not None else default_urdf_path()
+        if not self.urdf_path.is_file():
+            raise FileNotFoundError(self.urdf_path)
+        self.model = pin.buildModelFromUrdf(str(self.urdf_path))
+        if self.model.nq != 7 or self.model.nv != 7:
+            raise ValueError(
+                f"expected RM75 model nq=nv=7, got nq={self.model.nq}, nv={self.model.nv}"
+            )
+        joint_names = tuple(self.model.names[1:])
+        if joint_names != EXPECTED_JOINT_NAMES:
+            raise ValueError(f"unexpected RM75 joint order: {joint_names}")
+        frame_id = self.model.getFrameId(FLANGE_FRAME)
+        if frame_id >= len(self.model.frames):
+            raise ValueError(f"missing flange frame {FLANGE_FRAME!r}")
+        self.flange_frame_id = frame_id
+        self.limits = limits or physical_joint_limits()
+        self.model.lowerPositionLimit[:7] = self.limits.lower
+        self.model.upperPositionLimit[:7] = self.limits.upper
+        self.data = self.model.createData()
+
+    def validate_q(self, q_rad: np.ndarray, *, require_within_limits: bool = True) -> np.ndarray:
+        q = np.asarray(q_rad, dtype=float)
+        if q.shape != (7,):
+            raise ValueError(f"RM75 configuration must have shape (7,), got {q.shape}")
+        if not np.all(np.isfinite(q)):
+            raise ValueError("RM75 configuration must be finite")
+        if require_within_limits and not self.limits.contains(q):
+            raise ValueError(f"configuration is outside {self.limits.name} joint limits")
+        return q.copy()
+
+    def forward(self, q_rad: np.ndarray, tool: Optional[pin.SE3] = None) -> pin.SE3:
+        q = self.validate_q(q_rad)
+        pin.framesForwardKinematics(self.model, self.data, q)
+        flange = self.data.oMf[self.flange_frame_id]
+        result = pin.SE3(flange.rotation.copy(), flange.translation.copy())
+        if tool is not None:
+            validate_se3(tool, "tool")
+            result = result * tool
+        return result
+
+    def jacobian(self, q_rad: np.ndarray) -> np.ndarray:
+        q = self.validate_q(q_rad)
+        jacobian = pin.computeFrameJacobian(
+            self.model,
+            self.data,
+            q,
+            self.flange_frame_id,
+            pin.ReferenceFrame.LOCAL,
+        )
+        return np.asarray(jacobian).copy()
+

ik_qp/src/rm75_ik/realman_reference.py

@@ -0,0 +1,95 @@
+from __future__ import annotations
+
+import importlib
+import os
+import sys
+from pathlib import Path
+from typing import Optional
+
+import numpy as np
+import pinocchio as pin
+
+from .kinematics import validate_se3
+
+
+class RealManFkReference:
+    """Offline RealMan Algo FK reference; this class never opens a robot connection."""
+
+    def __init__(self, sdk_root: Optional[Path | str] = None) -> None:
+        selected_root = sdk_root or os.environ.get("REALMAN_SDK_ROOT")
+        if selected_root is not None:
+            root = Path(selected_root).expanduser().resolve()
+            if not (root / "Robotic_Arm").is_dir():
+                raise FileNotFoundError(
+                    f"RealMan SDK root must contain Robotic_Arm/: {root}"
+                )
+            root_text = str(root)
+            if root_text not in sys.path:
+                sys.path.insert(0, root_text)
+        try:
+            module = importlib.import_module("Robotic_Arm.rm_robot_interface")
+            ctypes_module = importlib.import_module("Robotic_Arm.rm_ctypes_wrap")
+        except ImportError as exc:
+            raise ImportError(
+                "RealMan API2 Python SDK is unavailable; set REALMAN_SDK_ROOT "
+                "or pass sdk_root"
+            ) from exc
+
+        self._rm_frame_t = module.rm_frame_t
+        self._algo = module.Algo(
+            module.rm_robot_arm_model_e.RM_MODEL_RM_75_E,
+            module.rm_force_type_e.RM_MODEL_RM_B_E,
+        )
+        self.api_version = str(ctypes_module.rm_api_version())
+        self._active_tool_key: tuple[float, ...] | None = None
+        self._set_work_frame_identity()
+        self._set_tool_frame(None)
+
+    def _set_work_frame_identity(self) -> None:
+        frame = self._rm_frame_t(
+            frame_name="s1_work",
+            pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+            payload=0.0,
+            x=0.0,
+            y=0.0,
+            z=0.0,
+        )
+        self._algo.rm_algo_set_workframe(frame)
+
+    def _set_tool_frame(self, tool: Optional[pin.SE3]) -> None:
+        if tool is None:
+            pose = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        else:
+            validate_se3(tool, "tool")
+            rpy = pin.rpy.matrixToRpy(tool.rotation)
+            pose = tuple(float(value) for value in (*tool.translation, *rpy))
+        key = tuple(round(value, 12) for value in pose)
+        if key == self._active_tool_key:
+            return
+        frame = self._rm_frame_t(
+            frame_name="s1_tool",
+            pose=pose,
+            payload=0.0,
+            x=0.0,
+            y=0.0,
+            z=0.0,
+        )
+        self._algo.rm_algo_set_toolframe(frame)
+        self._active_tool_key = key
+
+    def forward(self, q_rad: np.ndarray, tool: Optional[pin.SE3] = None) -> pin.SE3:
+        q = np.asarray(q_rad, dtype=float)
+        if q.shape != (7,) or not np.all(np.isfinite(q)):
+            raise ValueError("RealMan FK configuration must be a finite shape-(7,) vector")
+        self._set_tool_frame(tool)
+        pose = self._algo.rm_algo_forward_kinematics(np.rad2deg(q).tolist(), flag=0)
+        if len(pose) != 7 or not np.all(np.isfinite(pose)):
+            raise RuntimeError(f"RealMan Algo returned an invalid FK pose: {pose!r}")
+        quaternion_values = np.asarray(pose[3:7], dtype=float)
+        norm = float(np.linalg.norm(quaternion_values))
+        if norm <= 0.0:
+            raise RuntimeError("RealMan Algo returned a zero quaternion")
+        qw, qx, qy, qz = quaternion_values / norm
+        quaternion = pin.Quaternion(qw, qx, qy, qz)
+        return pin.SE3(quaternion.matrix(), np.asarray(pose[:3], dtype=float))
+

ik_qp/src/rm75_ik/solver.py

@@ -0,0 +1,252 @@
+from __future__ import annotations
+
+from dataclasses import replace
+from time import perf_counter
+from typing import Iterable, List
+
+import numpy as np
+import osqp
+import pinocchio as pin
+from scipy import sparse
+
+from .kinematics import RM75Kinematics, pose_errors, validate_se3
+from .types import IkOptions, IkResult, IkStatus, JointLimits
+
+
+class RM75IkSolver:
+    """Single-seed differential IK solved with a reused OSQP workspace."""
+
+    def __init__(self, kinematics: RM75Kinematics) -> None:
+        self.kinematics = kinematics
+        self.model = kinematics.model
+        self.data = kinematics.data
+        self.frame_id = kinematics.flange_frame_id
+        self._n = 7
+
+        pattern = sparse.triu(np.ones((self._n, self._n)), format="csc")
+        self._p_rows = pattern.indices.copy()
+        self._p_cols = np.repeat(np.arange(self._n), np.diff(pattern.indptr))
+        constraints = sparse.eye(self._n, format="csc")
+        self._osqp = osqp.OSQP()
+        self._osqp.setup(
+            P=pattern,
+            q=np.zeros(self._n),
+            A=constraints,
+            l=-np.ones(self._n),
+            u=np.ones(self._n),
+            verbose=False,
+            warm_start=True,
+            polish=False,
+            eps_abs=1e-6,
+            eps_rel=1e-6,
+            max_iter=1000,
+        )
+
+    def solve(
+        self,
+        target_se3: pin.SE3,
+        seed_rad: np.ndarray,
+        options: IkOptions = IkOptions(),
+    ) -> IkResult:
+        started = perf_counter()
+        try:
+            validate_se3(target_se3, "target_se3")
+            q = self.kinematics.validate_q(seed_rad)
+        except (TypeError, ValueError) as exc:
+            return IkResult(
+                IkStatus.INVALID_INPUT,
+                None,
+                float("inf"),
+                float("inf"),
+                0,
+                perf_counter() - started,
+                message=str(exc),
+            )
+
+        q_reference = q.copy()
+        weights = np.diag(np.asarray(options.task_weights, dtype=float))
+        damping = options.damping_initial
+        previous_error = float("inf")
+        best_error = float("inf")
+        stagnant_iterations = 0
+        last_osqp_status = ""
+        position_error = float("inf")
+        orientation_error = float("inf")
+
+        for iteration in range(options.max_iterations + 1):
+            elapsed = perf_counter() - started
+            if options.time_limit_sec is not None and elapsed >= options.time_limit_sec:
+                return IkResult(
+                    IkStatus.TIME_LIMIT,
+                    None,
+                    position_error,
+                    orientation_error,
+                    iteration,
+                    elapsed,
+                    last_osqp_status,
+                    "IK time budget exhausted",
+                )
+
+            pin.computeJointJacobians(self.model, self.data, q)
+            pin.framesForwardKinematics(self.model, self.data, q)
+            current = self.data.oMf[self.frame_id]
+            position_error, orientation_error = pose_errors(current, target_se3)
+            if (
+                position_error <= options.position_tolerance_m
+                and orientation_error <= options.orientation_tolerance_rad
+            ):
+                solution = q.copy()
+                solution.setflags(write=False)
+                return IkResult(
+                    IkStatus.SUCCESS,
+                    solution,
+                    position_error,
+                    orientation_error,
+                    iteration,
+                    perf_counter() - started,
+                    last_osqp_status,
+                )
+
+            if iteration == options.max_iterations:
+                break
+
+            error_transform = current.actInv(target_se3)
+            error_vector = pin.log6(error_transform).vector
+            error_norm = float(np.linalg.norm(error_vector))
+            if error_norm < best_error - options.stagnation_delta:
+                best_error = error_norm
+                stagnant_iterations = 0
+            else:
+                stagnant_iterations += 1
+                if stagnant_iterations >= options.stagnation_iterations:
+                    return IkResult(
+                        IkStatus.STAGNATED,
+                        None,
+                        position_error,
+                        orientation_error,
+                        iteration,
+                        perf_counter() - started,
+                        last_osqp_status,
+                        "SE(3) error stopped improving",
+                    )
+
+            if error_norm > previous_error * 1.1 and iteration > 10:
+                damping = min(options.damping_max, damping * 1.5)
+            else:
+                damping = max(options.damping_min, damping * options.damping_reduction)
+
+            jacobian = pin.getFrameJacobian(
+                self.model,
+                self.data,
+                self.frame_id,
+                pin.ReferenceFrame.LOCAL,
+            )
+            effective_jacobian = pin.Jlog6(error_transform) @ jacobian
+            hessian = effective_jacobian.T @ weights @ effective_jacobian
+            hessian += (
+                damping * damping + options.posture_weight
+            ) * np.eye(self._n)
+            gradient = -effective_jacobian.T @ weights @ error_vector
+            gradient += options.posture_weight * (q - q_reference)
+
+            lower = np.maximum(
+                -options.trust_region_rad,
+                self.kinematics.limits.lower - q,
+            )
+            upper = np.minimum(
+                options.trust_region_rad,
+                self.kinematics.limits.upper - q,
+            )
+            p_values = hessian[self._p_rows, self._p_cols]
+            self._osqp.update(Px=p_values, q=gradient, l=lower, u=upper)
+            osqp_result = self._osqp.solve()
+            last_osqp_status = str(osqp_result.info.status)
+            if last_osqp_status.lower() != "solved" or osqp_result.x is None:
+                return IkResult(
+                    IkStatus.OSQP_FAILURE,
+                    None,
+                    position_error,
+                    orientation_error,
+                    iteration,
+                    perf_counter() - started,
+                    last_osqp_status,
+                    "OSQP did not return a solved step",
+                )
+            step = np.asarray(osqp_result.x, dtype=float)
+            if step.shape != (7,) or not np.all(np.isfinite(step)):
+                return IkResult(
+                    IkStatus.OSQP_FAILURE,
+                    None,
+                    position_error,
+                    orientation_error,
+                    iteration,
+                    perf_counter() - started,
+                    last_osqp_status,
+                    "OSQP returned a non-finite step",
+                )
+            q = pin.integrate(self.model, q, step)
+            q = np.clip(
+                q,
+                self.kinematics.limits.lower,
+                self.kinematics.limits.upper,
+            )
+            previous_error = error_norm
+
+        return IkResult(
+            IkStatus.MAX_ITERATIONS,
+            None,
+            position_error,
+            orientation_error,
+            options.max_iterations,
+            perf_counter() - started,
+            last_osqp_status,
+            "maximum IK iterations reached",
+        )
+
+    def solve_multistart(
+        self,
+        target_se3: pin.SE3,
+        seeds_rad: Iterable[np.ndarray],
+        options: IkOptions = IkOptions(),
+    ) -> IkResult:
+        started = perf_counter()
+        last_result: IkResult | None = None
+        for index, seed in enumerate(seeds_rad, start=1):
+            result = self.solve(target_se3, seed, options)
+            if result.success:
+                return replace(
+                    result,
+                    solve_time_sec=perf_counter() - started,
+                    message=f"converged from recovery seed {index}",
+                )
+            last_result = result
+        if last_result is None:
+            return IkResult(
+                IkStatus.INVALID_INPUT,
+                None,
+                float("inf"),
+                float("inf"),
+                0,
+                perf_counter() - started,
+                message="no recovery seeds were provided",
+            )
+        return replace(
+            last_result,
+            solve_time_sec=perf_counter() - started,
+            message=f"all recovery seeds failed; last error: {last_result.message}",
+        )
+
+
+def deterministic_recovery_seeds(
+    limits: JointLimits,
+    count: int = 8,
+    random_seed: int = 75,
+) -> List[np.ndarray]:
+    if count <= 0:
+        raise ValueError("recovery seed count must be positive")
+    seeds = [np.clip(np.zeros(7), limits.lower, limits.upper)]
+    rng = np.random.default_rng(random_seed)
+    while len(seeds) < count:
+        seeds.append(rng.uniform(limits.lower, limits.upper))
+    return seeds
+

ik_qp/src/rm75_ik/types.py

@@ -0,0 +1,133 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from enum import Enum
+from math import radians
+from typing import Optional, Tuple
+
+import numpy as np
+
+
+class IkStatus(str, Enum):
+    SUCCESS = "success"
+    INVALID_INPUT = "invalid_input"
+    OSQP_FAILURE = "osqp_failure"
+    STAGNATED = "stagnated"
+    TIME_LIMIT = "time_limit"
+    MAX_ITERATIONS = "max_iterations"
+
+
+@dataclass(frozen=True)
+class JointLimits:
+    name: str
+    lower: np.ndarray
+    upper: np.ndarray
+
+    def __post_init__(self) -> None:
+        lower = np.asarray(self.lower, dtype=float).copy()
+        upper = np.asarray(self.upper, dtype=float).copy()
+        if lower.shape != (7,) or upper.shape != (7,):
+            raise ValueError("RM75 joint limits must each have shape (7,)")
+        if not np.all(np.isfinite(lower)) or not np.all(np.isfinite(upper)):
+            raise ValueError("joint limits must be finite")
+        if np.any(lower >= upper):
+            raise ValueError("every lower joint limit must be below its upper limit")
+        lower.setflags(write=False)
+        upper.setflags(write=False)
+        object.__setattr__(self, "lower", lower)
+        object.__setattr__(self, "upper", upper)
+
+    def contains(self, q: np.ndarray, tolerance: float = 1e-10) -> bool:
+        values = np.asarray(q, dtype=float)
+        return bool(
+            values.shape == (7,)
+            and np.all(values >= self.lower - tolerance)
+            and np.all(values <= self.upper + tolerance)
+        )
+
+
+def physical_joint_limits() -> JointLimits:
+    upper = np.deg2rad([178.0, 130.0, 178.0, 135.0, 178.0, 128.0, 360.0])
+    return JointLimits("physical", -upper, upper)
+
+
+def teleop_joint_limits() -> JointLimits:
+    lower = np.deg2rad([-150.0, -30.0, -170.0, -130.0, -175.0, -125.0, -179.0])
+    upper = np.deg2rad([150.0, 110.0, 170.0, 130.0, 175.0, 125.0, 179.0])
+    return JointLimits("teleop", lower, upper)
+
+
+def joint_limit_profile(name: str) -> JointLimits:
+    profiles = {
+        "physical": physical_joint_limits,
+        "teleop": teleop_joint_limits,
+    }
+    try:
+        return profiles[name]()
+    except KeyError as exc:
+        raise ValueError(f"unknown joint limit profile: {name!r}") from exc
+
+
+@dataclass(frozen=True)
+class IkOptions:
+    position_tolerance_m: float = 1e-3
+    orientation_tolerance_rad: float = radians(0.1)
+    max_iterations: int = 500
+    time_limit_sec: Optional[float] = None
+    trust_region_rad: float = 0.05
+    task_weights: Tuple[float, float, float, float, float, float] = (
+        1.0,
+        1.0,
+        1.0,
+        0.4,
+        0.4,
+        0.4,
+    )
+    posture_weight: float = 1e-5
+    damping_initial: float = 0.1
+    damping_min: float = 0.01
+    damping_max: float = 1.0
+    damping_reduction: float = 0.95
+    stagnation_iterations: int = 40
+    stagnation_delta: float = 1e-9
+
+    def __post_init__(self) -> None:
+        positive = {
+            "position_tolerance_m": self.position_tolerance_m,
+            "orientation_tolerance_rad": self.orientation_tolerance_rad,
+            "trust_region_rad": self.trust_region_rad,
+            "damping_initial": self.damping_initial,
+            "damping_min": self.damping_min,
+            "damping_max": self.damping_max,
+        }
+        for name, value in positive.items():
+            if not np.isfinite(value) or value <= 0.0:
+                raise ValueError(f"{name} must be finite and positive")
+        if self.max_iterations <= 0 or self.stagnation_iterations <= 0:
+            raise ValueError("iteration limits must be positive")
+        if self.time_limit_sec is not None and self.time_limit_sec <= 0.0:
+            raise ValueError("time_limit_sec must be positive when set")
+        if len(self.task_weights) != 6 or any(weight <= 0.0 for weight in self.task_weights):
+            raise ValueError("task_weights must contain six positive values")
+        if self.posture_weight < 0.0 or not np.isfinite(self.posture_weight):
+            raise ValueError("posture_weight must be finite and non-negative")
+        if not self.damping_min <= self.damping_initial <= self.damping_max:
+            raise ValueError("damping_initial must be within damping_min and damping_max")
+        if not 0.0 < self.damping_reduction <= 1.0:
+            raise ValueError("damping_reduction must be in (0, 1]")
+
+
+@dataclass(frozen=True)
+class IkResult:
+    status: IkStatus
+    q: Optional[np.ndarray]
+    position_error_m: float
+    orientation_error_rad: float
+    iterations: int
+    solve_time_sec: float
+    osqp_status: str = ""
+    message: str = ""
+
+    @property
+    def success(self) -> bool:
+        return self.status is IkStatus.SUCCESS

ik_qp/src/rm75_ik/validation.py

@@ -0,0 +1,718 @@
+from __future__ import annotations
+
+import csv
+import json
+from dataclasses import dataclass
+from datetime import datetime, timezone
+from math import radians
+from pathlib import Path
+from typing import Any, Dict, Iterable, List, Optional, Tuple
+
+import numpy as np
+import pinocchio as pin
+import yaml
+
+from .dual_arm import DualArmAssembly
+from .kinematics import RM75Kinematics, pose_errors
+from .realman_reference import RealManFkReference
+from .solver import RM75IkSolver, deterministic_recovery_seeds
+from .types import IkOptions, IkStatus, JointLimits, physical_joint_limits, teleop_joint_limits
+
+
+FK_POSITION_LIMIT_M = 1e-4
+FK_ORIENTATION_LIMIT_RAD = radians(0.01)
+IK_POSITION_LIMIT_M = 1e-3
+IK_ORIENTATION_LIMIT_RAD = radians(0.1)
+JACOBIAN_RELATIVE_LIMIT = 1e-3
+JACOBIAN_ABSOLUTE_LIMIT = 5e-4
+NEAR_IK_RATE_LIMIT = 0.995
+CONTINUOUS_IK_RATE_LIMIT = 0.999
+GLOBAL_RECOVERY_RATE_LIMIT = 0.85
+NEAR_IK_P99_LIMIT_SEC = 0.008
+CONTROL_PERIOD_SEC = 1.0 / 90.0
+MAX_CONTINUOUS_JOINT_STEP_RAD = radians(2.0)
+
+
+def _validation_ik_options(max_iterations: int) -> IkOptions:
+    # Keep a 10% convergence guard band for independent Algo verification.
+    return IkOptions(
+        position_tolerance_m=0.9 * IK_POSITION_LIMIT_M,
+        orientation_tolerance_rad=0.9 * IK_ORIENTATION_LIMIT_RAD,
+        max_iterations=max_iterations,
+    )
+
+
+@dataclass(frozen=True)
+class ValidationSettings:
+    seed: int = 20260629
+    fk_samples: int = 10_000
+    jacobian_samples: int = 200
+    near_ik_samples: int = 1_000
+    global_samples: int = 200
+    continuous_trajectories: int = 20
+    continuous_points: int = 500
+    tool_samples: int = 100
+    dual_samples: int = 100
+    strict: bool = True
+
+    @classmethod
+    def quick(cls, seed: int = 20260629, strict: bool = False) -> "ValidationSettings":
+        return cls(
+            seed=seed,
+            fk_samples=100,
+            jacobian_samples=10,
+            near_ik_samples=30,
+            global_samples=10,
+            continuous_trajectories=2,
+            continuous_points=25,
+            tool_samples=10,
+            dual_samples=10,
+            strict=strict,
+        )
+
+
+def _percentile(values: Iterable[float], percentile: float) -> float:
+    data = list(values)
+    return float(np.percentile(data, percentile)) if data else float("nan")
+
+
+def _sample_configurations(
+    rng: np.random.Generator,
+    limits: JointLimits,
+    count: int,
+    margin_rad: Optional[np.ndarray] = None,
+) -> np.ndarray:
+    margin = np.zeros(7) if margin_rad is None else np.asarray(margin_rad, dtype=float)
+    lower = limits.lower + margin
+    upper = limits.upper - margin
+    if np.any(lower >= upper):
+        raise ValueError(f"sampling margin is too large for {limits.name} limits")
+    return rng.uniform(lower, upper, size=(count, 7))
+
+
+def _tool_pose_from_values(values: Iterable[float]) -> pin.SE3:
+    pose = np.asarray(list(values), dtype=float)
+    if pose.shape != (7,) or not np.all(np.isfinite(pose)):
+        raise ValueError("tool pose must be [x,y,z,qx,qy,qz,qw]")
+    quaternion = pin.Quaternion(pose[6], pose[3], pose[4], pose[5])
+    if quaternion.norm() <= 0.0:
+        raise ValueError("tool quaternion must be non-zero")
+    quaternion.normalize()
+    return pin.SE3(quaternion.matrix(), pose[:3])
+
+
+def load_project_tools(config_path: Path | str) -> Dict[str, pin.SE3]:
+    with Path(config_path).open("r", encoding="utf-8") as stream:
+        data = yaml.safe_load(stream)
+    tools = data.get("tools_in_ee", {})
+    selected: Dict[str, pin.SE3] = {}
+    for name in ("scissor", "omnipic", "minisci"):
+        if name not in tools or "pose" not in tools[name]:
+            raise ValueError(f"missing tool pose for {name!r}")
+        selected[name] = _tool_pose_from_values(tools[name]["pose"])
+    return selected
+
+
+class Stage1Validator:
+    def __init__(
+        self,
+        reference: RealManFkReference,
+        settings: ValidationSettings = ValidationSettings(),
+        tools: Optional[Dict[str, pin.SE3]] = None,
+    ) -> None:
+        self.reference = reference
+        self.settings = settings
+        self.tools = tools or {}
+        self.rng = np.random.default_rng(settings.seed)
+        self.checks: Dict[str, Dict[str, Any]] = {}
+        self.failures: List[Dict[str, Any]] = []
+
+    def _record_failure(
+        self,
+        category: str,
+        index: int,
+        reason: str,
+        q: Optional[np.ndarray] = None,
+        position_error_m: float = float("nan"),
+        orientation_error_rad: float = float("nan"),
+        profile: str = "",
+    ) -> None:
+        if len(self.failures) >= 1000:
+            return
+        self.failures.append(
+            {
+                "category": category,
+                "profile": profile,
+                "sample": index,
+                "reason": reason,
+                "position_error_m": position_error_m,
+                "orientation_error_rad": orientation_error_rad,
+                "q_rad": json.dumps(q.tolist()) if q is not None else "",
+            }
+        )
+
+    def _add_check(
+        self,
+        name: str,
+        passed: bool,
+        metrics: Dict[str, Any],
+        *,
+        required: bool = True,
+    ) -> None:
+        self.checks[name] = {
+            "passed": bool(passed),
+            "required": required,
+            **metrics,
+        }
+
+    def run(self) -> Dict[str, Any]:
+        self._model_checks()
+        self._fk_checks()
+        self._jacobian_checks()
+        self._near_ik_checks()
+        self._continuous_ik_checks()
+        self._global_recovery_checks()
+        self._singularity_checks()
+        self._dual_arm_checks()
+        self._tool_checks()
+        required_checks = [
+            check["passed"]
+            for check in self.checks.values()
+            if check.get("required", True)
+        ]
+        return {
+            "schema_version": 1,
+            "generated_at": datetime.now(timezone.utc).isoformat(),
+            "seed": self.settings.seed,
+            "strict": self.settings.strict,
+            "realman_api_version": self.reference.api_version,
+            "passed": bool(all(required_checks)),
+            "checks": self.checks,
+            "failure_count": len(self.failures),
+        }
+
+    def _model_checks(self) -> None:
+        physical = RM75Kinematics(limits=physical_joint_limits())
+        teleop = RM75Kinematics(limits=teleop_joint_limits())
+        assembly = DualArmAssembly.from_source_urdf(limits=physical_joint_limits())
+        passed = (
+            physical.model.nq == physical.model.nv == 7
+            and teleop.model.nq == teleop.model.nv == 7
+            and assembly.dof == 14
+            and physical.limits.contains(np.zeros(7))
+            and teleop.limits.contains(np.zeros(7))
+        )
+        self._add_check(
+            "model_structure",
+            passed,
+            {
+                "single_arm_nq": physical.model.nq,
+                "single_arm_nv": physical.model.nv,
+                "dual_arm_dof": assembly.dof,
+                "right_visual_origin_delta_m": assembly.mounts.right_visual_origin_delta_m,
+            },
+        )
+
+    def _fk_checks(self) -> None:
+        for limits in (physical_joint_limits(), teleop_joint_limits()):
+            kinematics = RM75Kinematics(limits=limits)
+            samples = _sample_configurations(
+                self.rng, limits, self.settings.fk_samples
+            )
+            position_errors: List[float] = []
+            orientation_errors: List[float] = []
+            for index, q in enumerate(samples):
+                pin_pose = kinematics.forward(q)
+                reference_pose = self.reference.forward(q)
+                position_error, orientation_error = pose_errors(pin_pose, reference_pose)
+                position_errors.append(position_error)
+                orientation_errors.append(orientation_error)
+                if (
+                    position_error >= FK_POSITION_LIMIT_M
+                    or orientation_error >= FK_ORIENTATION_LIMIT_RAD
+                ):
+                    self._record_failure(
+                        "fk",
+                        index,
+                        "FK residual exceeded limit",
+                        q,
+                        position_error,
+                        orientation_error,
+                        limits.name,
+                    )
+            max_position = max(position_errors, default=float("inf"))
+            max_orientation = max(orientation_errors, default=float("inf"))
+            self._add_check(
+                f"fk_{limits.name}",
+                max_position < FK_POSITION_LIMIT_M
+                and max_orientation < FK_ORIENTATION_LIMIT_RAD,
+                {
+                    "samples": len(samples),
+                    "max_position_error_m": max_position,
+                    "p99_position_error_m": _percentile(position_errors, 99),
+                    "max_orientation_error_rad": max_orientation,
+                    "p99_orientation_error_rad": _percentile(
+                        orientation_errors, 99
+                    ),
+                },
+            )
+
+    def _numeric_reference_jacobian(self, q: np.ndarray, step: float = 2e-3) -> np.ndarray:
+        center = self.reference.forward(q)
+        numeric = np.zeros((6, 7))
+        for joint_index in range(7):
+            delta = np.zeros(7)
+            delta[joint_index] = step
+            plus = self.reference.forward(q + delta)
+            minus = self.reference.forward(q - delta)
+            plus_twist = pin.log6(center.actInv(plus)).vector
+            minus_twist = pin.log6(center.actInv(minus)).vector
+            numeric[:, joint_index] = (plus_twist - minus_twist) / (2.0 * step)
+        return numeric
+
+    def _jacobian_checks(self) -> None:
+        limits = physical_joint_limits()
+        kinematics = RM75Kinematics(limits=limits)
+        # Algo FK is returned as float32. A 2e-3 rad central-difference step
+        # keeps quantization below the analytic-Jacobian acceptance limit.
+        margin = np.full(7, 3e-3)
+        samples = _sample_configurations(
+            self.rng, limits, self.settings.jacobian_samples, margin
+        )
+        relative_errors: List[float] = []
+        absolute_errors: List[float] = []
+        for index, q in enumerate(samples):
+            analytic = kinematics.jacobian(q)
+            numeric = self._numeric_reference_jacobian(q)
+            difference = analytic - numeric
+            relative = float(
+                np.linalg.norm(difference) / max(np.linalg.norm(numeric), 1e-12)
+            )
+            absolute = float(np.max(np.abs(difference)))
+            relative_errors.append(relative)
+            absolute_errors.append(absolute)
+            if relative >= JACOBIAN_RELATIVE_LIMIT or absolute >= JACOBIAN_ABSOLUTE_LIMIT:
+                self._record_failure(
+                    "jacobian",
+                    index,
+                    f"relative={relative:.6g}, absolute={absolute:.6g}",
+                    q,
+                    profile=limits.name,
+                )
+        max_relative = max(relative_errors, default=float("inf"))
+        max_absolute = max(absolute_errors, default=float("inf"))
+        self._add_check(
+            "jacobian",
+            max_relative < JACOBIAN_RELATIVE_LIMIT
+            and max_absolute < JACOBIAN_ABSOLUTE_LIMIT,
+            {
+                "samples": len(samples),
+                "max_relative_error": max_relative,
+                "max_absolute_error": max_absolute,
+            },
+        )
+
+    def _externally_accept_solution(
+        self,
+        target: pin.SE3,
+        result_q: Optional[np.ndarray],
+        limits: JointLimits,
+    ) -> Tuple[bool, float, float]:
+        if result_q is None or not limits.contains(result_q):
+            return False, float("inf"), float("inf")
+        verified = self.reference.forward(result_q)
+        position_error, orientation_error = pose_errors(verified, target)
+        return (
+            position_error <= IK_POSITION_LIMIT_M
+            and orientation_error <= IK_ORIENTATION_LIMIT_RAD,
+            position_error,
+            orientation_error,
+        )
+
+    def _near_ik_checks(self) -> None:
+        all_times: List[float] = []
+        profile_rates: Dict[str, float] = {}
+        for limits in (physical_joint_limits(), teleop_joint_limits()):
+            kinematics = RM75Kinematics(limits=limits)
+            solver = RM75IkSolver(kinematics)
+            margin = np.deg2rad([5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 10.0])
+            targets_q = _sample_configurations(
+                self.rng, limits, self.settings.near_ik_samples, margin
+            )
+            successes = 0
+            profile_times: List[float] = []
+            options = _validation_ik_options(max_iterations=200)
+            for index, target_q in enumerate(targets_q):
+                seed = np.clip(
+                    target_q + self.rng.uniform(-radians(10), radians(10), 7),
+                    limits.lower,
+                    limits.upper,
+                )
+                target = self.reference.forward(target_q)
+                result = solver.solve(target, seed, options)
+                profile_times.append(result.solve_time_sec)
+                all_times.append(result.solve_time_sec)
+                accepted, position_error, orientation_error = self._externally_accept_solution(
+                    target, result.q, limits
+                )
+                if result.success and accepted:
+                    successes += 1
+                else:
+                    self._record_failure(
+                        "near_ik",
+                        index,
+                        f"status={result.status.value}; {result.message}",
+                        seed,
+                        position_error if np.isfinite(position_error) else result.position_error_m,
+                        orientation_error if np.isfinite(orientation_error) else result.orientation_error_rad,
+                        limits.name,
+                    )
+            rate = successes / max(len(targets_q), 1)
+            profile_rates[limits.name] = rate
+            self._add_check(
+                f"near_ik_{limits.name}",
+                rate >= NEAR_IK_RATE_LIMIT,
+                {
+                    "samples": len(targets_q),
+                    "successes": successes,
+                    "success_rate": rate,
+                    "p99_time_sec": _percentile(profile_times, 99),
+                    "max_time_sec": max(profile_times, default=float("nan")),
+                },
+            )
+
+        p99_time = _percentile(all_times, 99)
+        max_time = max(all_times, default=float("inf"))
+        self._add_check(
+            "near_ik_performance",
+            p99_time < NEAR_IK_P99_LIMIT_SEC and max_time < CONTROL_PERIOD_SEC,
+            {
+                "p99_time_sec": p99_time,
+                "max_time_sec": max_time,
+                "p99_limit_sec": NEAR_IK_P99_LIMIT_SEC,
+                "control_period_sec": CONTROL_PERIOD_SEC,
+            },
+            required=self.settings.strict,
+        )
+
+    def _continuous_ik_checks(self) -> None:
+        limits = teleop_joint_limits()
+        kinematics = RM75Kinematics(limits=limits)
+        solver = RM75IkSolver(kinematics)
+        options = _validation_ik_options(max_iterations=100)
+        successes = 0
+        total = 0
+        max_joint_step = 0.0
+        for trajectory_index in range(self.settings.continuous_trajectories):
+            span = limits.upper - limits.lower
+            center = self.rng.uniform(
+                limits.lower + 0.3 * span,
+                limits.upper - 0.3 * span,
+            )
+            amplitude = self.rng.uniform(0.015, 0.04, 7) * span
+            frequency = self.rng.uniform(0.03, 0.08, 7)
+            phase = self.rng.uniform(-np.pi, np.pi, 7)
+            times = np.arange(self.settings.continuous_points) / 90.0
+            path = center + amplitude * np.sin(
+                2.0 * np.pi * times[:, None] * frequency + phase
+            )
+            seed = path[0].copy()
+            previous_solution = seed.copy()
+            for point_index, target_q in enumerate(path):
+                total += 1
+                target = self.reference.forward(target_q)
+                result = solver.solve(target, seed, options)
+                accepted, position_error, orientation_error = self._externally_accept_solution(
+                    target, result.q, limits
+                )
+                if result.success and accepted and result.q is not None:
+                    joint_step = float(np.max(np.abs(result.q - previous_solution)))
+                    max_joint_step = max(max_joint_step, joint_step)
+                    previous_solution = result.q
+                    seed = result.q
+                    successes += 1
+                else:
+                    self._record_failure(
+                        "continuous_ik",
+                        trajectory_index * self.settings.continuous_points + point_index,
+                        f"status={result.status.value}; {result.message}",
+                        seed,
+                        position_error,
+                        orientation_error,
+                        limits.name,
+                    )
+        rate = successes / max(total, 1)
+        self._add_check(
+            "continuous_ik",
+            rate >= CONTINUOUS_IK_RATE_LIMIT
+            and max_joint_step <= MAX_CONTINUOUS_JOINT_STEP_RAD,
+            {
+                "trajectories": self.settings.continuous_trajectories,
+                "points": total,
+                "successes": successes,
+                "success_rate": rate,
+                "max_joint_step_rad": max_joint_step,
+                "joint_step_limit_rad": MAX_CONTINUOUS_JOINT_STEP_RAD,
+            },
+        )
+
+    def _global_recovery_checks(self) -> None:
+        limits = physical_joint_limits()
+        kinematics = RM75Kinematics(limits=limits)
+        solver = RM75IkSolver(kinematics)
+        options = _validation_ik_options(max_iterations=500)
+        margin = np.deg2rad([5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 10.0])
+        target_configurations = _sample_configurations(
+            self.rng, limits, self.settings.global_samples, margin
+        )
+        recovery_seeds = deterministic_recovery_seeds(limits)
+        single_successes = 0
+        recovery_successes = 0
+        recovery_times: List[float] = []
+        for index, target_q in enumerate(target_configurations):
+            target = self.reference.forward(target_q)
+            random_seed = self.rng.uniform(limits.lower, limits.upper)
+            single = solver.solve(target, random_seed, options)
+            single_accepted, _, _ = self._externally_accept_solution(
+                target, single.q, limits
+            )
+            single_successes += int(single.success and single_accepted)
+
+            recovered = solver.solve_multistart(target, recovery_seeds, options)
+            recovery_times.append(recovered.solve_time_sec)
+            accepted, position_error, orientation_error = self._externally_accept_solution(
+                target, recovered.q, limits
+            )
+            if recovered.success and accepted:
+                recovery_successes += 1
+            else:
+                self._record_failure(
+                    "global_recovery",
+                    index,
+                    f"status={recovered.status.value}; {recovered.message}",
+                    target_q,
+                    position_error,
+                    orientation_error,
+                    limits.name,
+                )
+        count = max(len(target_configurations), 1)
+        recovery_rate = recovery_successes / count
+        self._add_check(
+            "global_recovery",
+            recovery_rate >= GLOBAL_RECOVERY_RATE_LIMIT,
+            {
+                "samples": len(target_configurations),
+                "single_seed_success_rate": single_successes / count,
+                "recovery_successes": recovery_successes,
+                "recovery_success_rate": recovery_rate,
+                "recovery_p95_time_sec": _percentile(recovery_times, 95),
+                "recovery_max_time_sec": max(recovery_times, default=float("nan")),
+            },
+        )
+
+    def _singularity_checks(self) -> None:
+        limits = physical_joint_limits()
+        solver = RM75IkSolver(RM75Kinematics(limits=limits))
+        singular_degrees = np.asarray(
+            [
+                [0, 0, 0, 90, 0, 0, 0],
+                [0, 60, 0, 0, 0, 90, 0],
+                [0, 0, 90, 90, 0, 90, 0],
+                [0, 90, 90, 90, 90, 0, 0],
+            ],
+            dtype=float,
+        )
+        invalid_results = 0
+        total = 0
+        statuses: Dict[str, int] = {}
+        for case_index, singular_q in enumerate(np.deg2rad(singular_degrees)):
+            for perturbation in (-radians(0.1), 0.0, radians(0.1)):
+                total += 1
+                target_q = singular_q.copy()
+                target_q[case_index % 7] += perturbation
+                target = self.reference.forward(target_q)
+                seed = np.clip(
+                    target_q + self.rng.uniform(-radians(0.5), radians(0.5), 7),
+                    limits.lower,
+                    limits.upper,
+                )
+                result = solver.solve(
+                    target,
+                    seed,
+                    _validation_ik_options(max_iterations=200),
+                )
+                statuses[result.status.value] = statuses.get(result.status.value, 0) + 1
+                finite_diagnostics = np.isfinite(result.position_error_m) and np.isfinite(
+                    result.orientation_error_rad
+                )
+                accepted, _, _ = self._externally_accept_solution(target, result.q, limits)
+                pseudo_success = result.status is IkStatus.SUCCESS and not accepted
+                if not finite_diagnostics or pseudo_success:
+                    invalid_results += 1
+                    self._record_failure(
+                        "singularity",
+                        total - 1,
+                        "non-finite diagnostic or false success",
+                        seed,
+                        result.position_error_m,
+                        result.orientation_error_rad,
+                        limits.name,
+                    )
+        self._add_check(
+            "singularity_behavior",
+            invalid_results == 0,
+            {
+                "samples": total,
+                "invalid_results": invalid_results,
+                "statuses": statuses,
+            },
+        )
+
+    def _dual_arm_checks(self) -> None:
+        limits = physical_joint_limits()
+        assembly = DualArmAssembly.from_source_urdf(limits=limits)
+        samples = _sample_configurations(
+            self.rng, limits, self.settings.dual_samples
+        )
+        max_position = 0.0
+        max_orientation = 0.0
+        failures = 0
+        for arm in ("left", "right"):
+            mount = (
+                assembly.mounts.left_base if arm == "left" else assembly.mounts.right_base
+            )
+            for index, q in enumerate(samples):
+                world_pose = assembly.forward(arm, q)
+                local_pose = mount.actInv(world_pose)
+                reference_pose = self.reference.forward(q)
+                position_error, orientation_error = pose_errors(local_pose, reference_pose)
+                max_position = max(max_position, position_error)
+                max_orientation = max(max_orientation, orientation_error)
+                if (
+                    position_error >= FK_POSITION_LIMIT_M
+                    or orientation_error >= FK_ORIENTATION_LIMIT_RAD
+                ):
+                    failures += 1
+                    self._record_failure(
+                        "dual_arm",
+                        index,
+                        f"{arm} local FK residual exceeded limit",
+                        q,
+                        position_error,
+                        orientation_error,
+                        arm,
+                    )
+        self._add_check(
+            "dual_arm_assembly",
+            failures == 0,
+            {
+                "samples_per_arm": len(samples),
+                "max_position_error_m": max_position,
+                "max_orientation_error_rad": max_orientation,
+                "right_visual_origin_delta_m": assembly.mounts.right_visual_origin_delta_m,
+            },
+        )
+
+    def _tool_checks(self) -> None:
+        if not self.tools:
+            self._add_check(
+                "tool_frames",
+                True,
+                {"samples": 0, "message": "no tool configuration supplied"},
+                required=False,
+            )
+            return
+        limits = teleop_joint_limits()
+        kinematics = RM75Kinematics(limits=limits)
+        samples = _sample_configurations(
+            self.rng, limits, self.settings.tool_samples
+        )
+        max_position = 0.0
+        max_orientation = 0.0
+        failures = 0
+        for tool_name, tool in self.tools.items():
+            for index, q in enumerate(samples):
+                pin_pose = kinematics.forward(q, tool)
+                reference_pose = self.reference.forward(q, tool)
+                position_error, orientation_error = pose_errors(pin_pose, reference_pose)
+                max_position = max(max_position, position_error)
+                max_orientation = max(max_orientation, orientation_error)
+                if (
+                    position_error >= FK_POSITION_LIMIT_M
+                    or orientation_error >= FK_ORIENTATION_LIMIT_RAD
+                ):
+                    failures += 1
+                    self._record_failure(
+                        "tool_frame",
+                        index,
+                        f"{tool_name} residual exceeded limit",
+                        q,
+                        position_error,
+                        orientation_error,
+                        tool_name,
+                    )
+        self._add_check(
+            "tool_frames",
+            failures == 0,
+            {
+                "tools": sorted(self.tools),
+                "samples_per_tool": len(samples),
+                "max_position_error_m": max_position,
+                "max_orientation_error_rad": max_orientation,
+            },
+        )
+
+
+def write_validation_report(
+    output_dir: Path | str,
+    summary: Dict[str, Any],
+    failures: List[Dict[str, Any]],
+) -> Tuple[Path, Path, Path]:
+    directory = Path(output_dir)
+    directory.mkdir(parents=True, exist_ok=True)
+    json_path = directory / "stage1_summary.json"
+    csv_path = directory / "stage1_failures.csv"
+    markdown_path = directory / "stage1_report.md"
+
+    with json_path.open("w", encoding="utf-8") as stream:
+        json.dump(summary, stream, ensure_ascii=True, indent=2, sort_keys=True)
+        stream.write("\n")
+
+    fieldnames = [
+        "category",
+        "profile",
+        "sample",
+        "reason",
+        "position_error_m",
+        "orientation_error_rad",
+        "q_rad",
+    ]
+    with csv_path.open("w", encoding="utf-8", newline="") as stream:
+        writer = csv.DictWriter(stream, fieldnames=fieldnames)
+        writer.writeheader()
+        writer.writerows(failures)
+
+    lines = [
+        "# RM75-B Stage 1 Validation",
+        "",
+        f"- Overall: **{'PASS' if summary['passed'] else 'FAIL'}**",
+        f"- Seed: `{summary['seed']}`",
+        f"- RealMan API: `{summary['realman_api_version']}`",
+        f"- Failures recorded: `{summary['failure_count']}`",
+        "",
+        "| Check | Required | Result | Key metrics |",
+        "|---|---:|---:|---|",
+    ]
+    for name, check in summary["checks"].items():
+        metrics = {
+            key: value
+            for key, value in check.items()
+            if key not in {"passed", "required"}
+        }
+        metrics_text = json.dumps(metrics, ensure_ascii=True, sort_keys=True)
+        lines.append(
+            f"| `{name}` | {check['required']} | "
+            f"{'PASS' if check['passed'] else 'FAIL'} | `{metrics_text}` |"
+        )
+    markdown_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
+    return json_path, csv_path, markdown_path