YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implement dual-arm teleoperation with RM75 QP controller and MuJoCo backend

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-01 11:26:58 +08:00
parent fdf505eac5
commit 5678cc8e69
52 changed files with 612116 additions and 327 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
ik_qp/src/rm75_ik/__init__.py
View File

@ -1,7 +1,17 @@
from .dual_arm import DualArmAssembly, DualArmMounts, load_dual_arm_mounts
from .kinematics import RM75Kinematics, default_urdf_path, pose_errors, validate_se3
from .mujoco_model import build_normalized_dual_mjcf
from .realman_reference import RealManFkReference
from .solver import RM75IkSolver, deterministic_recovery_seeds
from .robot_backend import DualArmJointState, RobotBackend
from .teleop_config import ArmTeleopProfile, load_dual_arm_profiles
from .teleop_control import (
ControllerSample,
ControlCycleResult,
DualArmQpTeleopController,
RelativePoseMapper,
SafetyState,
)
from .types import (
IkOptions,
IkResult,
@ -14,21 +24,49 @@ from .types import (
__all__ = [
"DualArmAssembly",
"DualArmJointState",
"DualArmMounts",
"DualArmMuJoCo",
"DualArmQpTeleopController",
"ControllerSample",
"ControlCycleResult",
"IkOptions",
"IkResult",
"IkStatus",
"JointLimits",
"InitialPoseDiagnostic",
"MujocoRobot",
"PlaybackResult",
"RM75IkSolver",
"RM75Kinematics",
"RealManFkReference",
"RelativePoseMapper",
"RobotBackend",
"SafetyState",
"ArmTeleopProfile",
"default_urdf_path",
"build_normalized_dual_mjcf",
"deterministic_recovery_seeds",
"joint_limit_profile",
"load_dual_arm_mounts",
"load_dual_arm_profiles",
"physical_joint_limits",
"pose_errors",
"teleop_joint_limits",
"validate_se3",
]
def __getattr__(name):
if name in {"DualArmMuJoCo", "PlaybackResult"}:
from .mujoco_backend import DualArmMuJoCo, PlaybackResult
return {"DualArmMuJoCo": DualArmMuJoCo, "PlaybackResult": PlaybackResult}[name]
if name in {"MujocoRobot", "InitialPoseDiagnostic"}:
from .mujoco_robot import InitialPoseDiagnostic, MujocoRobot
return {
"MujocoRobot": MujocoRobot,
"InitialPoseDiagnostic": InitialPoseDiagnostic,
}[name]
raise AttributeError(name)

16
ik_qp/src/rm75_ik/dual_arm.py
View File

@ -21,15 +21,22 @@ def default_dual_source_path() -> Path:
)
if source_path.is_file():
return source_path
installed_path = (
installed_candidates = [
Path(sysconfig.get_path("data"))
/ "share"
/ "rm75_ik"
/ "models"
/ "dual_arm_mujoco_fixed.urdf"
)
if installed_path.is_file():
return installed_path
]
resolved = Path(__file__).resolve()
if len(resolved.parents) > 4:
installed_candidates.append(
resolved.parents[4]
/ "share/rm75_ik/models/dual_arm_mujoco_fixed.urdf"
)
for installed_path in installed_candidates:
if installed_path.is_file():
return installed_path
raise FileNotFoundError("dual_arm_mujoco_fixed.urdf was not found")
@ -129,4 +136,3 @@ class DualArmAssembly:
if arm == "right":
return self.mounts.right_base * local
raise ValueError("arm must be 'left' or 'right'")

15
ik_qp/src/rm75_ik/kinematics.py
View File

@ -23,15 +23,21 @@ def default_urdf_path() -> Path:
)
if source_path.is_file():
return source_path
installed_path = (
installed_candidates = [
Path(sysconfig.get_path("data"))
/ "share"
/ "rm75_ik"
/ "models"
/ "RM75-B.urdf"
)
if installed_path.is_file():
return installed_path
]
resolved = Path(__file__).resolve()
if len(resolved.parents) > 4:
installed_candidates.append(
resolved.parents[4] / "share/rm75_ik/models/RM75-B.urdf"
)
for installed_path in installed_candidates:
if installed_path.is_file():
return installed_path
raise FileNotFoundError("RM75-B.urdf was not found in source or installed data")
@ -121,4 +127,3 @@ class RM75Kinematics:
pin.ReferenceFrame.LOCAL,
)
return np.asarray(jacobian).copy()

247
ik_qp/src/rm75_ik/mujoco_backend.py Normal file
View File

@ -0,0 +1,247 @@
from __future__ import annotations
from dataclasses import dataclass
from time import perf_counter, sleep
from typing import Iterable, Optional
import mujoco
import numpy as np
import pinocchio as pin
from .kinematics import validate_se3
from .mujoco_model import build_normalized_dual_mjcf
from .types import JointLimits, physical_joint_limits
CONTROLLED_HOME_Q_RAD = np.deg2rad([0.0, 30.0, 0.0, 60.0, 0.0, 60.0, 0.0])
PROJECT_INITIAL_Q_RAD = {
"left": np.deg2rad([-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]),
"right": np.deg2rad([-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]),
}
@dataclass(frozen=True)
class PlaybackResult:
samples: int
elapsed_sec: float
max_joint_step_rad: float
final_flange_pose: pin.SE3
class DualArmMuJoCo:
"""Threadless, kinematic MuJoCo backend for the normalized dual RM75 scene."""
def __init__(
self,
controlled_arm: str = "left",
inactive_q_rad: Optional[np.ndarray] = None,
limits: Optional[JointLimits] = None,
) -> None:
if controlled_arm not in {"left", "right"}:
raise ValueError("controlled_arm must be 'left' or 'right'")
self.controlled_arm = controlled_arm
self.inactive_arm = "right" if controlled_arm == "left" else "left"
self.limits = limits or physical_joint_limits()
self.mjcf_xml, self.assets = build_normalized_dual_mjcf()
self.model = mujoco.MjModel.from_xml_string(self.mjcf_xml, self.assets)
self.data = mujoco.MjData(self.model)
self._qpos_addresses = {
arm: np.array(
[
self.model.jnt_qposadr[
mujoco.mj_name2id(
self.model,
mujoco.mjtObj.mjOBJ_JOINT,
f"{arm}_joint_{joint_index}",
)
]
for joint_index in range(1, 8)
],
dtype=int,
)
for arm in ("left", "right")
}
self._dof_addresses = {
arm: np.array(
[
self.model.jnt_dofadr[
mujoco.mj_name2id(
self.model,
mujoco.mjtObj.mjOBJ_JOINT,
f"{arm}_joint_{joint_index}",
)
]
for joint_index in range(1, 8)
],
dtype=int,
)
for arm in ("left", "right")
}
self._site_ids = {
arm: mujoco.mj_name2id(
self.model, mujoco.mjtObj.mjOBJ_SITE, f"{arm}_flange"
)
for arm in ("left", "right")
}
self._target_mocap_ids = {}
for arm in ("left", "right"):
marker_body_id = mujoco.mj_name2id(
self.model, mujoco.mjtObj.mjOBJ_BODY, f"{arm}_target_marker"
)
mocap_id = int(self.model.body_mocapid[marker_body_id])
if mocap_id < 0:
raise ValueError(f"{arm}_target_marker is not a MuJoCo mocap body")
self._target_mocap_ids[arm] = mocap_id
inactive = (
PROJECT_INITIAL_Q_RAD[self.inactive_arm]
if inactive_q_rad is None
else np.asarray(inactive_q_rad, dtype=float)
)
self.set_arm_configuration(self.inactive_arm, inactive)
self.set_arm_configuration(self.controlled_arm, CONTROLLED_HOME_Q_RAD)
for arm in ("left", "right"):
self.set_arm_target_marker(arm, self.get_flange_pose(arm))
self._manual_inactive_q: Optional[np.ndarray] = None
@staticmethod
def _validate_arm(arm: str) -> None:
if arm not in {"left", "right"}:
raise ValueError("arm must be 'left' or 'right'")
def _validate_q(self, q_rad: np.ndarray) -> np.ndarray:
q = np.asarray(q_rad, dtype=float)
if q.shape != (7,):
raise ValueError(f"arm configuration must have shape (7,), got {q.shape}")
if not np.all(np.isfinite(q)):
raise ValueError("arm configuration must be finite")
if not self.limits.contains(q):
raise ValueError(f"configuration is outside {self.limits.name} joint limits")
return q.copy()
def set_arm_configuration(self, arm: str, q_rad: np.ndarray) -> None:
self._validate_arm(arm)
q = self._validate_q(q_rad)
self.data.qpos[self._qpos_addresses[arm]] = q
self.data.qvel[:] = 0.0
mujoco.mj_forward(self.model, self.data)
def set_dual_configuration(
self, left_q_rad: np.ndarray, right_q_rad: np.ndarray
) -> None:
left_q = self._validate_q(left_q_rad)
right_q = self._validate_q(right_q_rad)
self.data.qpos[self._qpos_addresses["left"]] = left_q
self.data.qpos[self._qpos_addresses["right"]] = right_q
self.data.qvel[:] = 0.0
mujoco.mj_forward(self.model, self.data)
def get_arm_configuration(self, arm: str) -> np.ndarray:
self._validate_arm(arm)
return self.data.qpos[self._qpos_addresses[arm]].copy()
def get_flange_pose(self, arm: str) -> pin.SE3:
self._validate_arm(arm)
site_id = self._site_ids[arm]
return pin.SE3(
self.data.site_xmat[site_id].reshape(3, 3).copy(),
self.data.site_xpos[site_id].copy(),
)
def set_target_marker(self, target_se3: pin.SE3) -> None:
self.set_arm_target_marker(self.controlled_arm, target_se3)
def set_arm_target_marker(self, arm: str, target_se3: pin.SE3) -> None:
self._validate_arm(arm)
validate_se3(target_se3, "target_se3")
quaternion = pin.Quaternion(target_se3.rotation)
mocap_id = self._target_mocap_ids[arm]
self.data.mocap_pos[mocap_id] = target_se3.translation
self.data.mocap_quat[mocap_id] = [
quaternion.w,
quaternion.x,
quaternion.y,
quaternion.z,
]
mujoco.mj_forward(self.model, self.data)
def _validated_trajectory(self, q_trajectory: Iterable[np.ndarray]) -> np.ndarray:
trajectory = np.asarray(list(q_trajectory), dtype=float)
if trajectory.ndim != 2 or trajectory.shape[1:] != (7,):
raise ValueError("q_trajectory must have shape (N, 7)")
if trajectory.shape[0] == 0:
raise ValueError("q_trajectory must contain at least one sample")
if not np.all(np.isfinite(trajectory)):
raise ValueError("q_trajectory must be finite")
if np.any(trajectory < self.limits.lower) or np.any(
trajectory > self.limits.upper
):
raise ValueError(f"trajectory exceeds {self.limits.name} joint limits")
return trajectory
def play_trajectory(
self,
q_trajectory: Iterable[np.ndarray],
dt: float = 1.0 / 90.0,
realtime: bool = False,
viewer=None,
) -> PlaybackResult:
if not np.isfinite(dt) or dt <= 0.0:
raise ValueError("dt must be finite and positive")
trajectory = self._validated_trajectory(q_trajectory)
started = perf_counter()
previous = self.get_arm_configuration(self.controlled_arm)
max_step = 0.0
for q in trajectory:
frame_started = perf_counter()
max_step = max(max_step, float(np.max(np.abs(q - previous))))
self.set_arm_configuration(self.controlled_arm, q)
previous = q
if viewer is not None:
viewer.sync()
if realtime:
remaining = dt - (perf_counter() - frame_started)
if remaining > 0.0:
sleep(remaining)
return PlaybackResult(
samples=len(trajectory),
elapsed_sec=perf_counter() - started,
max_joint_step_rad=max_step,
final_flange_pose=self.get_flange_pose(self.controlled_arm),
)
def configure_manual_drag(self, damping: float = 1.5) -> None:
if not np.isfinite(damping) or damping <= 0.0:
raise ValueError("manual-drag damping must be finite and positive")
self.model.opt.gravity[:] = 0.0
self.model.dof_damping[self._dof_addresses[self.controlled_arm]] = damping
self.data.qvel[:] = 0.0
self.data.xfrc_applied[:] = 0.0
self._manual_inactive_q = self.get_arm_configuration(self.inactive_arm)
mujoco.mj_forward(self.model, self.data)
def step_manual_drag(self) -> None:
if self._manual_inactive_q is None:
raise RuntimeError("configure_manual_drag() must be called first")
mujoco.mj_step(self.model, self.data)
self.data.qpos[self._qpos_addresses[self.inactive_arm]] = self._manual_inactive_q
self.data.qvel[self._dof_addresses[self.inactive_arm]] = 0.0
mujoco.mj_forward(self.model, self.data)
def render(
self,
width: int = 1280,
height: int = 720,
*,
segmentation: bool = False,
) -> np.ndarray:
if width <= 0 or height <= 0:
raise ValueError("render dimensions must be positive")
renderer = mujoco.Renderer(self.model, height=height, width=width)
try:
if segmentation:
renderer.enable_segmentation_rendering()
renderer.update_scene(self.data, camera="overview")
return renderer.render().copy()
finally:
renderer.close()

