diff --git a/.gitignore b/.gitignore index d28b1cd..08438f4 100644 --- a/.gitignore +++ b/.gitignore @@ -71,6 +71,7 @@ Thumbs.db # Device-specific robot descriptions derived from local calibration runs # Includes full/partial zero-calibration outputs and local copies. /src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf +/src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py index adffc48..377e41b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py @@ -4,9 +4,11 @@ from ..registry import ProfileRegistry def register_profiles(registry: ProfileRegistry) -> None: + from .left_transfer import build_profile as build_left_transfer_profile from .profile import build_profile registry.register(build_profile()) + registry.register(build_left_transfer_profile()) __all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py index 878c988..5313922 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py @@ -2,6 +2,7 @@ from __future__ import annotations +from copy import deepcopy import json import hashlib import math @@ -557,6 +558,41 @@ def load_l6_urdf_input( def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: + from ...core import ProfileKey + + key = ProfileKey.parse(str(payload.get("profile_id", ""))) + if key == KEY: + profile = build_typed_profile() + active_joints = ACTIVE_JOINTS + passive_joints = PASSIVE_JOINTS + active_transfers = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT + passive_transfers = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT + else: + from .left_transfer import ( + KEY as LEFT_TRANSFER_KEY, + build_typed_profile as build_left_transfer_profile, + left_joint_name, + ) + + if key != LEFT_TRANSFER_KEY: + raise ValueError(f"unsupported L6 runtime profile: {key.profile_id}") + profile = build_left_transfer_profile() + active_joints = tuple(left_joint_name(name) for name in ACTIVE_JOINTS) + passive_joints = tuple(left_joint_name(name) for name in PASSIVE_JOINTS) + active_transfers = { + left_joint_name(name): left_joint_name(source) + for name, source in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items() + } + passive_transfers = { + left_joint_name(name): left_joint_name(source) + for name, source in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items() + } + passive_set = frozenset(passive_joints) + all_joints = frozenset(active_joints + passive_joints) + mimic_sources = profile.zero.mimic_source_by_joint + coupling_models = profile.zero.coupling_model_by_joint + command_indices = profile.command.command_index_by_joint + required_top = { "schema_version", "profile_id", "layout_id", "model", "side", "serial_number", "calibration_scope", "publication_pointer", @@ -568,13 +604,14 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError("schema v6 calibration has unexpected top-level fields") if ( payload["schema_version"] != 6 - or payload["profile_id"] != KEY.profile_id + or payload["profile_id"] != profile.key.profile_id or payload["model"] != "L6" - or payload["side"] != "right" + or payload["side"] != profile.key.side + or payload["layout_id"] != profile.key.layout or payload["calibration_scope"] != "partial" ): raise ValueError("schema v6 identity is invalid") - if payload["publication_pointer"] != "latest_partial_passed": + if payload["publication_pointer"] != profile.artifacts.publication_pointer: raise ValueError("L6 partial result has the wrong publication pointer") if payload["curve_input_domain"] != "feedback_u8": raise ValueError("schema v6 must be indexed by feedback_u8") @@ -582,9 +619,9 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError("schema v6 must retain both motion directions") if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: raise ValueError("schema v6 units are invalid") - if tuple(payload["command_names"]) != COMMAND_NAMES: + if tuple(payload["command_names"]) != profile.command.names: raise ValueError("schema v6 command channel order is invalid") - if payload["baseline_command_u8"] != [255] * 6: + if tuple(payload["baseline_command_u8"]) != profile.command.baseline_u8: raise ValueError("schema v6 baseline must be six open commands") protected = payload["protected_inputs"] expected_hashes = { @@ -600,21 +637,20 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: ): raise ValueError("schema v6 protected input hash is invalid") joints = payload["joints"] - if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS: + if not isinstance(joints, Mapping) or set(joints) != all_joints: raise ValueError("schema v6 must contain all 11 L6 revolute joints") - profile = build_typed_profile() - for name in ACTIVE_JOINTS + PASSIVE_JOINTS: + for name in active_joints + passive_joints: joint = joints[name] - motor = COMMAND_INDEX_BY_JOINT[ - MIMIC_SOURCE_BY_JOINT.get(name, name) + motor = command_indices[ + mimic_sources.get(name, name) ] if joint.get("urdf_joint") != name or int(joint.get("motor_index", -1)) != motor: raise ValueError(f"{name} has an invalid URDF/SDK mapping") - if joint.get("sdk_channel") != COMMAND_NAMES[motor]: + if joint.get("sdk_channel") != profile.command.names[motor]: raise ValueError(f"{name} has an invalid SDK channel") if joint.get("calibration_status") != profile.joint_coverage[name]: raise ValueError(f"{name} has an invalid coverage status") - if joint.get("passive") is not (name in PASSIVE_JOINTS): + if joint.get("passive") is not (name in passive_set): raise ValueError(f"{name} passive flag is invalid") if joint.get("zero_command_u8") != 255: raise ValueError(f"{name} zero command must be 255") @@ -624,11 +660,11 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError(f"{name}.{field} must contain 256 finite values") if np.any(np.diff(curve) > 1.0e-7): raise ValueError(f"{name}.{field} must be non-increasing") - if name in PASSIVE_JOINTS: - if joint.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name]: + if name in passive_set: + if joint.get("source_joint") != mimic_sources[name]: raise ValueError(f"{name} mimic source is invalid") model = str(joint.get("coupling_model", "")) - expected_model = COUPLING_MODEL_BY_JOINT.get( + expected_model = coupling_models.get( name, "linear_mimic" ) if model != expected_model: @@ -647,7 +683,7 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: "endpoint_linear_fallback" if model == "quadratic_runtime" else "exact_linear" - if name in MEASURED_PASSIVE_JOINTS + if name in profile.zero.fitted_mimic_joints else "cad_nominal" ) # Early schema-v6 linear artifacts predate the explicit policy @@ -671,8 +707,8 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: )) > 1.0e-7: raise ValueError(f"{name} coupling offset is inconsistent") transferred_from = ( - TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name) - or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name) + active_transfers.get(name) + or passive_transfers.get(name) ) if transferred_from is not None: if joint.get("transferred_from_joint") != transferred_from: @@ -688,6 +724,110 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError("schema v6 quality.passed must be true") +def build_l6_left_transferred_runtime_payload( + *, + right_payload: Mapping[str, Any], + source_left_urdf: str | Path, + transferred_left_urdf: str | Path, + serial_number: str, +) -> dict[str, Any]: + """Create a validated L6 left runtime JSON paired with a mirrored URDF.""" + from .left_transfer import ( + KEY as LEFT_TRANSFER_KEY, + build_typed_profile as build_left_transfer_profile, + left_joint_name, + ) + + validate_l6_runtime_payload(right_payload) + if str(right_payload["side"]) != "right": + raise ValueError("L6 left transfer requires a measured right payload") + left_source = Path(source_left_urdf).expanduser().resolve() + left_urdf = Path(transferred_left_urdf).expanduser().resolve() + if not left_source.is_file() or not left_urdf.is_file(): + raise ValueError("L6 left source and transferred URDF files are required") + profile = build_left_transfer_profile() + source_joint_names = { + str(joint.get("name")) + for joint in ET.parse(left_source).getroot().findall("joint") + if joint.get("type") == "revolute" + } + if source_joint_names != profile.zero.active_joints | profile.zero.passive_joints: + raise ValueError("L6 left source URDF differs from transferred profile") + + payload = deepcopy(dict(right_payload)) + payload.update({ + "profile_id": LEFT_TRANSFER_KEY.profile_id, + "layout_id": LEFT_TRANSFER_KEY.layout, + "side": "left", + "serial_number": str(serial_number), + "publication_pointer": profile.artifacts.publication_pointer, + }) + payload["protected_inputs"] = { + **dict(right_payload["protected_inputs"]), + "source_urdf_sha256": _sha256_file(left_source), + } + left_joints: dict[str, dict[str, Any]] = {} + for right_name, source_item in right_payload["joints"].items(): + left_name = left_joint_name(str(right_name)) + item = deepcopy(dict(source_item)) + item["urdf_joint"] = left_name + item["calibration_status"] = profile.joint_coverage[left_name] + item["mirrored_from_joint"] = str(right_name) + if "source_joint" in item: + item["source_joint"] = left_joint_name(str(item["source_joint"])) + if "transferred_from_joint" in item: + item["transferred_from_joint"] = left_joint_name( + str(item["transferred_from_joint"]) + ) + zero = item.get("zero_angles") + if isinstance(zero, Mapping): + zero = deepcopy(dict(zero)) + if "transferred_from_joint" in zero: + zero["transferred_from_joint"] = left_joint_name( + str(zero["transferred_from_joint"]) + ) + item["zero_angles"] = { + **zero, + "mirrored_from_joint": str(right_name), + "transfer_kind": "right_measurement_mirrored_to_left", + } + left_joints[left_name] = item + payload["joints"] = left_joints + + quality = deepcopy(dict(right_payload["quality"])) + thumb = quality.get("thumb_axis_zero") + if isinstance(thumb, Mapping): + thumb = deepcopy(dict(thumb)) + for field in ( + "offsets_rad", + "cycle_offsets_rad", + "geometry_fallback_reasons", + "validation_error_by_joint_rad", + ): + values = thumb.get(field) + if isinstance(values, Mapping): + thumb[field] = { + left_joint_name(str(name)): value + for name, value in values.items() + } + quality["thumb_axis_zero"] = thumb + canonical_right = json.dumps( + right_payload, ensure_ascii=False, sort_keys=True, separators=(",", ":") + ).encode("utf-8") + quality["transfer_provenance"] = { + "kind": "right_measurement_mirrored_to_left", + "source_profile_id": str(right_payload["profile_id"]), + "source_serial_number": str(right_payload["serial_number"]), + "source_payload_sha256": hashlib.sha256(canonical_right).hexdigest(), + "left_source_urdf_sha256": _sha256_file(left_source), + "transferred_left_urdf_sha256": _sha256_file(left_urdf), + "left_hand_measured": False, + } + payload["quality"] = quality + validate_l6_runtime_payload(payload) + return payload + + def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> Path: destination = Path(path).resolve() destination.parent.mkdir(parents=True, exist_ok=True) @@ -730,6 +870,7 @@ __all__ = [ "ALL_REVOLUTE_JOINTS", "artifact_hashes", "atomic_write_json", + "build_l6_left_transferred_runtime_payload", "build_l6_urdf_input_payload", "build_l6_runtime_payload", "load_l6_urdf_input", diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/left_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/left_transfer.py new file mode 100644 index 0000000..7e9474f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/left_transfer.py @@ -0,0 +1,155 @@ +"""Runtime-only L6 left profile derived from reviewed right-hand calibration.""" + +from __future__ import annotations + +from dataclasses import replace + +from ...core import ProfileKey +from ..registry import EngineBindings, RegisteredProfile +from .profile import build_typed_profile as build_right_typed_profile + + +KEY = ProfileKey("L6", "left", "l6_left_transferred_8", 1) + + +def left_joint_name(name: str) -> str: + value = str(name) + if not value.startswith("rh_"): + raise ValueError(f"L6 right joint name cannot be mirrored: {value}") + return "lh_" + value[3:] + + +def build_typed_profile(): + right = build_right_typed_profile() + command_index = { + left_joint_name(name): index + for name, index in right.command.command_index_by_joint.items() + } + active = frozenset(left_joint_name(name) for name in right.zero.active_joints) + passive = frozenset(left_joint_name(name) for name in right.zero.passive_joints) + measurements = { + left_joint_name(name): replace(spec, joint=left_joint_name(name)) + for name, spec in right.measurement.measurements.items() + } + tasks = tuple( + replace(task, joints=tuple(left_joint_name(name) for name in task.joints)) + for task in right.motion.tasks + ) + return replace( + right, + key=KEY, + namespace="/l6_left_transferred", + command=replace( + right.command, + command_index_by_joint=command_index, + urdf_joint_by_joint={name: name for name in active}, + ), + motion=replace(right.motion, tasks=tasks), + measurement=replace( + right.measurement, + measurements=measurements, + cross_view_sources={ + left_joint_name(name): left_joint_name(source) + for name, source in right.measurement.cross_view_sources.items() + }, + image_curve_joints=frozenset( + left_joint_name(name) + for name in right.measurement.image_curve_joints + ), + ), + zero=replace( + right.zero, + active_joints=active, + passive_joints=passive, + direct_zero_joints=tuple( + left_joint_name(name) for name in right.zero.direct_zero_joints + ), + axis_joints=tuple( + left_joint_name(name) for name in right.zero.axis_joints + ), + mechanical_endpoint_joints=frozenset( + left_joint_name(name) + for name in right.zero.mechanical_endpoint_joints + ), + post_solve_endpoint_joints=frozenset( + left_joint_name(name) + for name in right.zero.post_solve_endpoint_joints + ), + mimic_source_by_joint={ + left_joint_name(name): left_joint_name(source) + for name, source in right.zero.mimic_source_by_joint.items() + }, + cad_frozen_joints=passive, + endpoint_anchor_by_joint={ + left_joint_name(name): policy + for name, policy in right.zero.endpoint_anchor_by_joint.items() + }, + fitted_mimic_joints=frozenset( + left_joint_name(name) for name in right.zero.fitted_mimic_joints + ), + coupling_model_by_joint={ + left_joint_name(name): model + for name, model in right.zero.coupling_model_by_joint.items() + }, + ), + scope=replace( + right.scope, + calibrate_joints={ + scope: frozenset(left_joint_name(name) for name in names) + for scope, names in right.scope.calibrate_joints.items() + }, + frozen_joints={ + scope: frozenset(left_joint_name(name) for name in names) + for scope, names in right.scope.frozen_joints.items() + }, + ), + artifacts=replace( + right.artifacts, + calibration_filename=( + "l6_left_{serial_number}_transferred_calibration.json" + ), + corrected_urdf_filename=( + "linkerhand_l6_left_{serial_number}_transferred.urdf" + ), + publication_pointer="latest_transferred", + session_compatibility_tokens=frozenset( + {"l6_left_transfer_v1", "feedback_curves_v6"} + ), + ), + joint_coverage={ + left_joint_name(name): status + for name, status in right.joint_coverage.items() + }, + ) + + +def _unsupported(_args=None) -> None: + raise ValueError( + "L6 left transferred profile is runtime-only; calibrate the left hand " + "with a dedicated measured profile before treating it as measured" + ) + + +def build_profile() -> RegisteredProfile: + from .motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, + ) + + typed = build_typed_profile() + return RegisteredProfile( + profile=typed, + engine=EngineBindings( + hand_profile=typed, + zero_profile=typed.zero, + motion_command=build_calibration_motion_command, + preparation_waypoints=build_calibration_preparation_waypoints, + return_waypoints=build_calibration_return_waypoints, + cli_main=_unsupported, + node_main=_unsupported, + ), + ) + + +__all__ = ["KEY", "build_profile", "build_typed_profile", "left_joint_name"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py index 8bbf226..e7fdff7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py @@ -71,6 +71,138 @@ def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str: ) +def _revolute_joints(root: ET.Element) -> dict[str, ET.Element]: + return { + str(joint.get("name")): joint + for joint in root.findall("joint") + if joint.get("type") == "revolute" + } + + +def _origin_rotation(joint: ET.Element) -> Rotation: + origin = joint.find("origin") + if origin is None or origin.get("rpy") is None: + raise ValueError(f"joint {joint.get('name')} has no origin.rpy") + return Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0"))) + + +def write_l6_left_from_right_calibration( + *, + source_left_urdf: str | Path, + source_right_urdf: str | Path, + calibrated_right_urdf: str | Path, + destination_urdf: str | Path, +) -> Path: + """Mirror a reviewed L6 right correction onto the original left CAD.""" + source_left = Path(source_left_urdf).expanduser().resolve() + source_right = Path(source_right_urdf).expanduser().resolve() + calibrated_right = Path(calibrated_right_urdf).expanduser().resolve() + destination = Path(destination_urdf).expanduser().resolve() + if not source_left.is_file() or not source_right.is_file(): + raise ValueError("original L6 left and right URDF files are required") + if not calibrated_right.is_file(): + raise ValueError("calibrated L6 right URDF is required") + if ( + "calibrated" in source_left.stem.lower() + or "calibrated" in source_right.stem.lower() + ): + raise ValueError("L6 transfer sources must be immutable original URDF files") + + left_root = ET.parse(source_left).getroot() + right_source_root = ET.parse(source_right).getroot() + right_corrected_root = ET.parse(calibrated_right).getroot() + left = _revolute_joints(left_root) + right_source = _revolute_joints(right_source_root) + right_corrected = _revolute_joints(right_corrected_root) + expected_right = {name.replace("lh_", "rh_", 1) for name in left} + if set(right_source) != expected_right or set(right_corrected) != expected_right: + raise ValueError("L6 left/right revolute topology is not mirror-compatible") + + joint_patches: dict[str, UrdfJointPatch] = {} + for left_name, left_joint in sorted(left.items()): + right_name = left_name.replace("lh_", "rh_", 1) + source_joint = right_source[right_name] + corrected_joint = right_corrected[right_name] + source_axis_node = source_joint.find("axis") + source_axis = _triplet( + "1 0 0" + if source_axis_node is None + else source_axis_node.get("xyz", "1 0 0") + ) + source_axis /= np.linalg.norm(source_axis) + relative = _origin_rotation(source_joint).inv() * _origin_rotation( + corrected_joint + ) + rotation_vector = relative.as_rotvec() + offset = float(rotation_vector @ source_axis) + if np.linalg.norm(rotation_vector - offset * source_axis) > 1.0e-7: + raise ValueError( + f"right L6 correction is not about its declared axis: {right_name}" + ) + if abs(offset) > math.radians(15.0): + raise ValueError(f"right L6 correction exceeds transfer bound: {right_name}") + corrected_limit = corrected_joint.find("limit") + if corrected_limit is None: + raise ValueError(f"right L6 joint has no limit: {right_name}") + values: dict[str, str] = { + "limit_lower": str(corrected_limit.get("lower")), + "limit_upper": str(corrected_limit.get("upper")), + } + if abs(offset) > 1.0e-12: + values["origin_rpy"] = _corrected_origin_rpy(left_joint, offset) + left_mimic = left_joint.find("mimic") + right_mimic = corrected_joint.find("mimic") + if (left_mimic is None) != (right_mimic is None): + raise ValueError(f"L6 passive topology differs: {left_name}") + if left_mimic is not None and right_mimic is not None: + expected_source = str(right_mimic.get("joint")).replace("rh_", "lh_", 1) + if left_mimic.get("joint") != expected_source: + raise ValueError(f"L6 mimic source is not mirrored: {left_name}") + values["mimic_multiplier"] = str(right_mimic.get("multiplier")) + values["mimic_offset"] = str(right_mimic.get("offset", "0")) + joint_patches[left_name] = UrdfJointPatch(**values) + + right_equalities = { + str(node.get("joint1")): node + for node in right_corrected_root.findall("./mujoco/equality/joint") + } + equality_patches: dict[str, MujocoEqualityPatch] = {} + for left_equality in left_root.findall("./mujoco/equality/joint"): + left_target = str(left_equality.get("joint1", "")) + left_source_name = str(left_equality.get("joint2", "")) + right_target = left_target.replace("lh_", "rh_", 1) + right_equality = right_equalities.get(right_target) + if right_equality is None: + raise ValueError(f"right L6 equality is missing: {right_target}") + expected_left_source = str(right_equality.get("joint2", "")).replace( + "rh_", "lh_", 1 + ) + if left_source_name != expected_left_source: + raise ValueError(f"L6 equality source is not mirrored: {left_target}") + equality_name = str(left_equality.get("name", "")) + if not equality_name: + raise ValueError(f"left L6 equality has no name: {left_target}") + equality_patches[equality_name] = MujocoEqualityPatch( + polycoef=str(right_equality.get("polycoef", "")), + expected_joint1=left_target, + expected_joint2=left_source_name, + ) + if len(equality_patches) != len(right_equalities): + raise ValueError("L6 left/right equality topology differs") + + write_urdf_patches( + source_urdf=source_left, + destination_urdf=destination, + patches=UrdfPatchSet( + joints=joint_patches, + mujoco_equalities=equality_patches, + ), + forbidden_source_stem_patterns=(r"calibrated",), + copy_complete_mesh_directory=True, + ) + return destination + + def _validate_passive_ranges( joints: Mapping[str, ET.Element], result: L6FitResult ) -> None: @@ -290,4 +422,8 @@ def write_l6_corrected_urdf( ) -__all__ = ["L6UrdfCorrection", "write_l6_corrected_urdf"] +__all__ = [ + "L6UrdfCorrection", + "write_l6_corrected_urdf", + "write_l6_left_from_right_calibration", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py index 622724b..bffbc3e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py @@ -4,9 +4,11 @@ from ..registry import ProfileRegistry def register_profiles(registry: ProfileRegistry) -> None: + from .left_transfer import build_profile as build_left_transfer_profile from .profile import build_profile registry.register(build_profile()) + registry.register(build_left_transfer_profile()) __all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py index d6c3bb3..7d84051 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py @@ -2,6 +2,7 @@ from __future__ import annotations +from copy import deepcopy import hashlib import json import math @@ -240,6 +241,41 @@ def build_o6_runtime_payload( def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: + from ...core import ProfileKey + + key = ProfileKey.parse(str(payload.get("profile_id", ""))) + if key == KEY: + profile = build_typed_profile() + active_joints = ACTIVE_JOINTS + passive_joints = PASSIVE_JOINTS + active_transfers = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT + passive_transfers = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT + else: + from .left_transfer import ( + KEY as LEFT_TRANSFER_KEY, + build_typed_profile as build_left_transfer_profile, + left_joint_name, + ) + + if key != LEFT_TRANSFER_KEY: + raise ValueError(f"unsupported O6 runtime profile: {key.profile_id}") + profile = build_left_transfer_profile() + active_joints = tuple(left_joint_name(name) for name in ACTIVE_JOINTS) + passive_joints = tuple(left_joint_name(name) for name in PASSIVE_JOINTS) + active_transfers = { + left_joint_name(name): left_joint_name(source) + for name, source in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items() + } + passive_transfers = { + left_joint_name(name): left_joint_name(source) + for name, source in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items() + } + passive_set = frozenset(passive_joints) + all_joints = frozenset(active_joints + passive_joints) + mimic_sources = profile.zero.mimic_source_by_joint + coupling_models = profile.zero.coupling_model_by_joint + command_indices = profile.command.command_index_by_joint + required = { "schema_version", "profile_id", "layout_id", "model", "side", "serial_number", "calibration_scope", "publication_pointer", "angle_unit", @@ -249,16 +285,17 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: if set(payload) != required: raise ValueError("O6 schema v6 has unexpected top-level fields") if ( - payload["schema_version"] != 6 or payload["profile_id"] != KEY.profile_id - or payload["model"] != "O6" or payload["side"] != "right" - or payload["layout_id"] != KEY.layout + payload["schema_version"] != 6 + or payload["profile_id"] != profile.key.profile_id + or payload["model"] != "O6" or payload["side"] != profile.key.side + or payload["layout_id"] != profile.key.layout or payload["calibration_scope"] != "partial" - or payload["publication_pointer"] != "latest_partial_passed" + or payload["publication_pointer"] != profile.artifacts.publication_pointer or payload["curve_input_domain"] != "feedback_u8" or payload["runtime_curve_policy"] != "direction_aware" or payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255] - or tuple(payload["command_names"]) != COMMAND_NAMES - or payload["baseline_command_u8"] != [255] * 6 + or tuple(payload["command_names"]) != profile.command.names + or tuple(payload["baseline_command_u8"]) != profile.command.baseline_u8 ): raise ValueError("O6 schema v6 identity or command contract is invalid") protected = payload["protected_inputs"] @@ -268,17 +305,16 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: } or any(len(str(value)) != 64 for value in protected.values()): raise ValueError("O6 protected inputs are invalid") joints = payload["joints"] - if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS: + if not isinstance(joints, Mapping) or set(joints) != all_joints: raise ValueError("O6 schema v6 must contain all 11 revolute joints") - profile = build_typed_profile() - for name in ACTIVE_JOINTS + PASSIVE_JOINTS: + for name in active_joints + passive_joints: item = joints[name] - motor = COMMAND_INDEX_BY_JOINT[MIMIC_SOURCE_BY_JOINT.get(name, name)] + motor = command_indices[mimic_sources.get(name, name)] if ( item.get("urdf_joint") != name or item.get("motor_index") != motor - or item.get("sdk_channel") != COMMAND_NAMES[motor] + or item.get("sdk_channel") != profile.command.names[motor] or item.get("calibration_status") != profile.joint_coverage[name] - or item.get("passive") is not (name in PASSIVE_JOINTS) + or item.get("passive") is not (name in passive_set) or item.get("zero_command_u8") != 255 ): raise ValueError(f"O6 joint contract is invalid: {name}") @@ -288,10 +324,10 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError(f"{name}.{field} must contain 256 finite values") if np.any(np.diff(values) > 1e-7): raise ValueError(f"{name}.{field} must be non-increasing") - if name in PASSIVE_JOINTS: + if name in passive_set: if ( - item.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name] - or item.get("coupling_model") != COUPLING_MODEL_BY_JOINT[name] + item.get("source_joint") != mimic_sources[name] + or item.get("coupling_model") != coupling_models[name] or item.get("urdf_mimic_enabled") is not True or item.get("urdf_mimic_policy") != "endpoint_linear_fallback" ): @@ -310,7 +346,7 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: multiplier = float(item.get("mimic_multiplier", "nan")) if not math.isfinite(multiplier) or multiplier <= 0.0: raise ValueError(f"O6 passive mimic multiplier is invalid: {name}") - donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name) or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name) + donor = active_transfers.get(name) or passive_transfers.get(name) if donor is not None: if item.get("transferred_from_joint") != donor: raise ValueError(f"O6 transfer provenance is invalid: {name}") @@ -321,6 +357,112 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError("O6 quality.passed must be true") +def build_o6_left_transferred_runtime_payload( + *, + right_payload: Mapping[str, Any], + source_left_urdf: str | Path, + transferred_left_urdf: str | Path, + serial_number: str, +) -> dict[str, Any]: + """Create a validated left runtime JSON paired with a mirrored URDF.""" + from .left_transfer import ( + KEY as LEFT_TRANSFER_KEY, + build_typed_profile as build_left_transfer_profile, + left_joint_name, + ) + + validate_o6_runtime_payload(right_payload) + if str(right_payload["side"]) != "right": + raise ValueError("O6 left transfer requires a measured right payload") + left_source = Path(source_left_urdf).expanduser().resolve() + left_urdf = Path(transferred_left_urdf).expanduser().resolve() + if not left_source.is_file() or not left_urdf.is_file(): + raise ValueError("O6 left source and transferred URDF files are required") + profile = build_left_transfer_profile() + source_joint_names = { + str(joint.get("name")) + for joint in ET.parse(left_source).getroot().findall("joint") + if joint.get("type") == "revolute" + } + if source_joint_names != profile.zero.active_joints | profile.zero.passive_joints: + raise ValueError("O6 left source URDF differs from transferred profile") + + payload = deepcopy(dict(right_payload)) + payload.update({ + "profile_id": LEFT_TRANSFER_KEY.profile_id, + "layout_id": LEFT_TRANSFER_KEY.layout, + "side": "left", + "serial_number": str(serial_number), + "publication_pointer": profile.artifacts.publication_pointer, + }) + payload["protected_inputs"] = { + **dict(right_payload["protected_inputs"]), + "source_urdf_sha256": _sha256(left_source), + } + left_joints: dict[str, dict[str, Any]] = {} + for right_name, source_item in right_payload["joints"].items(): + left_name = left_joint_name(str(right_name)) + item = deepcopy(dict(source_item)) + item["urdf_joint"] = left_name + item["calibration_status"] = profile.joint_coverage[left_name] + item["mirrored_from_joint"] = str(right_name) + if "source_joint" in item: + item["source_joint"] = left_joint_name(str(item["source_joint"])) + if "transferred_from_joint" in item: + item["transferred_from_joint"] = left_joint_name( + str(item["transferred_from_joint"]) + ) + zero = item.get("zero_angles") + if isinstance(zero, Mapping): + item["zero_angles"] = { + **dict(zero), + "mirrored_from_joint": str(right_name), + "transfer_kind": "right_measurement_mirrored_to_left", + } + left_joints[left_name] = item + payload["joints"] = left_joints + + quality = deepcopy(dict(right_payload["quality"])) + hysteresis = quality.get("maximum_hysteresis_by_joint_rad") + if isinstance(hysteresis, Mapping): + quality["maximum_hysteresis_by_joint_rad"] = { + left_joint_name(str(name)): value + for name, value in hysteresis.items() + } + thumb = quality.get("thumb_axis_zero") + if isinstance(thumb, Mapping): + thumb = deepcopy(dict(thumb)) + for field in ( + "offsets_rad", + "candidate_geometry_offsets_rad", + "policy_by_joint", + "validation_error_by_joint_rad", + "validation_line_error_by_joint_m", + ): + values = thumb.get(field) + if isinstance(values, Mapping): + thumb[field] = { + left_joint_name(str(name)): value + for name, value in values.items() + } + quality["thumb_axis_zero"] = thumb + canonical_right = json.dumps( + right_payload, ensure_ascii=False, sort_keys=True, separators=(",", ":") + ).encode("utf-8") + quality["transfer_provenance"] = { + "kind": "right_measurement_mirrored_to_left", + "source_profile_id": str(right_payload["profile_id"]), + "source_serial_number": str(right_payload["serial_number"]), + "source_payload_sha256": hashlib.sha256(canonical_right).hexdigest(), + "left_source_urdf_sha256": _sha256(left_source), + "transferred_left_urdf_sha256": _sha256(left_urdf), + "left_hand_measured": False, + } + payload["quality"] = quality + validate_o6_runtime_payload(payload) + return payload + + def build_o6_urdf_input_payload( *, serial_number: str, source_urdf: str | Path, result: O6FitResult ) -> dict[str, Any]: @@ -444,7 +586,8 @@ def load_o6_urdf_input( __all__ = [ "ALL_REVOLUTE_JOINTS", "artifact_hashes", "atomic_write_json", - "build_o6_runtime_payload", "build_o6_urdf_input_payload", "load_o6_urdf_input", + "build_o6_left_transferred_runtime_payload", "build_o6_runtime_payload", + "build_o6_urdf_input_payload", "load_o6_urdf_input", "publish_partial_session", "validate_o6_runtime_payload", "validate_o6_urdf_input_payload", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/left_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/left_transfer.py new file mode 100644 index 0000000..0f240fd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/left_transfer.py @@ -0,0 +1,159 @@ +"""Runtime-only O6 left profile derived from reviewed right-hand calibration.""" + +from __future__ import annotations + +from dataclasses import replace + +from ...core import ProfileKey +from ..registry import EngineBindings, RegisteredProfile +from .profile import build_typed_profile as build_right_typed_profile + + +KEY = ProfileKey("O6", "left", "o6_left_transferred_8", 1) + + +def left_joint_name(name: str) -> str: + value = str(name) + if not value.startswith("rh_"): + raise ValueError(f"O6 right joint name cannot be mirrored: {value}") + return "lh_" + value[3:] + + +def build_typed_profile(): + right = build_right_typed_profile() + command_index = { + left_joint_name(name): index + for name, index in right.command.command_index_by_joint.items() + } + active = frozenset(left_joint_name(name) for name in right.zero.active_joints) + passive = frozenset(left_joint_name(name) for name in right.zero.passive_joints) + measurements = { + left_joint_name(name): replace(spec, joint=left_joint_name(name)) + for name, spec in right.measurement.measurements.items() + } + tasks = tuple( + replace(task, joints=tuple(left_joint_name(name) for name in task.joints)) + for task in right.motion.tasks + ) + return replace( + right, + key=KEY, + namespace="/o6_left_transferred", + command=replace( + right.command, + command_index_by_joint=command_index, + urdf_joint_by_joint={name: name for name in active}, + ), + motion=replace(right.motion, tasks=tasks), + measurement=replace( + right.measurement, + measurements=measurements, + cross_view_sources={ + left_joint_name(name): left_joint_name(source) + for name, source in right.measurement.cross_view_sources.items() + }, + image_curve_joints=frozenset( + left_joint_name(name) + for name in right.measurement.image_curve_joints + ), + ), + zero=replace( + right.zero, + active_joints=active, + passive_joints=passive, + direct_zero_joints=tuple( + left_joint_name(name) for name in right.zero.direct_zero_joints + ), + axis_joints=tuple( + left_joint_name(name) for name in right.zero.axis_joints + ), + mechanical_endpoint_joints=frozenset( + left_joint_name(name) + for name in right.zero.mechanical_endpoint_joints + ), + post_solve_endpoint_joints=frozenset( + left_joint_name(name) + for name in right.zero.post_solve_endpoint_joints + ), + mimic_source_by_joint={ + left_joint_name(name): left_joint_name(source) + for name, source in right.zero.mimic_source_by_joint.items() + }, + cad_frozen_joints=passive, + endpoint_anchor_by_joint={ + left_joint_name(name): policy + for name, policy in right.zero.endpoint_anchor_by_joint.items() + }, + fitted_mimic_joints=frozenset( + left_joint_name(name) for name in right.zero.fitted_mimic_joints + ), + coupling_model_by_joint={ + left_joint_name(name): model + for name, model in right.zero.coupling_model_by_joint.items() + }, + ), + scope=replace( + right.scope, + calibrate_joints={ + scope: frozenset(left_joint_name(name) for name in names) + for scope, names in right.scope.calibrate_joints.items() + }, + frozen_joints={ + scope: frozenset(left_joint_name(name) for name in names) + for scope, names in right.scope.frozen_joints.items() + }, + ), + artifacts=replace( + right.artifacts, + calibration_filename=( + "o6_left_{serial_number}_transferred_calibration.json" + ), + corrected_urdf_filename=( + "linkerhand_o6_left_{serial_number}_transferred.urdf" + ), + publication_pointer="latest_transferred", + session_compatibility_tokens=frozenset( + {"o6_left_transfer_v1", "feedback_curves_v6"} + ), + ), + joint_coverage={ + name: ( + "transferred_static_dynamic" + if name in active + else "transferred_dynamic_cad_static" + ) + for name in active | passive + }, + ) + + +def _unsupported(_args=None) -> None: + raise ValueError( + "O6 left transferred profile is runtime-only; calibrate the left hand " + "with a dedicated measured profile before treating it as measured" + ) + + +def build_profile() -> RegisteredProfile: + from ..l6.motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, + ) + + typed = build_typed_profile() + return RegisteredProfile( + profile=typed, + engine=EngineBindings( + hand_profile=typed, + zero_profile=typed.zero, + motion_command=build_calibration_motion_command, + preparation_waypoints=build_calibration_preparation_waypoints, + return_waypoints=build_calibration_return_waypoints, + cli_main=_unsupported, + node_main=_unsupported, + ), + ) + + +__all__ = ["KEY", "build_profile", "build_typed_profile", "left_joint_name"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py index f8c9baa..029c9fa 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py @@ -9,9 +9,10 @@ from typing import Mapping import xml.etree.ElementTree as ET import numpy as np +from scipy.spatial.transform import Rotation from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches -from ..l6.urdf import L6UrdfCorrection, _corrected_origin_rpy +from ..l6.urdf import L6UrdfCorrection, _corrected_origin_rpy, _triplet from .fitting import O6FitResult from .profile import ( CALIBRATED_ACTIVE_JOINTS, @@ -27,6 +28,111 @@ from .profile import ( O6UrdfCorrection = L6UrdfCorrection +def _joint_map(root: ET.Element) -> dict[str, ET.Element]: + return { + str(joint.get("name")): joint + for joint in root.findall("joint") + if joint.get("type") == "revolute" + } + + +def _origin_rotation(joint: ET.Element) -> Rotation: + origin = joint.find("origin") + if origin is None or origin.get("rpy") is None: + raise ValueError(f"joint {joint.get('name')} has no origin.rpy") + return Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0"))) + + +def write_o6_left_from_right_calibration( + *, + source_left_urdf: str | Path, + source_right_urdf: str | Path, + calibrated_right_urdf: str | Path, + destination_urdf: str | Path, +) -> Path: + """Mirror reviewed right-hand scalar corrections onto left-hand CAD. + + This is an explicitly transferred preview, not a left-hand measurement. + Left mesh, inertia, topology and joint origins remain authoritative. The + measured right-hand ranges and passive ratios are scalar mechanism data; + the yaw origin correction is reapplied about the left joint's mirrored + local axis instead of copying the right-hand Euler angle. + """ + source_left = Path(source_left_urdf).expanduser().resolve() + source_right = Path(source_right_urdf).expanduser().resolve() + calibrated_right = Path(calibrated_right_urdf).expanduser().resolve() + destination = Path(destination_urdf).expanduser().resolve() + if not source_left.is_file() or not source_right.is_file(): + raise ValueError("original O6 left and right URDF files are required") + if not calibrated_right.is_file(): + raise ValueError("calibrated O6 right URDF is required") + if "calibrated" in source_left.stem.lower() or "calibrated" in source_right.stem.lower(): + raise ValueError("O6 transfer sources must be immutable original URDF files") + + left_root = ET.parse(source_left).getroot() + right_source_root = ET.parse(source_right).getroot() + right_corrected_root = ET.parse(calibrated_right).getroot() + left = _joint_map(left_root) + right_source = _joint_map(right_source_root) + right_corrected = _joint_map(right_corrected_root) + expected_right = {name.replace("lh_", "rh_", 1) for name in left} + if set(right_source) != expected_right or set(right_corrected) != expected_right: + raise ValueError("O6 left/right revolute topology is not mirror-compatible") + + right_yaw_source = right_source["rh_thumb_cmc_yaw"] + right_yaw_corrected = right_corrected["rh_thumb_cmc_yaw"] + axis = _triplet(right_yaw_source.find("axis").get("xyz", "0 0 -1")) + axis /= np.linalg.norm(axis) + relative = _origin_rotation(right_yaw_source).inv() * _origin_rotation( + right_yaw_corrected + ) + rotation_vector = relative.as_rotvec() + yaw_offset = float(rotation_vector @ axis) + if np.linalg.norm(rotation_vector - yaw_offset * axis) > 1.0e-8: + raise ValueError("right O6 yaw correction is not about its declared axis") + if abs(yaw_offset) > math.radians(15.0): + raise ValueError("right O6 yaw correction exceeds transfer safety bound") + + source_pitch_rpy = right_source["rh_thumb_cmc_pitch"].find("origin").get("rpy") + corrected_pitch_rpy = right_corrected["rh_thumb_cmc_pitch"].find("origin").get("rpy") + if source_pitch_rpy != corrected_pitch_rpy: + raise ValueError("right O6 calibration does not use the reviewed CAD pitch zero") + + patches: dict[str, UrdfJointPatch] = {} + for left_name, left_joint in sorted(left.items()): + right_name = left_name.replace("lh_", "rh_", 1) + right_joint = right_corrected[right_name] + right_limit = right_joint.find("limit") + if right_limit is None: + raise ValueError(f"right O6 joint has no limit: {right_name}") + values: dict[str, str] = { + "limit_lower": str(right_limit.get("lower")), + "limit_upper": str(right_limit.get("upper")), + } + if left_name == "lh_thumb_cmc_yaw": + values["origin_rpy"] = _corrected_origin_rpy(left_joint, yaw_offset) + left_mimic = left_joint.find("mimic") + right_mimic = right_joint.find("mimic") + if (left_mimic is None) != (right_mimic is None): + raise ValueError(f"O6 passive topology differs: {left_name}") + if left_mimic is not None and right_mimic is not None: + expected_source = str(right_mimic.get("joint")).replace("rh_", "lh_", 1) + if left_mimic.get("joint") != expected_source: + raise ValueError(f"O6 mimic source is not mirrored: {left_name}") + values["mimic_multiplier"] = str(right_mimic.get("multiplier")) + values["mimic_offset"] = str(right_mimic.get("offset", "0")) + patches[left_name] = UrdfJointPatch(**values) + + write_urdf_patches( + source_urdf=source_left, + destination_urdf=destination, + patches=UrdfPatchSet(joints=patches), + forbidden_source_stem_patterns=(r"calibrated",), + copy_complete_mesh_directory=True, + ) + return destination + + def _validate_passive_ranges( joints: Mapping[str, ET.Element], result: O6FitResult ) -> dict[str, tuple[float, float]]: @@ -169,4 +275,8 @@ def write_o6_corrected_urdf( ) -__all__ = ["O6UrdfCorrection", "write_o6_corrected_urdf"] +__all__ = [ + "O6UrdfCorrection", + "write_o6_corrected_urdf", + "write_o6_left_from_right_calibration", +] diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py index d465df8..048183e 100644 --- a/src/linkerhand_calibration/test/test_architecture.py +++ b/src/linkerhand_calibration/test/test_architecture.py @@ -145,7 +145,7 @@ def test_runtime_has_no_concrete_model_or_view_assumption() -> None: def test_every_registered_profile_passes_static_integrity_checks() -> None: registry = get_default_registry() - assert len(registry) == 5 + assert len(registry) == 7 for registered in registry: validate_profile(registered.profile) assert registered.profile.zero.active_joints diff --git a/src/linkerhand_calibration/test/test_l6_right_profile.py b/src/linkerhand_calibration/test/test_l6_right_profile.py index e39b5cf..067c143 100644 --- a/src/linkerhand_calibration/test/test_l6_right_profile.py +++ b/src/linkerhand_calibration/test/test_l6_right_profile.py @@ -16,6 +16,7 @@ from linkerhand_calibration.calibrated_joint_state_bridge import ( from linkerhand_calibration.core import validate_profile from linkerhand_calibration.models.l6.artifacts import ( atomic_write_json, + build_l6_left_transferred_runtime_payload, build_l6_urdf_input_payload, build_l6_runtime_payload, load_l6_urdf_input, @@ -44,7 +45,10 @@ from linkerhand_calibration.models.l6.profile import ( build_typed_profile, ) from linkerhand_calibration.models.l6.runner import render_l6_progress_zh -from linkerhand_calibration.models.l6.urdf import write_l6_corrected_urdf +from linkerhand_calibration.models.l6.urdf import ( + write_l6_corrected_urdf, + write_l6_left_from_right_calibration, +) from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel from linkerhand_calibration.product import load_product_config, sha256_file @@ -52,6 +56,7 @@ from linkerhand_calibration.product import load_product_config, sha256_file PACKAGE = Path(__file__).resolve().parents[1] SOURCE = PACKAGE / "urdf/l6_right/linkerhand_l6v3.1_right.urdf" +LEFT_SOURCE = PACKAGE / "urdf/l6_left/linkerhand_l6v3.1_left.urdf" PRODUCT = PACKAGE / "config/l6_right_product.yaml" TRAVELS = { @@ -714,6 +719,116 @@ def test_l6_urdf_writer_changes_only_authorized_joint_fields(tmp_path: Path) -> ) +def test_l6_right_corrections_are_mirrored_onto_left_cad(tmp_path: Path) -> None: + result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records())) + right = write_l6_corrected_urdf( + source_urdf=SOURCE, + output_directory=tmp_path / "right", + serial_number="RIGHT_TEST", + result=result, + timestamp="20260903_120000", + ).path + left_path = write_l6_left_from_right_calibration( + source_left_urdf=LEFT_SOURCE, + source_right_urdf=SOURCE, + calibrated_right_urdf=right, + destination_urdf=tmp_path / "left" / "l6_left_transferred.urdf", + ) + original = { + str(joint.get("name")): joint + for joint in ET.parse(LEFT_SOURCE).getroot().findall("joint") + if joint.get("type") == "revolute" + } + transferred_root = ET.parse(left_path).getroot() + transferred = { + str(joint.get("name")): joint + for joint in transferred_root.findall("joint") + if joint.get("type") == "revolute" + } + corrected_right = { + str(joint.get("name")): joint + for joint in ET.parse(right).getroot().findall("joint") + if joint.get("type") == "revolute" + } + for left_name, joint in transferred.items(): + right_name = left_name.replace("lh_", "rh_", 1) + assert joint.find("origin").get("xyz") == original[left_name].find( + "origin" + ).get("xyz") + assert joint.find("axis").attrib == original[left_name].find("axis").attrib + assert joint.find("limit").get("lower") == corrected_right[ + right_name + ].find("limit").get("lower") + assert joint.find("limit").get("upper") == corrected_right[ + right_name + ].find("limit").get("upper") + if joint.find("mimic") is not None: + assert joint.find("mimic").get("multiplier") == corrected_right[ + right_name + ].find("mimic").get("multiplier") + for name, expected in THUMB_ZERO_OFFSETS.items(): + left_name = name.replace("rh_", "lh_", 1) + source_rotation = Rotation.from_euler( + "xyz", + [float(value) for value in original[left_name].find("origin").get( + "rpy" + ).split()], + ) + corrected_rotation = Rotation.from_euler( + "xyz", + [float(value) for value in transferred[left_name].find("origin").get( + "rpy" + ).split()], + ) + axis = np.asarray([ + float(value) + for value in original[left_name].find("axis").get("xyz").split() + ]) + axis /= np.linalg.norm(axis) + applied = float((source_rotation.inv() * corrected_rotation).as_rotvec() @ axis) + assert applied == pytest.approx(expected, abs=1.0e-6) + left_equalities = { + str(node.get("joint1")): node.get("polycoef") + for node in transferred_root.findall("./mujoco/equality/joint") + } + right_equalities = { + str(node.get("joint1")).replace("rh_", "lh_", 1): node.get("polycoef") + for node in ET.parse(right).getroot().findall("./mujoco/equality/joint") + } + assert left_equalities == right_equalities + + right_payload = build_l6_runtime_payload( + serial_number="RIGHT_TEST", + source_urdf=SOURCE, + result=result, + protected_inputs={ + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "calibration_config_sha256": "2" * 64, + "tag_config_sha256": "3" * 64, + }, + ) + left_payload = build_l6_left_transferred_runtime_payload( + right_payload=right_payload, + source_left_urdf=LEFT_SOURCE, + transferred_left_urdf=left_path, + serial_number="LEFT_TRANSFER_TEST", + ) + validate_l6_runtime_payload(left_payload) + mapper = CalibratedCommandMapper(left_payload, expected_side="left") + assert mapper.profile_id == "L6/left/l6_left_transferred_8/v1" + assert set(mapper.urdf_joint_names) == set(transferred) + assert left_payload["joints"]["lh_pinky_dip"]["source_joint"] == ( + "lh_pinky_mcp_pitch" + ) + assert left_payload["joints"]["lh_index_dip"][ + "transferred_from_joint" + ] == "lh_pinky_dip" + assert left_payload["quality"]["transfer_provenance"][ + "left_hand_measured" + ] is False + + def test_l6_schema_v6_bridge_uses_feedback_and_rh_joint_names() -> None: result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records())) hashes = { diff --git a/src/linkerhand_calibration/test/test_o6_right_profile.py b/src/linkerhand_calibration/test/test_o6_right_profile.py index 887a2cb..81b3fa0 100644 --- a/src/linkerhand_calibration/test/test_o6_right_profile.py +++ b/src/linkerhand_calibration/test/test_o6_right_profile.py @@ -14,6 +14,7 @@ from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel from linkerhand_calibration.models.l6.node import MotionStep from linkerhand_calibration.models.o6.artifacts import ( + build_o6_left_transferred_runtime_payload, build_o6_runtime_payload, validate_o6_runtime_payload, ) @@ -38,14 +39,26 @@ from linkerhand_calibration.models.o6.profile import ( build_typed_profile, ) from linkerhand_calibration.models.o6.runner import render_o6_progress_zh -from linkerhand_calibration.models.o6.urdf import write_o6_corrected_urdf +from linkerhand_calibration.models.o6.urdf import ( + write_o6_corrected_urdf, + write_o6_left_from_right_calibration, +) PACKAGE = Path(__file__).resolve().parents[1] SOURCE = PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf" +LEFT_SOURCE = PACKAGE / "urdf/o6_left/linkerhand_o6_left.urdf" KINEMATIC_MODEL = UrdfKinematicModel(SOURCE) +def _joint_elements(path: Path) -> dict[str, ET.Element]: + return { + str(joint.get("name")): joint + for joint in ET.parse(path).getroot().findall("joint") + if joint.get("type") == "revolute" + } + + def test_o6_profile_declares_reviewed_six_channel_contract() -> None: profile = build_typed_profile() validate_profile(profile) @@ -475,3 +488,80 @@ def test_o6_uses_geometric_yaw_and_cad_endpoint_pitch() -> None: assert result.thumb_zero_result.direct_offsets_rad[ "rh_thumb_cmc_pitch" ] == pytest.approx(-0.02, abs=1e-6) + + +def test_o6_right_corrections_are_mirrored_without_replacing_left_geometry( + tmp_path: Path, +) -> None: + result = fit_o6_session( + SOURCE, _geometric_records(), require_thumb_axis_zero=True + ) + right = write_o6_corrected_urdf( + source_urdf=SOURCE, + output_directory=tmp_path / "right", + serial_number="RIGHT_TEST", + result=result, + ).path + destination = write_o6_left_from_right_calibration( + source_left_urdf=LEFT_SOURCE, + source_right_urdf=SOURCE, + calibrated_right_urdf=right, + destination_urdf=tmp_path / "left" / "o6_left_transferred.urdf", + ) + original = _joint_elements(LEFT_SOURCE) + transferred = _joint_elements(destination) + corrected_right = _joint_elements(right) + for left_name, joint in transferred.items(): + right_name = left_name.replace("lh_", "rh_", 1) + assert joint.find("origin").get("xyz") == original[left_name].find( + "origin" + ).get("xyz") + assert joint.find("axis").attrib == original[left_name].find("axis").attrib + assert joint.find("limit").get("upper") == corrected_right[ + right_name + ].find("limit").get("upper") + if joint.find("mimic") is not None: + assert joint.find("mimic").get("multiplier") == corrected_right[ + right_name + ].find("mimic").get("multiplier") + assert transferred["lh_thumb_cmc_pitch"].find("origin").get("rpy") == ( + original["lh_thumb_cmc_pitch"].find("origin").get("rpy") + ) + left_yaw = Rotation.from_euler( + "xyz", + [float(value) for value in transferred["lh_thumb_cmc_yaw"].find( + "origin" + ).get("rpy").split()], + ) + assert left_yaw.as_rotvec()[2] == pytest.approx(0.03, abs=1e-6) + + right_payload = build_o6_runtime_payload( + serial_number="RIGHT_TEST", + source_urdf=SOURCE, + result=result, + protected_inputs={ + "source_urdf_sha256": "0" * 64, + "camera_extrinsics_sha256": "1" * 64, + "calibration_config_sha256": "2" * 64, + "tag_config_sha256": "3" * 64, + }, + ) + left_payload = build_o6_left_transferred_runtime_payload( + right_payload=right_payload, + source_left_urdf=LEFT_SOURCE, + transferred_left_urdf=destination, + serial_number="LEFT_TRANSFER_TEST", + ) + validate_o6_runtime_payload(left_payload) + mapper = CalibratedCommandMapper(left_payload, expected_side="left") + assert mapper.profile_id == "O6/left/o6_left_transferred_8/v1" + assert set(mapper.urdf_joint_names) == set(transferred) + mapped = dict(zip( + mapper.urdf_joint_names, + mapper.map_positions([0, 255, 255, 255, 255, 255]), + )) + assert mapped["lh_thumb_cmc_pitch"] == pytest.approx(0.5, abs=2.0e-4) + assert mapped["lh_thumb_ip"] == pytest.approx(0.93, abs=2.0e-4) + assert left_payload["quality"]["transfer_provenance"][ + "left_hand_measured" + ] is False diff --git a/src/linkerhand_calibration/urdf/l6_left/linkerhand_l6v3.1_left.urdf b/src/linkerhand_calibration/urdf/l6_left/linkerhand_l6v3.1_left.urdf new file mode 100644 index 0000000..306f89a --- /dev/null +++ b/src/linkerhand_calibration/urdf/l6_left/linkerhand_l6v3.1_left.urdf @@ -0,0 +1,956 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0084 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0082 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0082 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0082 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0082 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0144 + + + + + + + + + + + + + + + + + + + + + 1.5 + 1.5 + true + + + 1.5 + 1.5 + true + + + 1.1 + 1.1 + true + + + 1.1 + 1.1 + true + + + 1.1 + 1.1 + true + + + 1.1 + 1.1 + true + + diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/hand_base_link.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/hand_base_link.STL new file mode 100644 index 0000000..861415f Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/hand_base_link.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/index_distal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/index_distal.STL new file mode 100644 index 0000000..15ba6e1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/index_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/index_proximal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/index_proximal.STL new file mode 100644 index 0000000..07cd2ab Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/index_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/middle_distal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/middle_distal.STL new file mode 100644 index 0000000..b5e1445 Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/middle_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/middle_proximal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/middle_proximal.STL new file mode 100644 index 0000000..c3b110d Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/middle_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/pinky_distal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/pinky_distal.STL new file mode 100644 index 0000000..abcf4da Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/pinky_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/pinky_proximal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/pinky_proximal.STL new file mode 100644 index 0000000..0d48b8e Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/ring_distal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/ring_distal.STL new file mode 100644 index 0000000..03cdc8d Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/ring_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/ring_proximal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/ring_proximal.STL new file mode 100644 index 0000000..1f4901a Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/ring_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_distal.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_distal.STL new file mode 100644 index 0000000..9c70d83 Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_metacarpals.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_metacarpals.STL new file mode 100644 index 0000000..5176c90 Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_metacarpals_base1.STL new file mode 100644 index 0000000..4d4c32f Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/thumb_metacarpals_base1.STL differ diff --git a/src/linkerhand_calibration/urdf/l6_left/meshes/wrist_ft_link.stl b/src/linkerhand_calibration/urdf/l6_left/meshes/wrist_ft_link.stl new file mode 100644 index 0000000..e377c27 Binary files /dev/null and b/src/linkerhand_calibration/urdf/l6_left/meshes/wrist_ft_link.stl differ diff --git a/src/linkerhand_calibration/urdf/o6_left/linkerhand_o6_left.urdf b/src/linkerhand_calibration/urdf/o6_left/linkerhand_o6_left.urdf new file mode 100644 index 0000000..1925ca0 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o6_left/linkerhand_o6_left.urdf @@ -0,0 +1,666 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/hand_base_link.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/hand_base_link.STL new file mode 100644 index 0000000..b7ab0c1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/hand_base_link.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/index_distal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/index_distal.STL new file mode 100644 index 0000000..db30679 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/index_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/index_proximal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/index_proximal.STL new file mode 100644 index 0000000..f427e3c Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/index_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/middle_distal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/middle_distal.STL new file mode 100644 index 0000000..a4838a5 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/middle_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/middle_proximal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/middle_proximal.STL new file mode 100644 index 0000000..e8d4268 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/middle_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/pinky_distal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/pinky_distal.STL new file mode 100644 index 0000000..abcb9c0 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/pinky_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/pinky_proximal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/pinky_proximal.STL new file mode 100644 index 0000000..5f013e6 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/ring_distal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/ring_distal.STL new file mode 100644 index 0000000..b8247b1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/ring_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/ring_proximal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/ring_proximal.STL new file mode 100644 index 0000000..7c47ff6 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/ring_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_distal.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_distal.STL new file mode 100644 index 0000000..19eee73 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_metacarpals.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_metacarpals.STL new file mode 100644 index 0000000..9f1ab59 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_metacarpals_base2.STL new file mode 100644 index 0000000..5e66834 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o6_left/meshes/thumb_metacarpals_base2.STL differ