413
ik_qp/src/rm75_ik/mujoco_model.py Normal file
View File

@ -0,0 +1,413 @@
from __future__ import annotations
import re
import xml.etree.ElementTree as ET
from pathlib import Path
from typing import Dict, Iterable, Optional, Tuple
import numpy as np
import pinocchio as pin
from .dual_arm import default_dual_source_path, load_dual_arm_mounts
from .kinematics import default_urdf_path
from .types import physical_joint_limits
def _numbers(text: str, expected: int) -> np.ndarray:
values = np.fromstring(text, sep=" ", dtype=float)
if values.shape != (expected,):
raise ValueError(f"expected {expected} numeric values, got {text!r}")
return values
def _format(values: Iterable[float]) -> str:
return " ".join(f"{float(value):.12g}" for value in values)
def _quaternion_from_rpy(rpy: np.ndarray) -> np.ndarray:
quaternion = pin.Quaternion(pin.rpy.rpyToMatrix(*rpy))
return np.array([quaternion.w, quaternion.x, quaternion.y, quaternion.z])
def _se3_attributes(transform: pin.SE3) -> Dict[str, str]:
quaternion = pin.Quaternion(transform.rotation)
return {
"pos": _format(transform.translation),
"quat": _format([quaternion.w, quaternion.x, quaternion.y, quaternion.z]),
}
def _origin_attributes(parent: ET.Element) -> Dict[str, str]:
origin = parent.find("origin")
if origin is None:
return {"pos": "0 0 0", "quat": "1 0 0 0"}
xyz = _numbers(origin.get("xyz", "0 0 0"), 3)
rpy = _numbers(origin.get("rpy", "0 0 0"), 3)
return {"pos": _format(xyz), "quat": _format(_quaternion_from_rpy(rpy))}
def _mesh_asset_name(filename: str, prefix: str) -> str:
stem = Path(filename).stem.lower()
return f"{prefix}_{re.sub(r'[^a-z0-9_]+', '_', stem)}"
def _dual_obj_directory(dual_source_path: Path) -> Path:
source_candidate = dual_source_path.parent / "dual_arm_obj"
if source_candidate.is_dir():
return source_candidate
raise FileNotFoundError(
f"dual-arm OBJ directory was not found beside {dual_source_path}"
)
def _single_mesh_directory(single_urdf_path: Path) -> Path:
candidates = (
single_urdf_path.parent / "meshes",
single_urdf_path.parent / "rm75_meshes",
)
for candidate in candidates:
if candidate.is_dir():
return candidate
raise FileNotFoundError(
f"RM75-B STL mesh directory was not found beside {single_urdf_path}"
)
def _add_inertial(body: ET.Element, link: ET.Element) -> None:
inertial = link.find("inertial")
if inertial is None:
raise ValueError(f"link {link.get('name')!r} has no inertial data")
mass_element = inertial.find("mass")
inertia_element = inertial.find("inertia")
if mass_element is None or inertia_element is None:
raise ValueError(f"link {link.get('name')!r} has incomplete inertial data")
origin = inertial.find("origin")
xyz = _numbers(origin.get("xyz", "0 0 0"), 3) if origin is not None else np.zeros(3)
rpy = _numbers(origin.get("rpy", "0 0 0"), 3) if origin is not None else np.zeros(3)
inertia_matrix = np.array(
[
[float(inertia_element.get("ixx")), float(inertia_element.get("ixy")), float(inertia_element.get("ixz"))],
[float(inertia_element.get("ixy")), float(inertia_element.get("iyy")), float(inertia_element.get("iyz"))],
[float(inertia_element.get("ixz")), float(inertia_element.get("iyz")), float(inertia_element.get("izz"))],
]
)
rotation = pin.rpy.rpyToMatrix(*rpy)
inertia_matrix = rotation @ inertia_matrix @ rotation.T
full_inertia = [
inertia_matrix[0, 0],
inertia_matrix[1, 1],
inertia_matrix[2, 2],
inertia_matrix[0, 1],
inertia_matrix[0, 2],
inertia_matrix[1, 2],
]
ET.SubElement(
body,
"inertial",
{
"pos": _format(xyz),
"mass": mass_element.get("value", "0"),
"fullinertia": _format(full_inertia),
},
)
def _add_link_visual(
body: ET.Element,
link: ET.Element,
arm: str,
single_mesh_keys: Dict[str, str],
) -> None:
visual = link.find("visual")
if visual is None:
return
mesh = visual.find("geometry/mesh")
if mesh is None:
return
filename = Path(mesh.get("filename", "")).name
try:
mesh_name = single_mesh_keys[filename]
except KeyError as exc:
raise ValueError(f"missing packaged single-arm mesh {filename!r}") from exc
ET.SubElement(
body,
"geom",
{
"name": f"{arm}_{link.get('name')}_visual",
"type": "mesh",
"mesh": mesh_name,
"material": f"{arm}_arm",
"contype": "0",
"conaffinity": "0",
"group": "1",
**_origin_attributes(visual),
},
)
def _add_arm(
worldbody: ET.Element,
arm: str,
mount: pin.SE3,
single_root: ET.Element,
single_mesh_keys: Dict[str, str],
gripper_mesh_name: str,
) -> None:
links = {link.get("name"): link for link in single_root.findall("link")}
joints_by_parent: Dict[str, list[ET.Element]] = {}
for joint in single_root.findall("joint"):
parent_element = joint.find("parent")
if parent_element is None:
continue
joints_by_parent.setdefault(parent_element.get("link", ""), []).append(joint)
limits = physical_joint_limits()
base_link = links["base_link"]
base_body = ET.SubElement(
worldbody,
"body",
{"name": f"{arm}_base_link", **_se3_attributes(mount)},
)
_add_link_visual(base_body, base_link, arm, single_mesh_keys)
def append_children(parent_body: ET.Element, parent_link_name: str) -> None:
for joint in joints_by_parent.get(parent_link_name, []):
child_element = joint.find("child")
if child_element is None:
raise ValueError(f"joint {joint.get('name')!r} has no child")
child_name = child_element.get("link", "")
child_link = links[child_name]
body = ET.SubElement(
parent_body,
"body",
{
"name": f"{arm}_{child_name}",
**_origin_attributes(joint),
},
)
joint_index = int(joint.get("name", "joint_0").split("_")[-1]) - 1
axis_element = joint.find("axis")
axis = "0 0 1" if axis_element is None else axis_element.get("xyz", "0 0 1")
ET.SubElement(
body,
"joint",
{
"name": f"{arm}_joint_{joint_index + 1}",
"type": "hinge",
"axis": axis,
"range": _format(
[limits.lower[joint_index], limits.upper[joint_index]]
),
"limited": "true",
"damping": "0",
},
)
_add_inertial(body, child_link)
_add_link_visual(body, child_link, arm, single_mesh_keys)
if child_name == "link_7":
ET.SubElement(
body,
"site",
{
"name": f"{arm}_flange",
"type": "sphere",
"size": "0.008",
"rgba": "0.1 0.9 0.2 1" if arm == "left" else "0.1 0.5 0.95 1",
"group": "2",
},
)
ET.SubElement(
body,
"geom",
{
"name": f"{arm}_gripper_visual",
"type": "mesh",
"mesh": gripper_mesh_name,
"pos": "0 0 0.092",
"quat": _format(_quaternion_from_rpy(np.array([-np.pi, np.pi, -np.pi]))),
"material": f"{arm}_gripper",
"contype": "0",
"conaffinity": "0",
"group": "1",
},
)
append_children(body, child_name)
append_children(base_body, "base_link")
def build_normalized_dual_mjcf(
single_urdf_path: Optional[Path | str] = None,
dual_source_path: Optional[Path | str] = None,
) -> Tuple[str, Dict[str, bytes]]:
"""Build a canonical 14-DOF dual-arm MJCF and its in-memory mesh assets."""
single_path = (
Path(single_urdf_path) if single_urdf_path is not None else default_urdf_path()
)
dual_path = (
Path(dual_source_path)
if dual_source_path is not None
else default_dual_source_path()
)
single_root = ET.parse(single_path).getroot()
dual_root = ET.parse(dual_path).getroot()
single_mesh_dir = _single_mesh_directory(single_path)
dual_obj_dir = _dual_obj_directory(dual_path)
mujoco_root = ET.Element("mujoco", {"model": "rm75_normalized_dual_stage2"})
ET.SubElement(
mujoco_root,
"compiler",
{
"angle": "radian",
"autolimits": "true",
"inertiafromgeom": "false",
"balanceinertia": "true",
},
)
ET.SubElement(
mujoco_root,
"option",
{"timestep": "0.002", "gravity": "0 0 -9.81", "integrator": "implicitfast"},
)
ET.SubElement(mujoco_root, "statistic", {"center": "0 0 0.65", "extent": "1.4"})
visual = ET.SubElement(mujoco_root, "visual")
ET.SubElement(
visual,
"global",
{
"azimuth": "90",
"elevation": "-18",
"offwidth": "1280",
"offheight": "720",
},
)
ET.SubElement(visual, "rgba", {"haze": "0.15 0.18 0.2 1"})
assets_element = ET.SubElement(mujoco_root, "asset")
ET.SubElement(assets_element, "material", {"name": "left_arm", "rgba": "0.82 0.84 0.86 1"})
ET.SubElement(assets_element, "material", {"name": "right_arm", "rgba": "0.7 0.74 0.78 1"})
ET.SubElement(assets_element, "material", {"name": "left_gripper", "rgba": "0.15 0.75 0.85 1"})
ET.SubElement(assets_element, "material", {"name": "right_gripper", "rgba": "0.9 0.48 0.18 1"})
ET.SubElement(assets_element, "material", {"name": "platform", "rgba": "0.28 0.31 0.34 1"})
assets: Dict[str, bytes] = {}
single_mesh_keys: Dict[str, str] = {}
for mesh_path in sorted(single_mesh_dir.glob("*.STL")):
asset_key = f"rm75_meshes/{mesh_path.name}"
mesh_name = _mesh_asset_name(mesh_path.name, "rm75")
assets[asset_key] = mesh_path.read_bytes()
single_mesh_keys[mesh_path.name] = mesh_name
ET.SubElement(assets_element, "mesh", {"name": mesh_name, "file": asset_key})
dual_mesh_names: Dict[str, str] = {}
for mesh_path in sorted(dual_obj_dir.glob("*.obj")):
asset_key = f"dual_arm_obj/{mesh_path.name}"
mesh_name = _mesh_asset_name(mesh_path.name, "dual")
assets[asset_key] = mesh_path.read_bytes()
dual_mesh_names[mesh_path.name] = mesh_name
ET.SubElement(assets_element, "mesh", {"name": mesh_name, "file": asset_key})
if len(dual_mesh_names) != 19:
raise ValueError(f"expected 19 dual-arm OBJ assets, found {len(dual_mesh_names)}")
worldbody = ET.SubElement(mujoco_root, "worldbody")
ET.SubElement(
worldbody,
"light",
{
"name": "key_light",
"pos": "0 -1.2 2.8",
"dir": "0 0.4 -1",
"diffuse": "0.9 0.9 0.9",
},
)
ET.SubElement(
worldbody,
"camera",
{
"name": "overview",
"pos": "0 -2.6 1.25",
"xyaxes": "1 0 0 0 0.3 0.953939",
"fovy": "45",
},
)
ET.SubElement(
worldbody,
"geom",
{
"name": "floor",
"type": "plane",
"size": "2.5 2.5 0.05",
"rgba": "0.12 0.14 0.16 1",
"contype": "0",
"conaffinity": "0",
"group": "0",
},
)
robot_base = dual_root.find("./link[@name='robot_base']")
if robot_base is None:
raise ValueError("dual-arm source URDF has no robot_base link")
for visual_element in robot_base.findall("visual"):
mesh_element = visual_element.find("geometry/mesh")
if mesh_element is None:
continue
filename = Path(mesh_element.get("filename", "")).name
ET.SubElement(
worldbody,
"geom",
{
"name": f"platform_{visual_element.get('name', filename)}",
"type": "mesh",
"mesh": dual_mesh_names[filename],
"material": "platform",
"contype": "0",
"conaffinity": "0",
"group": "1",
**_origin_attributes(visual_element),
},
)
mounts = load_dual_arm_mounts(dual_path)
_add_arm(
worldbody,
"left",
mounts.left_base,
single_root,
single_mesh_keys,
dual_mesh_names["dual_arm_gripper1_vis_1.obj"],
)
_add_arm(
worldbody,
"right",
mounts.right_base,
single_root,
single_mesh_keys,
dual_mesh_names["dual_arm_gripper2_vis_1.obj"],
)
for arm, color in (
("left", "0.95 0.12 0.12 0.8"),
("right", "0.95 0.75 0.08 0.8"),
):
marker = ET.SubElement(
worldbody,
"body",
{"name": f"{arm}_target_marker", "mocap": "true", "pos": "0 0 0"},
)
ET.SubElement(
marker,
"geom",
{
"name": f"{arm}_target_marker_geom",
"type": "sphere",
"size": "0.018",
"rgba": color,
"contype": "0",
"conaffinity": "0",
"group": "2",
},
)
ET.indent(mujoco_root, space=" ")
return ET.tostring(mujoco_root, encoding="unicode"), assets

185
ik_qp/src/rm75_ik/mujoco_robot.py Normal file
View File

@ -0,0 +1,185 @@
from __future__ import annotations
from dataclasses import dataclass
from time import sleep
from typing import Collection, Dict, Mapping, Optional
import numpy as np
import pinocchio as pin
from .dual_arm import DualArmAssembly
from .kinematics import RM75Kinematics, pose_errors, validate_se3
from .mujoco_backend import DualArmMuJoCo
from .robot_backend import DualArmJointState
from .solver import RM75IkSolver, deterministic_recovery_seeds
from .teleop_config import ArmName, ArmTeleopProfile
from .types import IkOptions, physical_joint_limits, teleop_joint_limits
@dataclass(frozen=True)
class InitialPoseDiagnostic:
arm: ArmName
solved_q_rad: np.ndarray
solved_position_error_m: float
solved_orientation_error_rad: float
configured_joint_position_error_m: float
configured_joint_orientation_error_rad: float
class MujocoRobot:
"""Dual-arm kinematic backend implementing the backend-neutral robot contract."""
def __init__(
self,
profiles: Mapping[ArmName, ArmTeleopProfile],
*,
initialize_from_tcp: bool = True,
) -> None:
if set(profiles) != {"left", "right"}:
raise ValueError("profiles must contain left and right arms")
self.profiles = dict(profiles)
self.scene = DualArmMuJoCo(controlled_arm="left", limits=teleop_joint_limits())
self.assembly = DualArmAssembly.from_source_urdf()
self._kinematics = {
arm: RM75Kinematics(limits=teleop_joint_limits())
for arm in ("left", "right")
}
self._viewer = None
self._closed = False
self._stopped_arms: set[ArmName] = set()
self.initial_pose_diagnostics: Dict[ArmName, InitialPoseDiagnostic] = {}
if initialize_from_tcp:
self._initialize_from_configured_tcp()
def _initialize_from_configured_tcp(self) -> None:
solved: Dict[ArmName, np.ndarray] = {}
limits = teleop_joint_limits()
physical_kinematics = RM75Kinematics(limits=physical_joint_limits())
for seed_offset, arm in enumerate(("left", "right")):
profile = self.profiles[arm]
kinematics = RM75Kinematics(limits=limits)
solver = RM75IkSolver(kinematics)
flange_target = profile.initial_tcp * profile.tool_from_flange.inverse()
seeds = deterministic_recovery_seeds(
limits, count=24, random_seed=750 + seed_offset
)
if limits.contains(profile.configured_initial_q_rad):
seeds.insert(0, profile.configured_initial_q_rad)
result = solver.solve_multistart(
flange_target,
seeds,
IkOptions(max_iterations=700, time_limit_sec=None),
)
if not result.success or result.q is None:
raise RuntimeError(
f"failed to resolve {arm} initial_tcp_pose: "
f"{result.status.value}: {result.message}"
)
solved[arm] = result.q.copy()
solved_tcp = kinematics.forward(result.q, profile.tool_from_flange)
solved_error = pose_errors(solved_tcp, profile.initial_tcp)
configured_tcp = physical_kinematics.forward(
profile.configured_initial_q_rad, profile.tool_from_flange
)
configured_error = pose_errors(configured_tcp, profile.initial_tcp)
q = result.q.copy()
q.setflags(write=False)
self.initial_pose_diagnostics[arm] = InitialPoseDiagnostic(
arm=arm,
solved_q_rad=q,
solved_position_error_m=solved_error[0],
solved_orientation_error_rad=solved_error[1],
configured_joint_position_error_m=configured_error[0],
configured_joint_orientation_error_rad=configured_error[1],
)
self.scene.set_dual_configuration(solved["left"], solved["right"])
for arm in ("left", "right"):
self.set_target_tcp_pose(arm, self.profiles[arm].initial_tcp)
def connect(self) -> None:
if self._closed:
raise RuntimeError("MujocoRobot is closed")
def read_joint_positions(self) -> DualArmJointState:
self._require_open()
return DualArmJointState(
{
arm: self.scene.get_arm_configuration(arm)
for arm in ("left", "right")
}
)
def command_joint_positions(
self, targets_rad: Mapping[ArmName, np.ndarray]
) -> None:
self._require_open()
if not targets_rad or not set(targets_rad).issubset({"left", "right"}):
raise ValueError("joint targets must contain at least one valid arm")
current = self.read_joint_positions().positions_rad
left = np.asarray(targets_rad.get("left", current["left"]), dtype=float)
right = np.asarray(targets_rad.get("right", current["right"]), dtype=float)
self.scene.set_dual_configuration(left, right)
self._stopped_arms.difference_update(targets_rad)
def set_target_tcp_pose(self, arm: ArmName, target_tcp: pin.SE3) -> None:
self._require_open()
if arm not in ("left", "right"):
raise ValueError("arm must be 'left' or 'right'")
validate_se3(target_tcp, "target_tcp")
mount = (
self.assembly.mounts.left_base
if arm == "left"
else self.assembly.mounts.right_base
)
self.scene.set_arm_target_marker(arm, mount * target_tcp)
def get_tcp_pose(self, arm: ArmName) -> pin.SE3:
self._require_open()
q = self.scene.get_arm_configuration(arm)
return self._kinematics[arm].forward(
q, self.profiles[arm].tool_from_flange
)
def stop(self, arms: Collection[ArmName] = ("left", "right")) -> None:
self._require_open()
selected = set(arms)
if not selected.issubset({"left", "right"}):
raise ValueError("stop arms must contain only left/right")
self.scene.data.qvel[:] = 0.0
self._stopped_arms.update(selected)
def open_viewer(self):
self._require_open()
if self._viewer is None:
import mujoco.viewer
self._viewer = mujoco.viewer.launch_passive(
self.scene.model, self.scene.data
)
return self._viewer
def sync_viewer(self) -> bool:
if self._viewer is None:
return True
if not self._viewer.is_running():
return False
self._viewer.sync()
return True
def render(self, width: int = 1280, height: int = 720) -> np.ndarray:
self._require_open()
return self.scene.render(width, height)
def close(self) -> None:
if self._closed:
return
if self._viewer is not None:
self._viewer.close()
self._viewer = None
sleep(0.2)
self.scene.data.qvel[:] = 0.0
self._closed = True
def _require_open(self) -> None:
if self._closed:
raise RuntimeError("MujocoRobot is closed")

109
ik_qp/src/rm75_ik/mujoco_trajectories.py Normal file
View File

@ -0,0 +1,109 @@
from __future__ import annotations
from math import radians
from typing import Iterable, List
import numpy as np
import pinocchio as pin
from .kinematics import validate_se3
from .solver import RM75IkSolver
from .types import IkOptions
DEMO_TRAJECTORIES = ("joint", "line", "arc", "orientation", "combined")
def se3_target_trajectory(
start_pose: pin.SE3,
target_pose: pin.SE3,
points: int,
) -> List[pin.SE3]:
"""Interpolate translation linearly and orientation on SO(3)."""
if points < 2:
raise ValueError("trajectory must contain at least two points")
validate_se3(start_pose, "start_pose")
validate_se3(target_pose, "target_pose")
rotation_delta = pin.log3(start_pose.rotation.T @ target_pose.rotation)
targets = []
for fraction in np.linspace(0.0, 1.0, points):
translation = (
(1.0 - fraction) * start_pose.translation
+ fraction * target_pose.translation
)
rotation = start_pose.rotation @ pin.exp3(fraction * rotation_delta)
targets.append(pin.SE3(rotation, translation))
return targets
def cartesian_demo_targets(
kind: str,
start_pose: pin.SE3,
points: int = 180,
) -> List[pin.SE3]:
if kind not in DEMO_TRAJECTORIES[1:]:
raise ValueError(f"Cartesian trajectory must be one of {DEMO_TRAJECTORIES[1:]}")
if points < 2:
raise ValueError("trajectory must contain at least two points")
validate_se3(start_pose, "start_pose")
targets: List[pin.SE3] = []
for fraction in np.linspace(0.0, 1.0, points):
translation = start_pose.translation.copy()
rotation = start_pose.rotation.copy()
if kind == "line":
translation += np.array([0.04 * fraction, 0.0, 0.0])
elif kind == "arc":
angle = 0.5 * np.pi * fraction
translation += np.array(
[0.03 * np.sin(angle), 0.03 * (1.0 - np.cos(angle)), 0.0]
)
elif kind == "orientation":
rotation = rotation @ pin.rpy.rpyToMatrix(0.0, 0.0, radians(15) * fraction)
elif kind == "combined":
translation += np.array([0.035 * fraction, 0.015 * fraction, 0.0])
rotation = rotation @ pin.rpy.rpyToMatrix(
radians(8) * fraction,
0.0,
radians(12) * fraction,
)
targets.append(pin.SE3(rotation, translation))
return targets
def joint_demo_trajectory(
start_q_rad: np.ndarray,
points: int = 180,
) -> np.ndarray:
start = np.asarray(start_q_rad, dtype=float)
if start.shape != (7,) or not np.all(np.isfinite(start)):
raise ValueError("start_q_rad must be a finite shape-(7,) vector")
if points < 2:
raise ValueError("trajectory must contain at least two points")
offset = np.deg2rad([12.0, -8.0, 10.0, 8.0, -6.0, 7.0, 10.0])
blend = 0.5 - 0.5 * np.cos(np.linspace(0.0, np.pi, points))
return start[None, :] + blend[:, None] * offset[None, :]
def solve_pose_trajectory(
solver: RM75IkSolver,
targets: Iterable[pin.SE3],
seed_q_rad: np.ndarray,
options: IkOptions = IkOptions(
position_tolerance_m=9e-4,
orientation_tolerance_rad=radians(0.09),
max_iterations=200,
),
) -> np.ndarray:
seed = np.asarray(seed_q_rad, dtype=float).copy()
solutions = []
for index, target in enumerate(targets):
result = solver.solve(target, seed, options)
if not result.success or result.q is None:
raise RuntimeError(
f"IK failed at trajectory point {index}: "
f"{result.status.value} {result.message}"
)
seed = result.q.copy()
solutions.append(seed)
return np.asarray(solutions)

43
ik_qp/src/rm75_ik/robot_backend.py Normal file
View File

@ -0,0 +1,43 @@
from __future__ import annotations
from dataclasses import dataclass
from typing import Collection, Dict, Mapping, Protocol, runtime_checkable
import numpy as np
import pinocchio as pin
from .teleop_config import ArmName
@dataclass(frozen=True)
class DualArmJointState:
positions_rad: Mapping[ArmName, np.ndarray]
def __post_init__(self) -> None:
normalized: Dict[ArmName, np.ndarray] = {}
if set(self.positions_rad) != {"left", "right"}:
raise ValueError("dual-arm state must contain left and right positions")
for arm, values in self.positions_rad.items():
q = np.asarray(values, dtype=float).copy()
if q.shape != (7,) or not np.all(np.isfinite(q)):
raise ValueError(f"{arm} joint state must be finite with shape (7,)")
q.setflags(write=False)
normalized[arm] = q
object.__setattr__(self, "positions_rad", normalized)
@runtime_checkable
class RobotBackend(Protocol):
def connect(self) -> None: ...
def read_joint_positions(self) -> DualArmJointState: ...
def command_joint_positions(
self, targets_rad: Mapping[ArmName, np.ndarray]
) -> None: ...
def set_target_tcp_pose(self, arm: ArmName, target_tcp: pin.SE3) -> None: ...
def stop(self, arms: Collection[ArmName] = ("left", "right")) -> None: ...
def close(self) -> None: ...

80
ik_qp/src/rm75_ik/stage2_cli.py Normal file
View File

@ -0,0 +1,80 @@
from __future__ import annotations
import argparse
import os
import sys
from pathlib import Path
from typing import Optional, Sequence
os.environ.setdefault("MUJOCO_GL", "egl")
def _default_output_dir() -> Path:
package_root = Path(__file__).resolve().parents[2]
if (package_root / "pyproject.toml").is_file():
return package_root / "artifacts" / "stage2"
return Path.cwd() / "stage2_artifacts"
def build_parser() -> argparse.ArgumentParser:
parser = argparse.ArgumentParser(
description="Headless single-arm IK validation in the normalized dual RM75 scene"
)
parser.add_argument("--arm", choices=("left", "right"), default="left")
parser.add_argument(
"--sdk-root",
type=Path,
help="directory containing the RealMan Robotic_Arm Python package",
)
parser.add_argument("--output-dir", type=Path, default=_default_output_dir())
parser.add_argument("--seed", type=int, default=20260630)
parser.add_argument("--quick", action="store_true")
parser.add_argument(
"--report-only",
action="store_true",
help="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)
from .realman_reference import RealManFkReference
from .stage2_validation import (
Stage2Settings,
Stage2Validator,
write_stage2_report,
)
settings = (
Stage2Settings.quick(seed=args.seed)
if args.quick
else Stage2Settings(seed=args.seed)
)
validator = Stage2Validator(
RealManFkReference(args.sdk_root),
controlled_arm=args.arm,
settings=settings,
)
summary = validator.run()
json_path, csv_path, markdown_path, image_paths = write_stage2_report(
args.output_dir,
summary,
validator.failures,
validator.images,
)
print(f"RM75-B stage-2 validation: {'PASS' if summary['passed'] else 'FAIL'}")
for name, check in summary["checks"].items():
print(f" [{'PASS' if check['passed'] else 'FAIL'}] {name}")
print("Reports:")
for path in (json_path, csv_path, markdown_path, *image_paths):
print(f" {path}")
if args.report_only or summary["passed"]:
return 0
return 1
if __name__ == "__main__":
sys.exit(main())

232
ik_qp/src/rm75_ik/stage2_demo.py Normal file
View File

@ -0,0 +1,232 @@
from __future__ import annotations
import argparse
import os
import sys
from pathlib import Path
from time import perf_counter, sleep
from typing import Optional, Sequence
def build_parser() -> argparse.ArgumentParser:
parser = argparse.ArgumentParser(
description="Visualize first-stage IK in the normalized dual RM75 scene"
)
parser.add_argument("--arm", choices=("left", "right"), default="left")
parser.add_argument(
"--trajectory",
choices=("joint", "line", "arc", "orientation", "combined"),
default="combined",
)
parser.add_argument("--points", type=int, default=180)
parser.add_argument("--dt", type=float, default=1.0 / 90.0)
parser.add_argument(
"--target-position",
nargs=3,
type=float,
metavar=("X", "Y", "Z"),
help="target position in the controlled-arm base frame, in meters",
)
parser.add_argument(
"--target-rpy",
nargs=3,
type=float,
metavar=("ROLL", "PITCH", "YAW"),
help="target RPY in radians; defaults to the start orientation",
)
parser.add_argument(
"--duration",
type=float,
default=8.0,
help="target-point motion duration in seconds",
)
parser.add_argument("--wait-before", type=float, default=2.0)
parser.add_argument("--hold-after", type=float, default=3.0)
parser.add_argument(
"--manual-drag",
action="store_true",
help="enable zero-gravity mouse perturbation of the controlled arm",
)
parser.add_argument(
"--drag-damping",
type=float,
default=1.5,
help="joint damping used by manual-drag mode",
)
parser.add_argument(
"--headless",
action="store_true",
help="render the final frame without opening the interactive viewer",
)
parser.add_argument(
"--output",
type=Path,
default=Path("stage2_demo.png"),
help="headless output image",
)
parser.add_argument(
"--close-after-play",
action="store_true",
help="close the interactive viewer after one playback (smoke testing)",
)
return parser
def main(argv: Optional[Sequence[str]] = None) -> int:
args = build_parser().parse_args(argv)
if args.manual_drag and args.headless:
raise SystemExit("--manual-drag requires the interactive viewer")
if args.target_rpy is not None and args.target_position is None:
raise SystemExit("--target-rpy requires --target-position")
for name in ("dt", "duration", "wait_before", "hold_after"):
if getattr(args, name) < 0.0 or (name in {"dt", "duration"} and getattr(args, name) == 0.0):
raise SystemExit(f"--{name.replace('_', '-')} must be positive")
if args.headless:
os.environ.setdefault("MUJOCO_GL", "egl")
else:
os.environ.setdefault("MUJOCO_GL", "glfw")
import mujoco.viewer
import numpy as np
import pinocchio as pin
from PIL import Image
from .dual_arm import DualArmAssembly
from .mujoco_backend import CONTROLLED_HOME_Q_RAD, DualArmMuJoCo
from .mujoco_trajectories import (
cartesian_demo_targets,
joint_demo_trajectory,
se3_target_trajectory,
solve_pose_trajectory,
)
from .kinematics import RM75Kinematics, pose_errors
from .solver import RM75IkSolver
from .types import teleop_joint_limits
scene = DualArmMuJoCo(controlled_arm=args.arm)
if args.manual_drag:
scene.configure_manual_drag(args.drag_damping)
print("Manual drag mode")
print(" 1. Double-click a link to select it.")
print(" 2. Hold Ctrl and drag with the right mouse button.")
print(" 3. Close the viewer or press Ctrl+C to finish.")
viewer = mujoco.viewer.launch_passive(scene.model, scene.data)
try:
while viewer.is_running():
step_started = perf_counter()
scene.step_manual_drag()
viewer.sync()
remaining = scene.model.opt.timestep - (perf_counter() - step_started)
if remaining > 0.0:
sleep(remaining)
except KeyboardInterrupt:
pass
finally:
viewer.close()
sleep(0.2)
print("Final controlled-arm joints (rad):")
print(scene.get_arm_configuration(args.arm).tolist())
return 0
kinematics = RM75Kinematics(limits=teleop_joint_limits())
solver = RM75IkSolver(kinematics)
targets = None
target_pose = None
if args.target_position is not None:
start_pose = kinematics.forward(CONTROLLED_HOME_Q_RAD)
rotation = (
start_pose.rotation
if args.target_rpy is None
else pin.rpy.rpyToMatrix(*args.target_rpy)
)
target_pose = pin.SE3(rotation, np.asarray(args.target_position, dtype=float))
target_points = max(2, int(round(args.duration / args.dt)) + 1)
try:
targets = se3_target_trajectory(start_pose, target_pose, target_points)
trajectory = solve_pose_trajectory(
solver, targets, CONTROLLED_HOME_Q_RAD
)
except (RuntimeError, TypeError, ValueError) as exc:
raise SystemExit(f"target trajectory rejected: {exc}") from exc
print("Target mode")
print(" position (m):", target_pose.translation.tolist())
print(" rpy (rad):", pin.rpy.matrixToRpy(target_pose.rotation).tolist())
print(f" duration: {args.duration:.3f} s, points: {target_points}")
elif args.trajectory == "joint":
trajectory = joint_demo_trajectory(CONTROLLED_HOME_Q_RAD, args.points)
else:
targets = cartesian_demo_targets(
args.trajectory,
kinematics.forward(CONTROLLED_HOME_Q_RAD),
args.points,
)
trajectory = solve_pose_trajectory(
solver, targets, CONTROLLED_HOME_Q_RAD
)
assembly = DualArmAssembly.from_source_urdf()
mount = (
assembly.mounts.left_base
if args.arm == "left"
else assembly.mounts.right_base
)
if target_pose is not None:
scene.set_target_marker(mount * target_pose)
elif targets is not None:
scene.set_target_marker(mount * targets[-1])
else:
scene.set_arm_configuration(args.arm, trajectory[-1])
scene.set_target_marker(scene.get_flange_pose(args.arm))
scene.set_arm_configuration(args.arm, trajectory[0])
if args.headless:
result = scene.play_trajectory(trajectory, dt=args.dt, realtime=False)
args.output.parent.mkdir(parents=True, exist_ok=True)
Image.fromarray(scene.render()).save(args.output)
print(
f"Played {result.samples} samples; max joint step "
f"{result.max_joint_step_rad:.6f} rad"
)
if target_pose is not None:
actual_local = mount.actInv(result.final_flange_pose)
position_error, orientation_error = pose_errors(actual_local, target_pose)
print(f"Final position error: {position_error:.9f} m")
print(f"Final orientation error: {orientation_error:.9f} rad")
print(args.output)
return 0
viewer = mujoco.viewer.launch_passive(scene.model, scene.data)
try:
wait_until = perf_counter() + args.wait_before
while viewer.is_running() and perf_counter() < wait_until:
viewer.sync()
sleep(0.02)
scene.play_trajectory(
trajectory,
dt=args.dt,
realtime=True,
viewer=viewer,
)
if target_pose is not None:
actual_local = mount.actInv(scene.get_flange_pose(args.arm))
position_error, orientation_error = pose_errors(actual_local, target_pose)
print(f"Final position error: {position_error:.9f} m")
print(f"Final orientation error: {orientation_error:.9f} rad")
hold_until = perf_counter() + args.hold_after
while viewer.is_running() and perf_counter() < hold_until:
viewer.sync()
sleep(0.02)
if not args.close_after_play:
while viewer.is_running():
sleep(0.05)
except KeyboardInterrupt:
pass
finally:
viewer.close()
# GLFW tears down asynchronously; allow its render thread to exit.
sleep(0.2)
return 0
if __name__ == "__main__":
sys.exit(main())

595
ik_qp/src/rm75_ik/stage2_validation.py Normal file
View File

@ -0,0 +1,595 @@
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, List, Optional, Tuple
import mujoco
import numpy as np
import pinocchio as pin
from PIL import Image
from .dual_arm import DualArmAssembly
from .kinematics import RM75Kinematics, pose_errors
from .mujoco_backend import CONTROLLED_HOME_Q_RAD, DualArmMuJoCo
from .mujoco_trajectories import (
DEMO_TRAJECTORIES,
cartesian_demo_targets,
joint_demo_trajectory,
solve_pose_trajectory,
)
from .realman_reference import RealManFkReference
from .solver import RM75IkSolver
from .types import IkOptions, physical_joint_limits, teleop_joint_limits
MUJOCO_PIN_POSITION_LIMIT_M = 1e-9
MUJOCO_PIN_ORIENTATION_LIMIT_RAD = 1e-9
ALGO_FK_POSITION_LIMIT_M = 1e-4
ALGO_FK_ORIENTATION_LIMIT_RAD = radians(0.01)
IK_POSITION_LIMIT_M = 1e-3
IK_ORIENTATION_LIMIT_RAD = radians(0.1)
NEAR_IK_RATE_LIMIT = 0.995
CONTINUOUS_IK_RATE_LIMIT = 0.999
MAX_JOINT_STEP_RAD = radians(2.0)
INACTIVE_ARM_DELTA_LIMIT_RAD = 1e-12
@dataclass(frozen=True)
class Stage2Settings:
seed: int = 20260630
fk_samples: int = 10_000
ik_samples: int = 1_000
trajectories: int = 20
trajectory_points: int = 500
render_width: int = 1280
render_height: int = 720
@classmethod
def quick(cls, seed: int = 20260630) -> "Stage2Settings":
return cls(
seed=seed,
fk_samples=100,
ik_samples=30,
trajectories=2,
trajectory_points=25,
render_width=640,
render_height=360,
)
def _sample_configurations(
rng: np.random.Generator,
lower: np.ndarray,
upper: np.ndarray,
count: int,
margin: Optional[np.ndarray] = None,
) -> np.ndarray:
selected_margin = np.zeros(7) if margin is None else np.asarray(margin)
return rng.uniform(
lower + selected_margin,
upper - selected_margin,
size=(count, 7),
)
def _percentile(values: List[float], percentile: float) -> float:
return float(np.percentile(values, percentile)) if values else float("nan")
class Stage2Validator:
def __init__(
self,
reference: RealManFkReference,
controlled_arm: str = "left",
settings: Stage2Settings = Stage2Settings(),
) -> None:
self.reference = reference
self.controlled_arm = controlled_arm
self.settings = settings
self.rng = np.random.default_rng(settings.seed)
self.scene = DualArmMuJoCo(controlled_arm=controlled_arm)
self.assembly = DualArmAssembly.from_source_urdf()
self.teleop_kinematics = RM75Kinematics(limits=teleop_joint_limits())
self.solver = RM75IkSolver(self.teleop_kinematics)
self.mount = (
self.assembly.mounts.left_base
if controlled_arm == "left"
else self.assembly.mounts.right_base
)
self._inactive_initial = self.scene.get_arm_configuration(self.scene.inactive_arm)
self._max_inactive_delta = 0.0
self.checks: Dict[str, Dict[str, Any]] = {}
self.failures: List[Dict[str, Any]] = []
self.images: Dict[str, Tuple[np.ndarray, np.ndarray]] = {}
def _check_inactive_arm(self) -> None:
delta = float(
np.max(
np.abs(
self.scene.get_arm_configuration(self.scene.inactive_arm)
- self._inactive_initial
)
)
)
self._max_inactive_delta = max(self._max_inactive_delta, delta)
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"),
) -> None:
if len(self.failures) >= 1000:
return
self.failures.append(
{
"category": category,
"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]) -> None:
self.checks[name] = {"passed": bool(passed), "required": True, **metrics}
def _local_mujoco_pose(self) -> pin.SE3:
return self.mount.actInv(self.scene.get_flange_pose(self.controlled_arm))
def run(self) -> Dict[str, Any]:
self._model_structure_check()
self._fk_check()
self._single_point_ik_check()
self._continuous_ik_check()
self._trajectory_scenario_check()
self._visual_check()
self._add_check(
"inactive_arm_fixed",
self._max_inactive_delta < INACTIVE_ARM_DELTA_LIMIT_RAD,
{
"arm": self.scene.inactive_arm,
"max_qpos_delta_rad": self._max_inactive_delta,
"limit_rad": INACTIVE_ARM_DELTA_LIMIT_RAD,
},
)
passed = all(check["passed"] for check in self.checks.values())
return {
"schema_version": 1,
"generated_at": datetime.now(timezone.utc).isoformat(),
"seed": self.settings.seed,
"controlled_arm": self.controlled_arm,
"inactive_arm": self.scene.inactive_arm,
"realman_api_version": self.reference.api_version,
"mujoco_version": mujoco.__version__,
"passed": passed,
"checks": self.checks,
"failure_count": len(self.failures),
}
def _model_structure_check(self) -> None:
model = self.scene.model
joint_names = [model.joint(index).name for index in range(model.njnt)]
expected = [
f"{arm}_joint_{joint_index}"
for arm in ("left", "right")
for joint_index in range(1, 8)
]
obj_assets = [key for key in self.scene.assets if key.endswith(".obj")]
stl_assets = [key for key in self.scene.assets if key.endswith(".STL")]
passed = (
model.nq == model.nv == model.njnt == 14
and model.nu == 0
and model.nsite == 2
and model.nmesh == 27
and joint_names == expected
and len(obj_assets) == 19
and len(stl_assets) == 8
)
self._add_check(
"model_structure",
passed,
{
"nq": model.nq,
"nv": model.nv,
"njnt": model.njnt,
"nu": model.nu,
"nmesh": model.nmesh,
"obj_assets": len(obj_assets),
"stl_assets": len(stl_assets),
},
)
def _fk_check(self) -> None:
limits = physical_joint_limits()
samples = _sample_configurations(
self.rng, limits.lower, limits.upper, self.settings.fk_samples
)
pin_position_errors: List[float] = []
pin_orientation_errors: List[float] = []
algo_position_errors: List[float] = []
algo_orientation_errors: List[float] = []
for index, q in enumerate(samples):
self.scene.set_arm_configuration(self.controlled_arm, q)
self._check_inactive_arm()
mujoco_world = self.scene.get_flange_pose(self.controlled_arm)
pin_world = self.assembly.forward(self.controlled_arm, q)
pin_position, pin_orientation = pose_errors(mujoco_world, pin_world)
local_mujoco = self.mount.actInv(mujoco_world)
algo_position, algo_orientation = pose_errors(
local_mujoco, self.reference.forward(q)
)
pin_position_errors.append(pin_position)
pin_orientation_errors.append(pin_orientation)
algo_position_errors.append(algo_position)
algo_orientation_errors.append(algo_orientation)
if (
pin_position >= MUJOCO_PIN_POSITION_LIMIT_M
or pin_orientation >= MUJOCO_PIN_ORIENTATION_LIMIT_RAD
or algo_position >= ALGO_FK_POSITION_LIMIT_M
or algo_orientation >= ALGO_FK_ORIENTATION_LIMIT_RAD
):
self._record_failure(
"fk",
index,
"MuJoCo FK residual exceeded an acceptance limit",
q,
algo_position,
algo_orientation,
)
max_pin_position = max(pin_position_errors, default=float("inf"))
max_pin_orientation = max(pin_orientation_errors, default=float("inf"))
max_algo_position = max(algo_position_errors, default=float("inf"))
max_algo_orientation = max(algo_orientation_errors, default=float("inf"))
self._add_check(
"fk",
max_pin_position < MUJOCO_PIN_POSITION_LIMIT_M
and max_pin_orientation < MUJOCO_PIN_ORIENTATION_LIMIT_RAD
and max_algo_position < ALGO_FK_POSITION_LIMIT_M
and max_algo_orientation < ALGO_FK_ORIENTATION_LIMIT_RAD,
{
"samples": len(samples),
"max_mujoco_pin_position_error_m": max_pin_position,
"max_mujoco_pin_orientation_error_rad": max_pin_orientation,
"max_mujoco_algo_position_error_m": max_algo_position,
"max_mujoco_algo_orientation_error_rad": max_algo_orientation,
},
)
@staticmethod
def _ik_options(max_iterations: int) -> IkOptions:
return IkOptions(
position_tolerance_m=0.9 * IK_POSITION_LIMIT_M,
orientation_tolerance_rad=0.9 * IK_ORIENTATION_LIMIT_RAD,
max_iterations=max_iterations,
)
def _single_point_ik_check(self) -> None:
limits = teleop_joint_limits()
margin = np.deg2rad([5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 10.0])
targets_q = _sample_configurations(
self.rng,
limits.lower,
limits.upper,
self.settings.ik_samples,
margin,
)
successes = 0
solve_times: List[float] = []
max_position = 0.0
max_orientation = 0.0
for index, target_q in enumerate(targets_q):
target = self.reference.forward(target_q)
seed = np.clip(
target_q + self.rng.uniform(-radians(10), radians(10), 7),
limits.lower,
limits.upper,
)
result = self.solver.solve(target, seed, self._ik_options(200))
solve_times.append(result.solve_time_sec)
if result.success and result.q is not None:
self.scene.set_arm_configuration(self.controlled_arm, result.q)
self._check_inactive_arm()
position_error, orientation_error = pose_errors(
self._local_mujoco_pose(), target
)
max_position = max(max_position, position_error)
max_orientation = max(max_orientation, orientation_error)
if (
position_error <= IK_POSITION_LIMIT_M
and orientation_error <= IK_ORIENTATION_LIMIT_RAD
):
successes += 1
continue
else:
position_error = result.position_error_m
orientation_error = result.orientation_error_rad
self._record_failure(
"single_point_ik",
index,
f"status={result.status.value}; {result.message}",
seed,
position_error,
orientation_error,
)
rate = successes / max(len(targets_q), 1)
self._add_check(
"single_point_ik",
rate >= NEAR_IK_RATE_LIMIT,
{
"samples": len(targets_q),
"successes": successes,
"success_rate": rate,
"max_position_error_m": max_position,
"max_orientation_error_rad": max_orientation,
"p99_solve_time_sec": _percentile(solve_times, 99),
"max_solve_time_sec": max(solve_times, default=float("nan")),
},
)
def _continuous_ik_check(self) -> None:
limits = teleop_joint_limits()
successes = 0
total = 0
max_joint_step = 0.0
max_position = 0.0
max_orientation = 0.0
for trajectory_index in range(self.settings.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.trajectory_points) / 90.0
target_path = center + amplitude * np.sin(
2.0 * np.pi * times[:, None] * frequency + phase
)
seed = target_path[0].copy()
previous = seed.copy()
for point_index, target_q in enumerate(target_path):
total += 1
target = self.reference.forward(target_q)
result = self.solver.solve(target, seed, self._ik_options(100))
if result.success and result.q is not None:
self.scene.set_arm_configuration(self.controlled_arm, result.q)
self._check_inactive_arm()
position_error, orientation_error = pose_errors(
self._local_mujoco_pose(), target
)
joint_step = float(np.max(np.abs(result.q - previous)))
max_position = max(max_position, position_error)
max_orientation = max(max_orientation, orientation_error)
max_joint_step = max(max_joint_step, joint_step)
if (
position_error <= IK_POSITION_LIMIT_M
and orientation_error <= IK_ORIENTATION_LIMIT_RAD
):
successes += 1
seed = result.q
previous = result.q
continue
else:
position_error = result.position_error_m
orientation_error = result.orientation_error_rad
self._record_failure(
"continuous_ik",
trajectory_index * self.settings.trajectory_points + point_index,
f"status={result.status.value}; {result.message}",
seed,
position_error,
orientation_error,
)
rate = successes / max(total, 1)
self._add_check(
"continuous_ik",
rate >= CONTINUOUS_IK_RATE_LIMIT
and max_joint_step < MAX_JOINT_STEP_RAD,
{
"trajectories": self.settings.trajectories,
"points": total,
"successes": successes,
"success_rate": rate,
"max_position_error_m": max_position,
"max_orientation_error_rad": max_orientation,
"max_joint_step_rad": max_joint_step,
"joint_step_limit_rad": MAX_JOINT_STEP_RAD,
},
)
def _trajectory_scenario_check(self) -> None:
scenario_results: Dict[str, bool] = {}
start = CONTROLLED_HOME_Q_RAD.copy()
for kind in DEMO_TRAJECTORIES:
try:
if kind == "joint":
q_trajectory = joint_demo_trajectory(start, 120)
else:
targets = cartesian_demo_targets(
kind, self.teleop_kinematics.forward(start), 120
)
q_trajectory = solve_pose_trajectory(self.solver, targets, start)
self.scene.play_trajectory(q_trajectory)
self._check_inactive_arm()
scenario_results[kind] = True
except (RuntimeError, ValueError) as exc:
scenario_results[kind] = False
self._record_failure("trajectory_scenario", 0, f"{kind}: {exc}")
limits = teleop_joint_limits()
scenario_q = {
"limit_near": 0.8 * limits.upper + 0.2 * CONTROLLED_HOME_Q_RAD,
"singularity_near": np.deg2rad([0, 0, 0, 90, 0, 0, 0]),
}
blend = 0.5 - 0.5 * np.cos(np.linspace(0.0, np.pi, 120))
for name, endpoint in scenario_q.items():
path = start[None, :] + blend[:, None] * (endpoint - start)[None, :]
try:
targets = [self.reference.forward(q) for q in path]
solutions = solve_pose_trajectory(self.solver, targets, start)
self.scene.play_trajectory(solutions)
self._check_inactive_arm()
scenario_results[name] = True
except (RuntimeError, ValueError) as exc:
scenario_results[name] = False
self._record_failure("trajectory_scenario", 0, f"{name}: {exc}")
self._add_check(
"trajectory_scenarios",
all(scenario_results.values()),
{"scenarios": scenario_results},
)
def _visible_geom_names(self, segmentation: np.ndarray) -> set[str]:
pairs = np.unique(segmentation.reshape(-1, 2), axis=0)
names = set()
for object_id, object_type in pairs:
if object_type != mujoco.mjtObj.mjOBJ_GEOM or object_id < 0:
continue
name = mujoco.mj_id2name(
self.scene.model, mujoco.mjtObj.mjOBJ_GEOM, int(object_id)
)
if name is not None:
names.add(name)
return names
@staticmethod
def _segmentation_preview(segmentation: np.ndarray) -> np.ndarray:
object_ids = segmentation[:, :, 0]
preview = np.zeros((*object_ids.shape, 3), dtype=np.uint8)
foreground = object_ids >= 0
ids = object_ids[foreground].astype(np.uint32)
preview[foreground, 0] = (37 * ids + 53) % 255
preview[foreground, 1] = (97 * ids + 101) % 255
preview[foreground, 2] = (17 * ids + 211) % 255
return preview
def _visual_check(self) -> None:
start = CONTROLLED_HOME_Q_RAD.copy()
targets = cartesian_demo_targets(
"combined", self.teleop_kinematics.forward(start), 121
)
q_trajectory = solve_pose_trajectory(self.solver, targets, start)
all_frames_pass = True
frame_metrics: Dict[str, Dict[str, Any]] = {}
for label, index in (("start", 0), ("middle", 60), ("end", 120)):
self.scene.set_arm_configuration(self.controlled_arm, q_trajectory[index])
self.scene.set_target_marker(self.mount * targets[index])
self._check_inactive_arm()
rgb = self.scene.render(
self.settings.render_width, self.settings.render_height
)
segmentation = self.scene.render(
self.settings.render_width,
self.settings.render_height,
segmentation=True,
)
visible_names = self._visible_geom_names(segmentation)
left_visible = any(name.startswith("left_") for name in visible_names)
right_visible = any(name.startswith("right_") for name in visible_names)
arm_ids = {
mujoco.mj_name2id(
self.scene.model, mujoco.mjtObj.mjOBJ_GEOM, name
)
for name in visible_names
if name.startswith(("left_", "right_"))
}
mask = np.isin(segmentation[:, :, 0], list(arm_ids))
rows, columns = np.where(mask)
not_touching_border = bool(
len(rows)
and rows.min() > 1
and columns.min() > 1
and rows.max() < rgb.shape[0] - 2
and columns.max() < rgb.shape[1] - 2
)
frame_pass = (
float(rgb.std()) > 5.0
and left_visible
and right_visible
and not_touching_border
)
all_frames_pass &= frame_pass
frame_metrics[label] = {
"rgb_std": float(rgb.std()),
"left_visible": left_visible,
"right_visible": right_visible,
"not_touching_border": not_touching_border,
"visible_geom_count": len(visible_names),
}
self.images[label] = (rgb, self._segmentation_preview(segmentation))
self._add_check("visual_rendering", all_frames_pass, {"frames": frame_metrics})
def write_stage2_report(
output_dir: Path | str,
summary: Dict[str, Any],
failures: List[Dict[str, Any]],
images: Dict[str, Tuple[np.ndarray, np.ndarray]],
) -> Tuple[Path, Path, Path, List[Path]]:
directory = Path(output_dir)
directory.mkdir(parents=True, exist_ok=True)
json_path = directory / "stage2_summary.json"
csv_path = directory / "stage2_failures.csv"
markdown_path = directory / "stage2_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")
fields = [
"category",
"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=fields)
writer.writeheader()
writer.writerows(failures)
image_paths: List[Path] = []
for label, (rgb, segmentation) in images.items():
for suffix, image in (("rgb", rgb), ("segmentation", segmentation)):
path = directory / f"stage2_{label}_{suffix}.png"
Image.fromarray(image).save(path)
image_paths.append(path)
lines = [
"# RM75-B Stage 2 MuJoCo Validation",
"",
f"- Overall: **{'PASS' if summary['passed'] else 'FAIL'}**",
f"- Controlled arm: `{summary['controlled_arm']}`",
f"- Seed: `{summary['seed']}`",
f"- MuJoCo: `{summary['mujoco_version']}`",
f"- RealMan API: `{summary['realman_api_version']}`",
f"- Failures recorded: `{summary['failure_count']}`",
"",
"| Check | Result | Metrics |",
"|---|---:|---|",
]
for name, check in summary["checks"].items():
metrics = {
key: value
for key, value in check.items()
if key not in {"passed", "required"}
}
lines.append(
f"| `{name}` | {'PASS' if check['passed'] else 'FAIL'} | "
f"`{json.dumps(metrics, ensure_ascii=True, sort_keys=True)}` |"
)
markdown_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
return json_path, csv_path, markdown_path, image_paths

78
ik_qp/src/rm75_ik/stage3_cli.py Normal file
View File

@ -0,0 +1,78 @@
from __future__ import annotations
import argparse
import os
import sys
from pathlib import Path
from typing import Optional, Sequence
os.environ.setdefault("MUJOCO_GL", "egl")
def _source_root() -> Path:
return Path(__file__).resolve().parents[3]
def build_parser() -> argparse.ArgumentParser:
root = _source_root()
parser = argparse.ArgumentParser(
description="Validate right-arm and dual-grip QP control in MuJoCo"
)
parser.add_argument("--sdk-root", type=Path)
parser.add_argument(
"--teleop-config",
type=Path,
default=root / "xr_rm_bringup/config/dual_arm_rm75.yaml",
)
parser.add_argument(
"--peripheral-config",
type=Path,
default=root / "xr_rm_bringup/config/peripherals_rm75.yaml",
)
parser.add_argument(
"--output-dir",
type=Path,
default=root / "ik_qp/artifacts/stage3",
)
parser.add_argument("--seed", type=int, default=20260630)
parser.add_argument("--quick", action="store_true")
parser.add_argument("--report-only", action="store_true")
return parser
def main(argv: Optional[Sequence[str]] = None) -> int:
args = build_parser().parse_args(argv)
from .realman_reference import RealManFkReference
from .stage3_validation import (
Stage3Settings,
Stage3Validator,
write_stage3_report,
)
settings = (
Stage3Settings.quick(args.seed)
if args.quick
else Stage3Settings(seed=args.seed)
)
validator = Stage3Validator(
RealManFkReference(args.sdk_root),
args.teleop_config,
args.peripheral_config,
settings,
)
summary = validator.run()
paths = write_stage3_report(
args.output_dir, summary, validator.failures, validator.images
)
print(f"RM75-B stage-3 validation: {'PASS' if summary['passed'] else 'FAIL'}")
for name, check in summary["checks"].items():
print(f" [{'PASS' if check['passed'] else 'FAIL'}] {name}")
print("Reports:")
for path in (*paths[:3], *paths[3]):
print(f" {path}")
return 0 if args.report_only or summary["passed"] else 1
if __name__ == "__main__":
sys.exit(main())

311
ik_qp/src/rm75_ik/stage3_validation.py Normal file
View File

@ -0,0 +1,311 @@
from __future__ import annotations
import csv
import json
from dataclasses import dataclass
from datetime import datetime, timezone
from math import degrees, radians
from pathlib import Path
from typing import Any, Dict, List, Tuple
import mujoco
import numpy as np
from PIL import Image
from .mujoco_robot import MujocoRobot
from .realman_reference import RealManFkReference
from .stage2_validation import Stage2Settings, Stage2Validator
from .teleop_config import load_dual_arm_profiles
from .teleop_control import (
ControllerSample,
DualArmQpTeleopController,
SafetyState,
)
@dataclass(frozen=True)
class Stage3Settings:
seed: int = 20260630
fk_samples: int = 2_000
ik_samples: int = 200
trajectories: int = 5
trajectory_points: int = 200
render_width: int = 1280
render_height: int = 720
@classmethod
def quick(cls, seed: int = 20260630) -> "Stage3Settings":
return cls(
seed=seed,
fk_samples=100,
ik_samples=30,
trajectories=2,
trajectory_points=25,
render_width=640,
render_height=360,
)
def stage2_settings(self) -> Stage2Settings:
return Stage2Settings(
seed=self.seed,
fk_samples=self.fk_samples,
ik_samples=self.ik_samples,
trajectories=self.trajectories,
trajectory_points=self.trajectory_points,
render_width=self.render_width,
render_height=self.render_height,
)
class Stage3Validator:
def __init__(
self,
reference: RealManFkReference,
teleop_config_path: Path | str,
peripheral_config_path: Path | str,
settings: Stage3Settings = Stage3Settings(),
) -> None:
self.reference = reference
self.settings = settings
self.profiles = load_dual_arm_profiles(
teleop_config_path, peripheral_config_path
)
self.checks: Dict[str, Dict[str, Any]] = {}
self.failures: List[Dict[str, Any]] = []
self.images: Dict[str, np.ndarray] = {}
def run(self) -> Dict[str, Any]:
right_validator = Stage2Validator(
self.reference,
controlled_arm="right",
settings=self.settings.stage2_settings(),
)
right_summary = right_validator.run()
self.failures.extend(right_validator.failures)
for label, (rgb, segmentation) in right_validator.images.items():
self.images[f"right_{label}_rgb"] = rgb
self.images[f"right_{label}_segmentation"] = segmentation
self.checks["right_arm_validation"] = {
"passed": right_summary["passed"],
"required": True,
"checks": right_summary["checks"],
}
robot = MujocoRobot(self.profiles)
try:
self._initial_tcp_check(robot)
self._dual_grip_control_check(robot)
image = robot.render(
self.settings.render_width, self.settings.render_height
)
self.images["dual_control_final_rgb"] = image
self.checks["dual_control_render"] = {
"passed": float(image.std()) > 5.0,
"required": True,
"rgb_std": float(image.std()),
}
finally:
robot.close()
passed = all(check["passed"] for check in self.checks.values())
return {
"schema_version": 1,
"generated_at": datetime.now(timezone.utc).isoformat(),
"seed": self.settings.seed,
"mujoco_version": mujoco.__version__,
"realman_api_version": self.reference.api_version,
"passed": passed,
"checks": self.checks,
"failure_count": len(self.failures),
}
def _initial_tcp_check(self, robot: MujocoRobot) -> None:
metrics: Dict[str, Dict[str, Any]] = {}
passed = True
for arm in ("left", "right"):
diagnostic = robot.initial_pose_diagnostics[arm]
arm_passed = (
diagnostic.solved_position_error_m <= 1e-3
and diagnostic.solved_orientation_error_rad <= radians(0.1)
)
passed &= arm_passed
metrics[arm] = {
"active_tool": self.profiles[arm].active_tool_name,
"solved_q_deg": np.rad2deg(diagnostic.solved_q_rad).tolist(),
"solved_position_error_m": diagnostic.solved_position_error_m,
"solved_orientation_error_deg": degrees(
diagnostic.solved_orientation_error_rad
),
"configured_joint_position_error_m": (
diagnostic.configured_joint_position_error_m
),
"configured_joint_orientation_error_deg": degrees(
diagnostic.configured_joint_orientation_error_rad
),
}
if not arm_passed:
self._record_failure(
"initial_tcp",
0 if arm == "left" else 1,
f"{arm} initial TCP solve exceeded tolerance",
diagnostic.solved_q_rad,
diagnostic.solved_position_error_m,
diagnostic.solved_orientation_error_rad,
)
self.checks["initial_tcp_pose"] = {
"passed": passed,
"required": True,
"arms": metrics,
"note": "configured initial_joint_pose mismatch is diagnostic only",
}
def _dual_grip_control_check(self, robot: MujocoRobot) -> None:
controller = DualArmQpTeleopController(robot, self.profiles)
initial = robot.read_joint_positions().positions_rad
identity = np.array([0.0, 0.0, 0.0, 1.0])
def sample(arm: str, grip: bool, position) -> ControllerSample:
return ControllerSample(
hand=arm,
grip=grip,
trigger=0.0,
position_m=np.asarray(position, dtype=float),
quaternion_xyzw=identity,
)
controller.update_sample(sample("left", False, [0, 0, 0]), 0.0)
controller.update_sample(sample("right", False, [0, 0, 0]), 0.0)
idle = controller.step(0.0)
controller.update_sample(sample("left", True, [0, 0, 0]), 0.01)
controller.update_sample(sample("right", True, [0, 0, 0]), 0.01)
engaged = controller.step(0.01)
after_engage = robot.read_joint_positions().positions_rad
no_engage_jump = all(
np.max(np.abs(after_engage[arm] - initial[arm])) < 1e-10
for arm in ("left", "right")
)
last_result = engaged
for index in range(1, 11):
now = 0.01 + index / 90.0
hand_delta = 0.001 * index
controller.update_sample(
sample("left", True, [0, hand_delta, 0]), now
)
controller.update_sample(
sample("right", True, [0, hand_delta, 0]), now
)
last_result = controller.step(now)
if last_result.state is SafetyState.FAULT:
break
moved = robot.read_joint_positions().positions_rad
joint_delta = {
arm: float(np.max(np.abs(moved[arm] - initial[arm])))
for arm in ("left", "right")
}
both_moved = all(value > 1e-5 for value in joint_delta.values())
release_time = 0.15
controller.update_sample(sample("left", False, [0, 0.01, 0]), release_time)
controller.update_sample(sample("right", False, [0, 0.01, 0]), release_time)
released = controller.step(release_time)
passed = (
idle.state is SafetyState.IDLE
and engaged.state is SafetyState.ACTIVE
and no_engage_jump
and last_result.state is SafetyState.ACTIVE
and both_moved
and released.state is SafetyState.IDLE
)
if not passed:
self._record_failure(
"dual_grip_control",
0,
f"idle={idle.state.value}, engaged={engaged.state.value}, "
f"last={last_result.state.value}, released={released.state.value}",
)
self.checks["dual_grip_control"] = {
"passed": passed,
"required": True,
"no_engage_jump": no_engage_jump,
"max_joint_delta_rad": joint_delta,
"final_state": released.state.value,
}
def _record_failure(
self,
category: str,
index: int,
reason: str,
q: np.ndarray | None = None,
position_error_m: float = float("nan"),
orientation_error_rad: float = float("nan"),
) -> None:
self.failures.append(
{
"category": category,
"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 write_stage3_report(
output_dir: Path | str,
summary: Dict[str, Any],
failures: List[Dict[str, Any]],
images: Dict[str, np.ndarray],
) -> Tuple[Path, Path, Path, List[Path]]:
directory = Path(output_dir)
directory.mkdir(parents=True, exist_ok=True)
json_path = directory / "stage3_summary.json"
csv_path = directory / "stage3_failures.csv"
markdown_path = directory / "stage3_report.md"
json_path.write_text(
json.dumps(summary, ensure_ascii=True, indent=2, sort_keys=True) + "\n",
encoding="utf-8",
)
fields = [
"category",
"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=fields)
writer.writeheader()
writer.writerows(failures)
image_paths = []
for label, image in images.items():
path = directory / f"stage3_{label}.png"
Image.fromarray(image).save(path)
image_paths.append(path)
lines = [
"# RM75-B Stage 3 Dual-Arm QP Validation",
"",
f"- Overall: **{'PASS' if summary['passed'] else 'FAIL'}**",
f"- Seed: `{summary['seed']}`",
f"- MuJoCo: `{summary['mujoco_version']}`",
f"- RealMan API: `{summary['realman_api_version']}`",
f"- Failures recorded: `{summary['failure_count']}`",
"",
"| Check | Result | Metrics |",
"|---|---:|---|",
]
for name, check in summary["checks"].items():
metrics = {
key: value
for key, value in check.items()
if key not in {"passed", "required"}
}
lines.append(
f"| `{name}` | {'PASS' if check['passed'] else 'FAIL'} | "
f"`{json.dumps(metrics, ensure_ascii=True, sort_keys=True)}` |"
)
markdown_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
return json_path, csv_path, markdown_path, image_paths

197
ik_qp/src/rm75_ik/teleop_config.py Normal file
View File

@ -0,0 +1,197 @@
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Dict, Literal, Mapping
import numpy as np
import pinocchio as pin
import yaml
from .kinematics import validate_se3
ArmName = Literal["left", "right"]
def _readonly_vector(values, shape: tuple[int, ...], name: str) -> np.ndarray:
array = np.asarray(values, dtype=float).copy()
if array.shape != shape or not np.all(np.isfinite(array)):
raise ValueError(f"{name} must be finite with shape {shape}")
array.setflags(write=False)
return array
@dataclass(frozen=True)
class ArmTeleopProfile:
arm: ArmName
initial_tcp: pin.SE3
configured_initial_q_rad: np.ndarray
tool_from_flange: pin.SE3
active_tool_name: str
xr_to_robot: np.ndarray
scale: float
command_timeout_sec: float
deadband_m: float
target_filter_alpha: float
target_filter_alpha_fast: float
target_filter_fast_threshold_m: float
max_linear_speed_m_s: float
enable_position_axes: tuple[bool, bool, bool]
enable_orientation_control: bool
enable_orientation_axes: tuple[bool, bool, bool]
orientation_deadband_rad: float
orientation_filter_alpha: float
max_orientation_speed_rad_s: float
workspace_min: np.ndarray
workspace_max: np.ndarray
cylinder_radius_limit: np.ndarray
low_z_threshold: float
low_z_min_radius: float
joint_max_speed_rad_s: np.ndarray
def __post_init__(self) -> None:
if self.arm not in ("left", "right"):
raise ValueError("arm must be 'left' or 'right'")
validate_se3(self.initial_tcp, "initial_tcp")
validate_se3(self.tool_from_flange, "tool_from_flange")
object.__setattr__(
self,
"configured_initial_q_rad",
_readonly_vector(self.configured_initial_q_rad, (7,), "configured_initial_q_rad"),
)
matrix = _readonly_vector(self.xr_to_robot, (3, 3), "xr_to_robot")
if not np.allclose(matrix.T @ matrix, np.eye(3), atol=1e-9):
raise ValueError("xr_to_robot must be orthonormal")
if not np.isclose(np.linalg.det(matrix), 1.0, atol=1e-9):
raise ValueError("xr_to_robot must be a proper rotation")
object.__setattr__(self, "xr_to_robot", matrix)
workspace_min = _readonly_vector(self.workspace_min, (3,), "workspace_min")
workspace_max = _readonly_vector(self.workspace_max, (3,), "workspace_max")
if np.any(workspace_min >= workspace_max):
raise ValueError("workspace_min must be below workspace_max")
object.__setattr__(self, "workspace_min", workspace_min)
object.__setattr__(self, "workspace_max", workspace_max)
cylinder = _readonly_vector(
self.cylinder_radius_limit, (2,), "cylinder_radius_limit"
)
if cylinder[0] < 0.0 or cylinder[1] <= cylinder[0]:
raise ValueError("cylinder radius limits are invalid")
object.__setattr__(self, "cylinder_radius_limit", cylinder)
speeds = _readonly_vector(
self.joint_max_speed_rad_s, (7,), "joint_max_speed_rad_s"
)
if np.any(speeds <= 0.0):
raise ValueError("joint_max_speed_rad_s must be positive")
object.__setattr__(self, "joint_max_speed_rad_s", speeds)
for name in (
"scale",
"command_timeout_sec",
"target_filter_fast_threshold_m",
"max_linear_speed_m_s",
"max_orientation_speed_rad_s",
):
if not np.isfinite(getattr(self, name)) or getattr(self, name) <= 0.0:
raise ValueError(f"{name} must be finite and positive")
for name in (
"deadband_m",
"orientation_deadband_rad",
"low_z_threshold",
"low_z_min_radius",
):
if not np.isfinite(getattr(self, name)) or getattr(self, name) < 0.0:
raise ValueError(f"{name} must be finite and non-negative")
for name in (
"target_filter_alpha",
"target_filter_alpha_fast",
"orientation_filter_alpha",
):
if not 0.0 <= getattr(self, name) <= 1.0:
raise ValueError(f"{name} must be in [0, 1]")
def _pose_from_rpy(values, name: str) -> pin.SE3:
pose = _readonly_vector(values, (6,), name)
return pin.SE3(pin.rpy.rpyToMatrix(*pose[3:]), pose[:3])
def _tool_pose(values, name: str) -> pin.SE3:
pose = _readonly_vector(values, (7,), name)
quaternion = pin.Quaternion(pose[6], pose[3], pose[4], pose[5])
if quaternion.norm() <= 1e-12:
raise ValueError(f"{name} quaternion has zero norm")
quaternion.normalize()
return pin.SE3(quaternion.matrix(), pose[:3])
def load_dual_arm_profiles(
teleop_config_path: Path | str,
peripheral_config_path: Path | str,
) -> Dict[ArmName, ArmTeleopProfile]:
teleop_path = Path(teleop_config_path)
peripheral_path = Path(peripheral_config_path)
with teleop_path.open("r", encoding="utf-8") as stream:
teleop_document = yaml.safe_load(stream)
with peripheral_path.open("r", encoding="utf-8") as stream:
peripheral_document = yaml.safe_load(stream)
if not isinstance(teleop_document, Mapping) or not isinstance(
peripheral_document, Mapping
):
raise ValueError("teleop and peripheral YAML documents must be mappings")
tools = peripheral_document.get("tools_in_ee")
arms = peripheral_document.get("arms")
if not isinstance(tools, Mapping) or not isinstance(arms, Mapping):
raise ValueError("peripheral configuration is missing tools_in_ee or arms")
tool_names = list(tools)
profiles: Dict[ArmName, ArmTeleopProfile] = {}
for arm in ("left", "right"):
section_name = f"{arm}_arm_teleop"
section = teleop_document.get(section_name)
if not isinstance(section, Mapping):
raise ValueError(f"missing {section_name} configuration")
params = section.get("ros__parameters")
if not isinstance(params, Mapping):
raise ValueError(f"{section_name} is missing ros__parameters")
arm_tool = arms.get(arm)
if not isinstance(arm_tool, Mapping):
raise ValueError(f"peripheral configuration is missing arm {arm}")
tool_index = int(arm_tool.get("scissorgripper"))
try:
tool_name = tool_names[tool_index]
tool_values = tools[tool_name]["pose"]
except (IndexError, KeyError, TypeError) as exc:
raise ValueError(f"invalid active tool selection for {arm}") from exc
joint_speed = float(params["joint_max_speed"])
profiles[arm] = ArmTeleopProfile(
arm=arm,
initial_tcp=_pose_from_rpy(params["initial_tcp_pose"], f"{arm}.initial_tcp_pose"),
configured_initial_q_rad=np.deg2rad(params["initial_joint_pose"]),
tool_from_flange=_tool_pose(tool_values, f"tools.{tool_name}.pose"),
active_tool_name=tool_name,
xr_to_robot=np.asarray(params["xr_to_robot_matrix"], dtype=float).reshape(3, 3),
scale=float(params["scale"]),
command_timeout_sec=float(params["command_timeout_sec"]),
deadband_m=float(params["deadband_m"]),
target_filter_alpha=float(params["target_filter_alpha"]),
target_filter_alpha_fast=float(params["target_filter_alpha_fast"]),
target_filter_fast_threshold_m=float(
params["target_filter_fast_threshold_m"]
),
max_linear_speed_m_s=float(params["max_linear_speed"]),
enable_position_axes=tuple(bool(value) for value in params["enable_position_axes"]),
enable_orientation_control=bool(params["enable_orientation_control"]),
enable_orientation_axes=tuple(bool(value) for value in params["enable_orientation_axes"]),
orientation_deadband_rad=float(params["orientation_deadband_rad"]),
orientation_filter_alpha=float(params["orientation_filter_alpha"]),
max_orientation_speed_rad_s=float(params["max_orientation_speed"]),
workspace_min=params["workspace_min"],
workspace_max=params["workspace_max"],
cylinder_radius_limit=params["cyl_radius_limit"],
low_z_threshold=float(params["low_z_threshold"]),
low_z_min_radius=float(params["low_z_min_radius"]),
joint_max_speed_rad_s=np.full(7, np.deg2rad(joint_speed)),
)
return profiles

429
ik_qp/src/rm75_ik/teleop_control.py Normal file
View File

@ -0,0 +1,429 @@
from __future__ import annotations
from dataclasses import dataclass
from enum import Enum
from typing import Dict, Mapping, Optional
import numpy as np
import pinocchio as pin
from .kinematics import RM75Kinematics, validate_se3
from .robot_backend import RobotBackend
from .solver import RM75IkSolver
from .teleop_config import ArmName, ArmTeleopProfile
from .types import IkOptions, teleop_joint_limits
class SafetyState(str, Enum):
IDLE = "idle"
ACTIVE = "active"
FAULT = "fault"
@dataclass(frozen=True)
class ControllerSample:
hand: ArmName
grip: bool
trigger: float
position_m: np.ndarray
quaternion_xyzw: np.ndarray
def __post_init__(self) -> None:
if self.hand not in ("left", "right"):
raise ValueError("controller hand must be 'left' or 'right'")
position = np.asarray(self.position_m, dtype=float).copy()
quaternion = np.asarray(self.quaternion_xyzw, dtype=float).copy()
if position.shape != (3,) or not np.all(np.isfinite(position)):
raise ValueError("controller position must be finite with shape (3,)")
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
raise ValueError("controller quaternion must be finite with shape (4,)")
norm = float(np.linalg.norm(quaternion))
if norm <= 1e-9:
raise ValueError("controller quaternion has zero norm")
quaternion /= norm
if not np.isfinite(self.trigger):
raise ValueError("controller trigger must be finite")
position.setflags(write=False)
quaternion.setflags(write=False)
object.__setattr__(self, "position_m", position)
object.__setattr__(self, "quaternion_xyzw", quaternion)
@classmethod
def from_message(
cls, message, expected_arm: Optional[ArmName] = None
) -> "ControllerSample":
message_hand = str(getattr(message, "hand", "")).strip().lower()
hand = expected_arm or message_hand
if expected_arm is not None and message_hand and message_hand != expected_arm:
raise ValueError(
f"controller message hand {message_hand!r} does not match {expected_arm!r}"
)
pose = message.pose
return cls(
hand=hand,
grip=bool(message.grip),
trigger=float(message.trigger),
position_m=np.array(
[pose.position.x, pose.position.y, pose.position.z], dtype=float
),
quaternion_xyzw=np.array(
[
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
],
dtype=float,
),
)
@dataclass(frozen=True)
class MappedTarget:
target_tcp: pin.SE3
clamped: bool
just_engaged: bool
@dataclass(frozen=True)
class ControlCycleResult:
state: SafetyState
commanded_arms: tuple[ArmName, ...] = ()
targets_tcp: Optional[Mapping[ArmName, pin.SE3]] = None
reason: str = ""
class RelativePoseMapper:
def __init__(self, profile: ArmTeleopProfile) -> None:
self.profile = profile
self.active = False
self._controller_start_position: Optional[np.ndarray] = None
self._controller_start_rotation: Optional[np.ndarray] = None
self._robot_start_tcp: Optional[pin.SE3] = None
self._filtered_position: Optional[np.ndarray] = None
self._filtered_rotation: Optional[np.ndarray] = None
self._last_position: Optional[np.ndarray] = None
self._last_rotation: Optional[np.ndarray] = None
def reset(self) -> None:
self.active = False
self._controller_start_position = None
self._controller_start_rotation = None
self._robot_start_tcp = None
self._filtered_position = None
self._filtered_rotation = None
self._last_position = None
self._last_rotation = None
@staticmethod
def _rotation(sample: ControllerSample) -> np.ndarray:
x, y, z, w = sample.quaternion_xyzw
return pin.Quaternion(w, x, y, z).matrix()
def map(
self,
sample: ControllerSample,
current_tcp: pin.SE3,
dt: float,
) -> MappedTarget:
if sample.hand != self.profile.arm:
raise ValueError("controller sample was routed to the wrong arm")
validate_se3(current_tcp, "current_tcp")
if not np.isfinite(dt) or dt <= 0.0:
raise ValueError("control dt must be finite and positive")
rotation = self._rotation(sample)
if not self.active:
self.active = True
self._controller_start_position = sample.position_m.copy()
self._controller_start_rotation = rotation.copy()
self._robot_start_tcp = current_tcp.copy()
self._filtered_position = current_tcp.translation.copy()
self._filtered_rotation = current_tcp.rotation.copy()
self._last_position = current_tcp.translation.copy()
self._last_rotation = current_tcp.rotation.copy()
return MappedTarget(current_tcp.copy(), False, True)
assert self._controller_start_position is not None
assert self._controller_start_rotation is not None
assert self._robot_start_tcp is not None
delta = sample.position_m - self._controller_start_position
mapped_delta = self.profile.xr_to_robot @ delta
raw_position = (
self._robot_start_tcp.translation + self.profile.scale * mapped_delta
)
raw_position = np.where(
np.asarray(self.profile.enable_position_axes, dtype=bool),
raw_position,
self._robot_start_tcp.translation,
)
position, clamped = self._clamp_workspace(raw_position)
position = self._filter_position(position)
position, limited = self._limit_position_step(position, dt)
position, final_clamped = self._clamp_workspace(position)
target_rotation = self._target_rotation(rotation)
target_rotation = self._filter_rotation(target_rotation)
target_rotation, rotation_limited = self._limit_rotation_step(
target_rotation, dt
)
self._last_position = position.copy()
self._last_rotation = target_rotation.copy()
return MappedTarget(
pin.SE3(target_rotation, position),
clamped or limited or final_clamped or rotation_limited,
False,
)
def _clamp_workspace(self, target: np.ndarray) -> tuple[np.ndarray, bool]:
result = np.clip(
np.asarray(target, dtype=float),
self.profile.workspace_min,
self.profile.workspace_max,
)
min_radius, max_radius = self.profile.cylinder_radius_limit
if result[2] < self.profile.low_z_threshold:
min_radius = max(min_radius, self.profile.low_z_min_radius)
radius = float(np.hypot(result[0], result[1]))
if radius > max_radius:
result[:2] *= max_radius / radius
elif radius < min_radius:
if radius > 1e-9:
result[:2] *= min_radius / radius
else:
result[:2] = [min_radius, 0.0]
changed = not np.allclose(result, target, atol=1e-12, rtol=0.0)
return result, changed
def _filter_position(self, target: np.ndarray) -> np.ndarray:
assert self._filtered_position is not None
assert self._last_position is not None
if np.linalg.norm(target - self._last_position) < self.profile.deadband_m:
target = self._last_position
distance = float(np.linalg.norm(target - self._filtered_position))
ratio = min(
1.0, distance / self.profile.target_filter_fast_threshold_m
)
alpha = self.profile.target_filter_alpha + ratio * (
self.profile.target_filter_alpha_fast
- self.profile.target_filter_alpha
)
self._filtered_position = (
alpha * target + (1.0 - alpha) * self._filtered_position
)
return self._filtered_position.copy()
def _limit_position_step(
self, target: np.ndarray, dt: float
) -> tuple[np.ndarray, bool]:
assert self._last_position is not None
delta = target - self._last_position
distance = float(np.linalg.norm(delta))
maximum = self.profile.max_linear_speed_m_s * dt
if distance <= maximum or distance <= 1e-12:
return target, False
return self._last_position + delta * (maximum / distance), True
def _target_rotation(self, controller_rotation: np.ndarray) -> np.ndarray:
assert self._controller_start_rotation is not None
assert self._robot_start_tcp is not None
if not self.profile.enable_orientation_control:
return self._robot_start_tcp.rotation.copy()
xr_delta = controller_rotation @ self._controller_start_rotation.T
matrix = self.profile.xr_to_robot
robot_delta = matrix @ xr_delta @ matrix.T
target = robot_delta @ self._robot_start_tcp.rotation
axes = np.asarray(self.profile.enable_orientation_axes, dtype=bool)
if not np.all(axes):
target_rpy = pin.rpy.matrixToRpy(target)
start_rpy = pin.rpy.matrixToRpy(self._robot_start_tcp.rotation)
target = pin.rpy.rpyToMatrix(*np.where(axes, target_rpy, start_rpy))
return target
def _filter_rotation(self, target: np.ndarray) -> np.ndarray:
assert self._filtered_rotation is not None
assert self._last_rotation is not None
if (
np.linalg.norm(pin.log3(self._last_rotation.T @ target))
< self.profile.orientation_deadband_rad
):
target = self._last_rotation
delta = pin.log3(self._filtered_rotation.T @ target)
self._filtered_rotation = self._filtered_rotation @ pin.exp3(
self.profile.orientation_filter_alpha * delta
)
return self._filtered_rotation.copy()
def _limit_rotation_step(
self, target: np.ndarray, dt: float
) -> tuple[np.ndarray, bool]:
assert self._last_rotation is not None
delta = pin.log3(self._last_rotation.T @ target)
angle = float(np.linalg.norm(delta))
maximum = self.profile.max_orientation_speed_rad_s * dt
if angle <= maximum or angle <= 1e-12:
return target, False
return self._last_rotation @ pin.exp3(delta * (maximum / angle)), True
class DualArmQpTeleopController:
def __init__(
self,
robot: RobotBackend,
profiles: Mapping[ArmName, ArmTeleopProfile],
control_rate_hz: float = 90.0,
ik_options: Optional[IkOptions] = None,
) -> None:
if set(profiles) != {"left", "right"}:
raise ValueError("profiles must contain left and right arms")
if not np.isfinite(control_rate_hz) or control_rate_hz <= 0.0:
raise ValueError("control_rate_hz must be finite and positive")
self.robot = robot
self.profiles = dict(profiles)
self.dt = 1.0 / control_rate_hz
self.ik_options = ik_options or IkOptions(
max_iterations=120,
time_limit_sec=0.008,
)
self.kinematics = {
arm: RM75Kinematics(limits=teleop_joint_limits())
for arm in ("left", "right")
}
self.solvers = {
arm: RM75IkSolver(self.kinematics[arm]) for arm in ("left", "right")
}
self.mappers = {
arm: RelativePoseMapper(self.profiles[arm])
for arm in ("left", "right")
}
self._latest: Dict[ArmName, tuple[ControllerSample, float]] = {}
self._fault_reason = ""
self._closed = False
self.robot.connect()
@property
def safety_state(self) -> SafetyState:
if self._fault_reason:
return SafetyState.FAULT
if any(mapper.active for mapper in self.mappers.values()):
return SafetyState.ACTIVE
return SafetyState.IDLE
def update_controller(
self,
message,
timestamp_sec: float,
expected_arm: Optional[ArmName] = None,
) -> None:
self.update_sample(
ControllerSample.from_message(message, expected_arm), timestamp_sec
)
def update_sample(self, sample: ControllerSample, timestamp_sec: float) -> None:
if not np.isfinite(timestamp_sec):
raise ValueError("controller timestamp must be finite")
self._latest[sample.hand] = (sample, float(timestamp_sec))
def reject_input(self, reason: str) -> None:
self._trip_fault(f"invalid controller input: {reason}")
def step(self, timestamp_sec: float) -> ControlCycleResult:
if self._closed:
raise RuntimeError("controller is closed")
if not np.isfinite(timestamp_sec):
return self._trip_fault("control timestamp is non-finite")
if self._fault_reason:
if self._can_clear_fault(timestamp_sec):
self._fault_reason = ""
return ControlCycleResult(SafetyState.IDLE, reason="fault cleared")
return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
try:
state = self.robot.read_joint_positions()
commands: Dict[ArmName, np.ndarray] = {}
targets: Dict[ArmName, pin.SE3] = {}
for arm in ("left", "right"):
latest = self._latest.get(arm)
mapper = self.mappers[arm]
if latest is None:
continue
sample, sample_time = latest
age = timestamp_sec - sample_time
if age < -1e-6:
return self._trip_fault(f"{arm} controller timestamp is in the future")
if not sample.grip:
if mapper.active:
mapper.reset()
self.robot.stop((arm,))
continue
if age > self.profiles[arm].command_timeout_sec:
return self._trip_fault(f"{arm} controller input timed out")
q_current = state.positions_rad[arm]
current_tcp = self.kinematics[arm].forward(
q_current, self.profiles[arm].tool_from_flange
)
mapped = mapper.map(sample, current_tcp, self.dt)
flange_target = (
mapped.target_tcp * self.profiles[arm].tool_from_flange.inverse()
)
result = self.solvers[arm].solve(
flange_target, q_current, self.ik_options
)
if not result.success or result.q is None:
return self._trip_fault(
f"{arm} IK failed: {result.status.value}: {result.message}"
)
max_step = self.profiles[arm].joint_max_speed_rad_s * self.dt
q_command = np.clip(result.q, q_current - max_step, q_current + max_step)
limits = self.kinematics[arm].limits
q_command = np.clip(q_command, limits.lower, limits.upper)
commands[arm] = q_command
targets[arm] = mapped.target_tcp
if commands:
self.robot.command_joint_positions(commands)
for arm, target in targets.items():
self.robot.set_target_tcp_pose(arm, target)
return ControlCycleResult(
self.safety_state,
tuple(commands),
targets or None,
)
except Exception as exc:
return self._trip_fault(f"control/backend failure: {exc}")
def _can_clear_fault(self, timestamp_sec: float) -> bool:
if set(self._latest) != {"left", "right"}:
return False
for arm, (sample, sample_time) in self._latest.items():
if sample.grip:
return False
if timestamp_sec - sample_time > self.profiles[arm].command_timeout_sec:
return False
for mapper in self.mappers.values():
mapper.reset()
return True
def _trip_fault(self, reason: str) -> ControlCycleResult:
self._fault_reason = str(reason)
for mapper in self.mappers.values():
mapper.reset()
try:
self.robot.stop(("left", "right"))
except Exception:
pass
return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
def stop(self) -> None:
for mapper in self.mappers.values():
mapper.reset()
self.robot.stop(("left", "right"))
def close(self) -> None:
if self._closed:
return
try:
self.stop()
finally:
self.robot.close()
self._closed = True

2
ik_qp/src/rm75_ik/types.py
View File

@ -52,7 +52,7 @@ def physical_joint_limits() -> JointLimits:
def teleop_joint_limits() -> JointLimits:
lower = np.deg2rad([-150.0, -30.0, -170.0, -130.0, -175.0, -125.0, -179.0])
lower = np.deg2rad([-150.0, -110.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)