diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/README.md b/src/linkerhand_calibration/linkerhand_calibration/models/o12/README.md new file mode 100644 index 0000000..2ef898a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/README.md @@ -0,0 +1,5 @@ +# O12 标定 + +本目录存放 O12 的专属程序、配置和左右手 URDF。 `core/`、`runtime/` 和 `profiles/` 中的通用程序仍由各型号共享。 + +旧的 `config/`、`urdf/` 路径及 `compat/legacy_diagnostic_tools/models/` 导入路径保留兼容;现有产品配置与历史采集记录中的资源内容和哈希不变。 diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py new file mode 100644 index 0000000..a7a5062 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py @@ -0,0 +1,16 @@ +"""Registered O12 calibration profiles.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from linkerhand_calibration.models.registry import ProfileRegistry + + +def register_profiles(registry: ProfileRegistry) -> None: + from linkerhand_calibration.models.o12.profile import build_profile + registry.register(build_profile()) + + +__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py new file mode 100644 index 0000000..e58bb61 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py @@ -0,0 +1,495 @@ +"""Schema-v7 radian-knot runtime artifact for O12.""" + +from __future__ import annotations + +from dataclasses import asdict +import math +from pathlib import Path +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.models.o12.fitting import O12FitResult +from linkerhand_calibration.models.o12.profile import ( + ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, + COMMAND_NAMES, + KEY, + MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, + PASSIVE_JOINTS, + SAFE_LOWER_RAD, + SAFE_UPPER_RAD, + SDK_LOWER_RAD, + SDK_TO_URDF_JOINT, + SDK_TO_URDF_SIGN, + SDK_UPPER_RAD, + STATIC_ZERO_EXCLUDED_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + build_typed_profile, +) +from linkerhand_calibration.models.o12.urdf import o12_correction_plan +from linkerhand_calibration.models.o12.zero import SPATIAL_ZERO_POLICY + + +ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS) + + +def _motor_index(joint: str) -> int: + source = str(joint) + while source not in COMMAND_INDEX_BY_JOINT: + source = MIMIC_SOURCE_BY_JOINT[source] + return COMMAND_INDEX_BY_JOINT[source] + + +def _source_joint_metadata(source_urdf: str | Path) -> dict[str, dict[str, Any]]: + root = ET.parse(Path(source_urdf)).getroot() + result: dict[str, dict[str, Any]] = {} + for node in root.findall("joint"): + name = str(node.get("name", "")) + if name not in ALL_REVOLUTE_JOINTS: + continue + limit = node.find("limit") + if node.get("type") != "revolute" or limit is None: + raise ValueError(f"invalid O12 revolute joint: {name}") + item: dict[str, Any] = { + "lower": float(limit.get("lower", "nan")), + "upper": float(limit.get("upper", "nan")), + } + mimic = node.find("mimic") + if mimic is not None: + item.update({ + "source_joint": str(mimic.get("joint", "")), + "multiplier": float(mimic.get("multiplier", "nan")), + "offset": float(mimic.get("offset", "0")), + }) + result[name] = item + if set(result) != ALL_REVOLUTE_JOINTS: + raise ValueError("source URDF must contain all 19 O12 revolute joints") + return result + + +def _round(values: Sequence[float], digits: int = 10) -> list[float]: + array = np.asarray(values, dtype=float) + if array.shape != (65,) or not np.all(np.isfinite(array)): + raise ValueError("O12 knot curve must contain 65 finite values") + return [round(float(value), digits) for value in array] + + +def build_o12_runtime_payload( + *, + serial_number: str, + source_urdf: str | Path, + result: O12FitResult, + protected_inputs: Mapping[str, str], + passed: bool, +) -> dict[str, Any]: + profile = build_typed_profile() + if result.full_hand_zero_result is not None and not result.full_hand_zero_result.passed: + raise ValueError("O12 full-hand spatial zero validation has not passed") + for name in STATIC_ZERO_EXCLUDED_JOINTS: + if ( + not math.isclose( + float(result.zero_offsets_rad.get(name, math.nan)), + 0.0, + rel_tol=0.0, + abs_tol=1.0e-12, + ) + or result.zero_method_by_joint.get(name) + != "source_cad_zero_profile_excluded" + ): + raise ValueError( + f"O12 {name} must retain immutable source-CAD static zero" + ) + plan = o12_correction_plan(source_urdf, result) + curves: dict[str, tuple[np.ndarray, np.ndarray, np.ndarray, list[float]]] = {} + joints: dict[str, dict[str, Any]] = {} + + for name in ACTIVE_JOINTS: + donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + feedback_domain = result.feedback_domains_rad[donor] + mapping = result.output_mappings[name] + inputs = list(mapping.knots) + motor = mapping.motor_index + sign = float(SDK_TO_URDF_SIGN[motor]) + decreasing = np.asarray(mapping.decreasing_rad) + increasing = np.asarray(mapping.increasing_rad) + angle = np.asarray(mapping.angle_rad) + if donor != name: + lower, upper = plan.limits_output_rad[name] + values = np.concatenate((angle, decreasing, increasing)) + if np.min(values) < lower - 1e-9 or np.max(values) > upper + 1e-9: + raise ValueError(f"transferred curve exceeds recipient physical range: {name}; no clipping is permitted") + curves[name] = (angle, decreasing, increasing, inputs) + joint: dict[str, Any] = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor], + "motor_index": motor, + "passive": False, + "calibration_status": ( + "transferred_static_dynamic" if donor != name + else "measured_dynamic_cad_static" + if str(result.zero_method_by_joint[name]).startswith("source_cad_zero") + else "measured_static_dynamic" + ), + "curve_input_knots_rad": _round(inputs), + "angle_rad": _round(angle), + "decreasing_rad": _round(decreasing), + "increasing_rad": _round(increasing), + "zero_feedback_rad": 0.0, + "measured_feedback_domain_rad": [ + round(float(value), 10) for value in feedback_domain + ], + "static_urdf_origin_offset_rad": round( + float(result.zero_offsets_rad[name]), 10 + ), + "static_zero_method": str(result.zero_method_by_joint[name]), + "sdk_to_urdf_direction": sign, + "urdf_sign_adapter": "negate" if sign < 0.0 else "identity", + "runtime_mapping_source": "tag_rotation_over_sdk_feedback_rad", + "visual_arc_diagnostic_rad": round( + float(result.visual_arc_diagnostics_rad[donor]), 10 + ), + "raw_increasing_curve_branch": "increasing", + } + if donor != name: + joint["transferred_from_joint"] = donor + joint["transfer_policy"] = ( + "donor_scalar_mapping_on_own_cad_frame" + ) + joint["static_zero_transfer_policy"] = "donor_offset_on_own_cad_frame" + joints[name] = joint + + for name in PASSIVE_JOINTS: + source_name, multiplier, offset = plan.mimic_output[name] + motor = _motor_index(source_name) + source_curve, source_dec, source_inc, inputs = curves[source_name] + angle = offset + multiplier * source_curve + decreasing = offset + multiplier * source_dec + increasing = offset + multiplier * source_inc + coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0] + coupling_model = "linear_mimic" + policy = ("visual_standard_linear_mimic" if name in MEASURED_PASSIVE_JOINTS + else "cad_nominal_coordinate_transformed") + status = profile.joint_coverage[name] + curves[name] = (angle, decreasing, increasing, list(inputs)) + joint = { + "urdf_joint": name, + "sdk_channel": COMMAND_NAMES[motor], + "motor_index": motor, + "passive": True, + "source_joint": source_name, + "calibration_status": status, + "static_zero_policy": "unobservable_static_offset_retains_cad", + "curve_input_knots_rad": _round(inputs), + "angle_rad": _round(angle), + "decreasing_rad": _round(decreasing), + "increasing_rad": _round(increasing), + "coupling_model": coupling_model, + "coupling_coefficients": [round(float(v), 10) for v in coefficients], + "mimic_multiplier": round(float(multiplier), 10), + "urdf_mimic_policy": policy, + "runtime_mapping_source": ( + "exported_standard_urdf_mimic" + ), + "raw_increasing_curve_branch": "increasing", + } + if name in MEASURED_PASSIVE_JOINTS: + evidence = result.standard_mimic_evidence[name] + joint.update({ + "measured_feedback_domain_rad": list(result.feedback_domains_rad[name]), + "visual_observation_role": "independent_linear_mimic_training_and_holdout", + "observed_coupling_model": "linear_mimic", + "observed_mimic_multiplier": round(evidence.fit.multiplier, 10), + "observed_residual_rms_rad": round(float(np.sqrt(np.mean(np.square(evidence.fit.residuals_rad)))), 10), + "observed_visual_arc_rad": round(float(result.visual_arc_diagnostics_rad[name]), 10), + "linear_mimic_holdout_mae_rad": float(np.mean(np.abs(evidence.holdout_errors_rad))), + "linear_mimic_holdout_max_rad": float(np.max(np.abs(evidence.holdout_errors_rad))), + }) + joints[name] = joint + + errors = np.abs(np.concatenate([ + np.asarray(values, dtype=float) + for values in result.holdout_errors_rad.values() + ])) + payload: dict[str, Any] = { + "schema_version": 7, + "profile_id": KEY.profile_id, + "layout_id": KEY.layout, + "model": "O12", + "side": "right", + "serial_number": str(serial_number), + "calibration_scope": "full_dynamic_except_thumb_mcp_static_zero", + "publication_pointer": "latest_passed", + "angle_unit": "rad", + "curve_input_domain": "feedback_rad", + "runtime_curve_policy": "direction_aware_knots", + "command_names": list(COMMAND_NAMES), + "command_lower_rad": list(SAFE_LOWER_RAD), + "command_upper_rad": list(SAFE_UPPER_RAD), + "baseline_command_rad": [0.0] * 12, + "sdk_mapping": [ + { + "motor_index": i, + "sdk_channel": command, + "urdf_joint": urdf, + "sdk_lower_rad": SDK_LOWER_RAD[i], + "sdk_upper_rad": SDK_UPPER_RAD[i], + "calibration_lower_rad": SAFE_LOWER_RAD[i], + "calibration_upper_rad": SAFE_UPPER_RAD[i], + } + for i, (command, urdf) in enumerate(zip(COMMAND_NAMES, SDK_TO_URDF_JOINT)) + ], + "protected_inputs": dict(protected_inputs), + "joints": joints, + "quality": { + "passed": bool(passed), + "training_cycles": [0, 1, 2], + "holdout_cycle": 3, + "validation_mae_rad": round(float(np.mean(errors)), 10), + "validation_p95_rad": round(float(np.percentile(errors, 95.0)), 10), + "validation_max_rad": round(float(np.max(errors)), 10), + "roll_cross_view": { + joint: { + key: round(float(value), 10) + for key, value in metrics.items() + } + for joint, metrics in result.cross_view_roll_metrics.items() + }, + "thumb_root_spatial_zero": ( + None + if result.thumb_root_zero_result is None + else { + "offsets_rad": { + name: round(float(value), 10) + for name, value in result.thumb_root_zero_result.direct_offsets_rad.items() + }, + "validation_mae_rad": round(float(np.mean(np.abs( + result.thumb_root_zero_result.validation_errors_rad + ))), 10), + "validation_p95_rad": round(float(np.percentile(np.abs( + result.thumb_root_zero_result.validation_errors_rad + ), 95.0)), 10), + "validation_max_rad": round(float(np.max(np.abs( + result.thumb_root_zero_result.validation_errors_rad + ))), 10), + "axis_line_rms_m_diagnostic": round(float( + result.thumb_root_zero_result.axis_line_rms_m + ), 10), + "method": "roll_yaw_pitch_axes_with_pinky_palm_orientation", + "validation_scope": "root_axis_angles_not_full_chain_position", + } + ), + "ring_transfer_source": "pinky_mcp_pitch", + "full_hand_spatial_zero": ( + None if result.full_hand_zero_result is None else { + **asdict(result.full_hand_zero_result), + "policy": SPATIAL_ZERO_POLICY, + "validation_scope": "active_zero_axis_geometry_not_fingertip_contact", + } + ), + "cad_static_joints_not_measured": [ + name for name, method in result.zero_method_by_joint.items() + if str(method).startswith("source_cad_zero") + ], + "static_zero_exclusions": { + name: { + "static_origin_source": "immutable_source_cad", + "reason": "downstream_curved_shell_tag_not_absolute_axis_line_datum", + "dynamic_curve_and_holdout_required": True, + } + for name in sorted(STATIC_ZERO_EXCLUDED_JOINTS) + }, + "ring_cad_geometry_and_mimic_preserved": True, + "active_angle_source": "tag_rotation_over_sdk_feedback_rad", + "passive_runtime_source": "exported_standard_urdf_mimic", + "visual_measurement_role": "active_curve_and_passive_linear_fit_with_independent_holdout", + }, + } + validate_o12_runtime_payload(payload) + return payload + + +def validate_o12_runtime_payload(payload: Mapping[str, Any]) -> None: + required = { + "schema_version", "profile_id", "layout_id", "model", "side", + "serial_number", "calibration_scope", "publication_pointer", + "angle_unit", "curve_input_domain", "runtime_curve_policy", + "command_names", "command_lower_rad", "command_upper_rad", + "baseline_command_rad", "sdk_mapping", "protected_inputs", "joints", + "quality", + } + if set(payload) != required: + raise ValueError("schema v7 O12 artifact has unexpected top-level fields") + if ( + payload["schema_version"] != 7 + or payload["profile_id"] != KEY.profile_id + or payload["model"] != "O12" + or payload["side"] != "right" + or payload["angle_unit"] != "rad" + or payload["curve_input_domain"] != "feedback_rad" + or payload["runtime_curve_policy"] != "direction_aware_knots" + ): + raise ValueError("schema v7 O12 artifact identity/domain is invalid") + if tuple(payload["command_names"]) != COMMAND_NAMES: + raise ValueError("O12 SDK channel order differs from fixed contract") + if len(payload["sdk_mapping"]) != 12 or any( + int(row.get("motor_index", -1)) != i + or row.get("sdk_channel") != COMMAND_NAMES[i] + or row.get("urdf_joint") != SDK_TO_URDF_JOINT[i] + or float(row.get("sdk_lower_rad", math.nan)) != SDK_LOWER_RAD[i] + or float(row.get("sdk_upper_rad", math.nan)) != SDK_UPPER_RAD[i] + or float(row.get("calibration_lower_rad", math.nan)) != SAFE_LOWER_RAD[i] + or float(row.get("calibration_upper_rad", math.nan)) != SAFE_UPPER_RAD[i] + for i, row in enumerate(payload["sdk_mapping"]) + ): + raise ValueError("O12 SDK/URDF mapping differs from fixed contract") + protected = payload["protected_inputs"] + expected = build_typed_profile().artifacts.protected_input_fields + if not isinstance(protected, Mapping) or set(protected) != expected: + raise ValueError("O12 protected input hashes are incomplete") + if any(len(str(v)) != 64 or any(c not in "0123456789abcdef" for c in str(v)) for v in protected.values()): + raise ValueError("O12 protected input hash is invalid") + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS: + raise ValueError("schema v7 must contain all 19 O12 revolute joints") + for name, joint in joints.items(): + inputs = np.asarray(joint.get("curve_input_knots_rad"), dtype=float) + if inputs.shape != (65,) or not np.all(np.isfinite(inputs)) or np.any(np.diff(inputs) <= 0.0): + raise ValueError(f"{name} has invalid radian knots") + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(joint.get(field), dtype=float) + if values.shape != (65,) or not np.all(np.isfinite(values)): + raise ValueError(f"{name}.{field} must contain 65 finite values") + if int(joint.get("motor_index", -1)) != _motor_index(name): + raise ValueError(f"{name} has invalid O12 motor binding") + if joint.get("raw_increasing_curve_branch") not in {"decreasing", "increasing"}: + raise ValueError(f"{name} has invalid O12 direction binding") + for name in ACTIVE_JOINTS: + joint = joints[name] + motor = COMMAND_INDEX_BY_JOINT[name] + sign = float(SDK_TO_URDF_SIGN[motor]) + offset = float(joint.get("static_urdf_origin_offset_rad", math.nan)) + if not math.isfinite(offset) or abs(offset) > math.radians(20.0): + raise ValueError(f"{name} has invalid O12 static origin offset") + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + differences = np.diff(np.asarray(joint[field], dtype=float)) + if np.any(sign * differences < -1.0e-7): + raise ValueError( + f"{name}.{field} disagrees with fixed O12 SDK/URDF direction" + ) + for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): + if not math.isclose( + float(joints[target]["static_urdf_origin_offset_rad"]), + float(joints[donor]["static_urdf_origin_offset_rad"]), + rel_tol=0.0, abs_tol=1.0e-9, + ): + raise ValueError(f"{target} static zero differs from transfer donor {donor}") + for name in PASSIVE_JOINTS: + joint = joints[name] + source_joint = str(joint.get("source_joint", "")) + if source_joint != MIMIC_SOURCE_BY_JOINT[name]: + raise ValueError(f"{name} has invalid O12 mimic source") + multiplier = float(joint.get("mimic_multiplier", math.nan)) + coefficients = np.asarray( + joint.get("coupling_coefficients", ()), dtype=float + ) + if coefficients.shape != (6,) or not np.all(np.isfinite(coefficients)): + raise ValueError(f"{name} has invalid O12 CAD mimic coefficients") + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"{name} has invalid O12 mimic multiplier") + if joint.get("coupling_model") != "linear_mimic" or not np.allclose( + coefficients[1:], [multiplier, 0, 0, 0, 0], rtol=0, atol=2e-9 + ): + raise ValueError(f"{name} cannot replace standard URDF mimic with a nonlinear JSON model") + if not np.allclose(joint["curve_input_knots_rad"], joints[source_joint]["curve_input_knots_rad"], rtol=0, atol=1e-10): + raise ValueError(f"{name} mimic and source must have the same input knots") + offset = float(coefficients[0]) + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + expected_values = offset + multiplier * np.asarray(joints[source_joint][field], dtype=float) + if not np.allclose(np.asarray(joint[field], dtype=float), expected_values, rtol=0, atol=2e-9): + raise ValueError(f"{name}.{field} differs from standard URDF mimic chain") + if not bool(payload["quality"].get("passed")): + raise ValueError("failed O12 calibration cannot be published") + + +def validate_o12_runtime_payload_against_urdf( + payload: Mapping[str, Any], urdf: str | Path, + *, source_urdf: str | Path | None = None, +) -> None: + """Check curve bounds/mimics and, with original CAD, actual static frames. + + This verifies the artifact contract, not full-hand physical accuracy. + Passive runtime values must equal the actual exported linear URDF mimic. + """ + validate_o12_runtime_payload(payload) + metadata = _source_joint_metadata(urdf) + joints = payload["joints"] + for name, joint in joints.items(): + expected = metadata[name] + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(joint[field], dtype=float) + if bool(joint.get("passive")): + source_name = str(expected["source_joint"]) + if source_name != str(joint["source_joint"]): + raise ValueError( + f"{name} corrected URDF mimic source differs from payload" + ) + if not math.isclose( + float(expected["multiplier"]), + float(joint["mimic_multiplier"]), + rel_tol=0.0, + abs_tol=2.0e-9, + ): + raise ValueError( + f"{name} corrected URDF mimic differs from payload" + ) + source_values = np.asarray(joints[source_name][field], dtype=float) + urdf_values = float(expected["offset"]) + float(expected["multiplier"]) * source_values + if not np.allclose(values, urdf_values, rtol=0, atol=2e-9): + raise ValueError(f"{name}.{field} differs from exported standard URDF mimic") + if ( + float(np.min(values)) + < float(expected["lower"]) - 2.0e-9 + ): + raise ValueError(f"{name}.{field} is below the URDF physical limit") + if ( + float(np.max(values)) + > float(expected["upper"]) + 2.0e-9 + ): + raise ValueError(f"{name}.{field} is above the URDF physical limit") + if source_urdf is not None: + original = ET.parse(source_urdf).getroot() + corrected = ET.parse(urdf).getroot() + for name in ACTIVE_JOINTS: + before = original.find(f"joint[@name='{name}']") + after = corrected.find(f"joint[@name='{name}']") + if before is None or after is None: + raise ValueError(f"{name} missing from static-frame validation") + b_origin, a_origin = before.find("origin"), after.find("origin") + if b_origin is None or a_origin is None: + raise ValueError(f"{name} missing static origin") + if b_origin.get("xyz") != a_origin.get("xyz"): + raise ValueError(f"{name} static origin translation changed") + axis_node = before.find("axis") + if axis_node is None: + raise ValueError(f"{name} missing CAD axis") + axis = np.asarray([float(v) for v in axis_node.get("xyz").split()]) + axis /= np.linalg.norm(axis) + b_rot = Rotation.from_euler("xyz", [float(v) for v in b_origin.get("rpy", "0 0 0").split()]) + a_rot = Rotation.from_euler("xyz", [float(v) for v in a_origin.get("rpy", "0 0 0").split()]) + expected_rot = b_rot * Rotation.from_rotvec( + axis * float(joints[name]["static_urdf_origin_offset_rad"]) + ) + if (expected_rot.inv() * a_rot).magnitude() > 1.0e-8: + raise ValueError(f"{name} static origin disagrees with payload") + + +__all__ = [ + "ALL_REVOLUTE_JOINTS", + "build_o12_runtime_payload", + "validate_o12_runtime_payload", + "validate_o12_runtime_payload_against_urdf", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_right_16_tags.yaml b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_right_16_tags.yaml new file mode 100644 index 0000000..8b19198 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_right_16_tags.yaml @@ -0,0 +1,41 @@ +/o12_calibration/front/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false} + pose_estimation_method: pnp + tag: + ids: [0, 1, 2, 3, 12, 13] + frames: [front_base, thumb_cmc, thumb_mcp, thumb_dip, middle_roll, index_roll] + sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + +/o12_calibration/side/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false} + pose_estimation_method: pnp + tag: + ids: [4, 5, 6, 7, 8, 9, 10, 11] + frames: [side_base, pinky_mcp, pinky_pip, pinky_dip, middle_pip, middle_dip, index_pip, index_dip] + sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + +/o12_calibration/top/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + max_hamming: 0 + detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false} + pose_estimation_method: pnp + tag: + ids: [14, 15] + frames: [top_base, thumb_yaw] + sizes: [0.016, 0.016] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_right_product.yaml b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_right_product.yaml new file mode 100644 index 0000000..510d00e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_right_product.yaml @@ -0,0 +1,47 @@ +schema_version: 3 +profile_id: O12/right/o12_right_16/v1 +profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml +profile_config_sha256: 865ed7ea58ccdeecbbdd79a68c89e45f784050644a9a3581571623a77de5f9c1 +model: O12 +side: right +tag_layout: o12_right_16 +namespace: /o12_calibration +serial_number: O12_RIGHT_001 +output_root: calibration_output + +sdk: + driver: o12_sdk_bridge + python_package: src/agillink_omnihand_sdk/linux/x64/python/omnihand-1.1.8-cp312-cp312-linux_x86_64.whl + package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170 + transport: hcan + setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash + config: package://linkerhand_calibration/config/o12_sdk.yaml + config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c + +cameras: + front: + serial_number: DB2163742 + camera_name: hikrobot_front_DB2163742 + camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml + side: + serial_number: DB2163749 + camera_name: hikrobot_side_DB2163749 + camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml + top: + serial_number: DB2163739 + camera_name: hikrobot_top_DB2163739 + camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml + +artifacts: + source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf + source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb + camera_extrinsics: config/o12_three_camera_extrinsics.yaml + camera_extrinsics_sha256: 5a515d0706f4e67d5e26bfddb348b519817bd72e885ea9e43997e41016176e53 + calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml + calibration_config_sha256: c2fcdd532e22c15013413e655311de2ca87107c23a96d5c043e8f340f232eebd + tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml + tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab + +release: + required_independent_passes: 1 + static_repeatability_deg: 1.0 diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_sdk.yaml b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_sdk.yaml new file mode 100644 index 0000000..706fe46 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_sdk.yaml @@ -0,0 +1,45 @@ +# OmniHand Pro 2025 (O12) Configuration +# connection_type: "zlgcan" | "hcan" | "socketcan" | "rs485" | "usb" +# request_interval_ms: min gap between requests, SDK default 0 = no throttling, range 0..100 +# frame_recv_timeout_ms: single-frame reply wait, SDK default 50, range 10..1000 + +/**: + ros__parameters: + # This workstation uses one O12 right hand through HCAN device 0/channel 0. + # Leave left_hand undefined so the node does not try to open a second hand. + right_hand: + hand_device_id: 1 + connection_type: "hcan" + canfd_device_id: 0 + canfd_channel_id: 0 + # O12 POSITION streams set-points and has no G20-style firmware speed + # interpolation, so do not throttle the 50 Hz calibration command stream. + request_interval_ms: 0 + frame_recv_timeout_ms: 100 + show_data_details: false + + # Alternative ZLG CANFD example: + # right_hand: + # hand_device_id: 1 + # connection_type: "zlgcan" + # canfd_device_id: 0 + # canfd_channel_id: 0 + # request_interval_ms: 0 + # frame_recv_timeout_ms: 50 + # show_data_details: false + # socketcan example + # left_hand: + # hand_device_id: 1 + # connection_type: "socketcan" + # can_interface: "can0" + # request_interval_ms: 0 + # frame_recv_timeout_ms: 50 + # show_data_details: true + + # right_hand: + # hand_device_id: 1 + # connection_type: "socketcan" + # can_interface: "can1" + # request_interval_ms: 0 + # frame_recv_timeout_ms: 50 + # show_data_details: true diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_three_camera_calibration.yaml b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_three_camera_calibration.yaml new file mode 100644 index 0000000..1ba194c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/o12_three_camera_calibration.yaml @@ -0,0 +1,53 @@ +o12_calibration: + ros__parameters: + command_topic: /o12/right/joint_cmd + state_topic: /o12/right/joint_states + front_camera_info_topic: /o12_calibration/front/camera/camera_info + front_detections_topic: /o12_calibration/front/apriltag/detections + side_camera_info_topic: /o12_calibration/side/camera/camera_info + side_detections_topic: /o12_calibration/side/apriltag/detections + top_camera_info_topic: /o12_calibration/top/camera/camera_info + top_detections_topic: /o12_calibration/top/apriltag/detections + + # O12 standard interface is radians. Scan the complete reviewed SDK range; + # the source CAD limits are outputs to correct, not acquisition limits. + # Raw 0..2000 mixed control is forbidden. + command_rate_hz: 50.0 + repetitions: 4 + tag_size_m: 0.016 + tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15] + tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + minimum_detection_rate: 0.95 + minimum_joint_frame_rate: 0.85 + minimum_feedback_hz: 35.0 + maximum_state_image_skew_ms: 50.0 + minimum_sweep_frames: 40 + minimum_state_span_fraction: 0.90 + minimum_sweep_bins: 32 + maximum_bin_gap: 2 + endpoint_tolerance_rad: 0.01 + endpoint_hold_seconds: 0.5 + motor_stall_timeout_seconds: 2.0 + position_timeout_seconds: 60.0 + sweep_timeout_seconds: 180.0 + automatic_sweep_retry_limit: 1 + non_target_motion_tolerance_rad: 0.015 + maximum_temperature_c: 70 + # 当前 O12 固件返回空温度报告。仍发起查询;5秒无结果后依赖已验证的 + # joint_error_states bit1 过热保护,并在会话记录中明确标注降级。 + temperature_report_required: false + temperature_fallback_after_seconds: 5.0 + # 快速标定档:各任务按4倍请求,但拇指pitch、侧摆、屈曲及避让均有独立硬上限。 + motion_speed_scale: 4.0 + # 正弦速度加减速时间;中段保持关节限速,避免位置余弦全程低速。 + trajectory_ramp_seconds: 0.4 + maximum_hamming: 0 + minimum_decision_margin: 30.0 + minimum_edge_pixels: 30.0 + fixed_base_maximum_corner_drift_px: 5.0 + fixed_base_movement_confirmation_frames: 10 + pnp_maximum_reprojection_error_px: 1.5 + pnp_maximum_pose_jump_deg: 35.0 + pnp_maximum_translation_jump_m: 0.04 + pnp_maximum_tag_tilt_deg: 75.0 + pnp_tracker_reset_seconds: 5.0 diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/profiles/o12_right_16.yaml b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/profiles/o12_right_16.yaml new file mode 100644 index 0000000..fed6d9d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/config/profiles/o12_right_16.yaml @@ -0,0 +1,1029 @@ +schema_version: 1 +profile_id: O12/right/o12_right_16/v1 +namespace: /o12_calibration +sdk_adapter: o12_hcan_sdk +command: + unit: rad + feedback_by_index: true + sdk_to_joint_direction: + - 1 + - -1 + - -1 + - -1 + - -1 + - 1 + - 1 + - -1 + - 1 + - 1 + - 1 + - 1 + names: + - thumb_roll + - thumb_abad + - thumb_mcp + - thumb_pip + - index_abad + - index_mcp + - index_pip + - middle_abad + - middle_mcp + - middle_pip + - ring_mcp + - pinky_mcp + baseline_u8: [] + baseline: + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + lower_bounds: + - 0 + - -1.387536755335492 + - -0.8272860654453121 + - -1.2915436464758039 + - -0.2617993877991494 + - 0 + - 0 + - -0.2617993877991494 + - 0 + - 0 + - 0 + - 0 + upper_bounds: + - 0.9424777960769379 + - 0 + - 0 + - 0 + - 0.2617993877991494 + - 1.3526301702956054 + - 1.530653753999027 + - 0.2617993877991494 + - 1.3578661580515883 + - 1.8151424220741028 + - 1.53588974175501 + - 1.53588974175501 + feedback_lower_bounds: + - -0.03490658503988659 + - -1.4224433403753787 + - -0.8621926504851987 + - -1.3264502315156905 + - -0.296705972839036 + - -0.03490658503988659 + - -0.03490658503988659 + - -0.296705972839036 + - -0.03490658503988659 + - -0.03490658503988659 + - -0.03490658503988659 + - -0.03490658503988659 + feedback_upper_bounds: + - 0.9773843811168246 + - 0.03490658503988659 + - 0.03490658503988659 + - 0.03490658503988659 + - 0.296705972839036 + - 1.387536755335492 + - 1.5655603390389137 + - 0.296705972839036 + - 1.392772743091475 + - 1.8500490071139892 + - 1.5707963267948966 + - 1.5707963267948966 + command_index_by_joint: + thumb_cmc_roll: 0 + thumb_cmc_yaw: 1 + thumb_cmc_pitch: 2 + thumb_mcp: 3 + index_mcp_roll: 4 + index_mcp_pitch: 5 + index_pip: 6 + middle_mcp_roll: 7 + middle_mcp_pitch: 8 + middle_pip: 9 + ring_mcp_pitch: 10 + pinky_mcp_pitch: 11 + urdf_joint_by_joint: + thumb_cmc_roll: thumb_cmc_roll + thumb_cmc_yaw: thumb_cmc_yaw + thumb_cmc_pitch: thumb_cmc_pitch + thumb_mcp: thumb_mcp + index_mcp_roll: index_mcp_roll + index_mcp_pitch: index_mcp_pitch + index_pip: index_pip + middle_mcp_roll: middle_mcp_roll + middle_mcp_pitch: middle_mcp_pitch + middle_pip: middle_pip + ring_mcp_pitch: ring_mcp_pitch + pinky_mcp_pitch: pinky_mcp_pitch + maximum_velocity: + - 0.16 + - 0.16 + - 0.1 + - 0.32 + - 0.12 + - 0.32 + - 0.32 + - 0.12 + - 0.32 + - 0.32 + - 0.32 + - 0.32 +vision: + common_frame: calibration_common + extrinsic_reference_view: front + extrinsics_quality_limits: + reprojection_rms_px: 2.0 + maximum_rotation_repeatability_deg: 0.3 + maximum_translation_repeatability_m: 0.0015 + minimum_capture_counts: + front_side_captures: 15 + front_top_captures: 15 + views: + - name: front + tags: + - role: front_base + id: 0 + fixed_reference: true + - role: thumb_cmc + id: 1 + - role: thumb_mcp + id: 2 + - role: thumb_dip + id: 3 + - role: middle_roll + id: 12 + - role: index_roll + id: 13 + - name: side + tags: + - role: side_base + id: 4 + fixed_reference: true + - role: pinky_mcp + id: 5 + - role: pinky_pip + id: 6 + - role: pinky_dip + id: 7 + - role: middle_pip + id: 8 + - role: middle_dip + id: 9 + - role: index_pip + id: 10 + - role: index_dip + id: 11 + - name: top + tags: + - role: top_base + id: 14 + fixed_reference: true + - role: thumb_yaw + id: 15 +motion: + speed_parameters: + command_rate_hz: 50 + clearance_flex_rad_s: 0.1 + clearance_splay_rad_s: 0.04 + probe_travel_rad: 0.05235987755982989 + probe_speed_rad_s: 0.02 + endpoint_hold_seconds: 1.0 + stall_timeout_seconds: 2.0 + baseline_rad_s: 0.2 + trajectory_ramp_seconds: 0.4 + tasks: + - key: thumb_pitch_front + view: front + command_index: 2 + joints: + - thumb_cmc_pitch + start: 0 + end: -0.8272860654453121 + preflight_speed: 0.06 + formal_speed: 0.1 + preparation_groups: + - - 0 + - 1 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 2 + - key: thumb_roll_front + view: front + command_index: 0 + joints: + - thumb_cmc_roll + start: 0 + end: 0.9424777960769379 + preflight_speed: 0.06 + formal_speed: 0.16 + preparation_groups: + - - 1 + - 2 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 0 + - key: thumb_mcp_dip_front + view: front + command_index: 3 + joints: + - thumb_mcp + - thumb_dip + start: 0 + end: -1.2915436464758039 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 0 + - 1 + - 2 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 3 + - key: thumb_yaw_top + view: top + command_index: 1 + joints: + - thumb_cmc_yaw + start: 0 + end: -1.387536755335492 + preflight_speed: 0.06 + formal_speed: 0.16 + preparation_groups: + - - 0 + - 2 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 1 + - key: pinky_chain_side + view: side + command_index: 11 + joints: + - pinky_mcp_pitch + - pinky_pip + - pinky_dip + start: 0 + end: 1.53588974175501 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 0 + - 1 + - 2 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - - 11 + - key: middle_roll_front + view: front + command_index: 7 + joints: + - middle_mcp_roll + auxiliary_commands: + - - 4 + - -0.17453292519943295 + - - 5 + - 0.11 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - 8 + - 0.17 + start: 0.2617993877991494 + end: -0.2617993877991494 + preflight_speed: 0.06 + formal_speed: 0.12 + preparation_groups: + - - 10 + - 11 + - - 5 + - 6 + - - 4 + - - 0 + - 1 + - 2 + - 3 + - - 8 + - 9 + - - 7 + - key: middle_mcp_side + view: side + command_index: 8 + joints: + - middle_mcp_pitch + auxiliary_commands: + - - 4 + - -0.17453292519943295 + - - 5 + - 0.11 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.3578661580515883 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 10 + - 11 + - - 5 + - 6 + - - 4 + - - 0 + - 1 + - 2 + - 3 + - - 7 + - 9 + - - 8 + - key: middle_pip_dip_side + view: side + command_index: 9 + joints: + - middle_pip + - middle_dip + auxiliary_commands: + - - 4 + - -0.17453292519943295 + - - 5 + - 0.11 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.8151424220741028 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 10 + - 11 + - - 5 + - 6 + - - 4 + - - 0 + - 1 + - 2 + - 3 + - - 7 + - 8 + - - 9 + - key: index_roll_front + view: front + command_index: 4 + joints: + - index_mcp_roll + auxiliary_commands: + - - 7 + - 0 + - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - 5 + - 0.17 + start: 0.2617993877991494 + end: -0.2617993877991494 + preflight_speed: 0.06 + formal_speed: 0.12 + entry_waypoints: + - - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - - 4 + - 0 + - - 7 + - 0 + - - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + preparation_groups: + - - 10 + - 11 + - - 7 + - - 8 + - 9 + - - 0 + - 1 + - 2 + - 3 + - - 5 + - 6 + - - 4 + - key: index_mcp_side + view: side + command_index: 5 + joints: + - index_mcp_pitch + auxiliary_commands: + - - 7 + - 0 + - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.3526301702956054 + preflight_speed: 0.06 + formal_speed: 0.32 + entry_waypoints: + - - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - - 4 + - 0 + - - 7 + - 0 + - - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + preparation_groups: + - - 10 + - 11 + - - 7 + - - 8 + - 9 + - - 0 + - 1 + - 2 + - 3 + - - 4 + - 6 + - - 5 + - key: index_pip_dip_side + view: side + command_index: 6 + joints: + - index_pip + - index_dip + auxiliary_commands: + - - 7 + - 0 + - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.530653753999027 + preflight_speed: 0.06 + formal_speed: 0.32 + entry_waypoints: + - - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - - 4 + - 0 + - - 7 + - 0 + - - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + preparation_groups: + - - 10 + - 11 + - - 7 + - - 8 + - 9 + - - 0 + - 1 + - 2 + - 3 + - - 4 + - 5 + - - 6 + return_groups: + - - 4 + - 7 + - - 8 + - 9 + - - 10 + - 11 + - - 0 + - 1 + - 2 + - 3 + - 5 + - 6 + joint_zero_references: + thumb_cmc_pitch: + task_key: thumb_pitch_front + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.0, 0.0, -0.8272860654453121, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + thumb_cmc_roll: + task_key: thumb_roll_front + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.9424777960769379, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + thumb_mcp: + task_key: thumb_mcp_dip_front + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.0, 0.0, 0.0, -1.2915436464758039, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + thumb_dip: + task_key: thumb_mcp_dip_front + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.0, 0.0, 0.0, -1.2915436464758039, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + thumb_cmc_yaw: + task_key: thumb_yaw_top + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.0, -1.387536755335492, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pinky_mcp_pitch: + task_key: pinky_chain_side + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501] + pinky_pip: + task_key: pinky_chain_side + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501] + pinky_dip: + task_key: pinky_chain_side + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501] + middle_mcp_roll: + task_key: middle_roll_front + command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.17, 0.0, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, -0.2617993877991494, 0.17, 0.0, 1.53588974175501, 1.53588974175501] + middle_mcp_pitch: + task_key: middle_mcp_side + command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 1.3578661580515883, 0.0, 1.53588974175501, 1.53588974175501] + middle_pip: + task_key: middle_pip_dip_side + command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + middle_dip: + task_key: middle_pip_dip_side + command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + index_mcp_roll: + task_key: index_roll_front + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.17, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, -0.2617993877991494, 0.17, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + index_mcp_pitch: + task_key: index_mcp_side + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, 0.0, 1.3526301702956054, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + index_pip: + task_key: index_pip_dip_side + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.530653753999027, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + index_dip: + task_key: index_pip_dip_side + command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] + approach_commands: + - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.530653753999027, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501] +measurement: + transferred_motion_sources: + ring_mcp_pitch: pinky_mcp_pitch + ring_pip: pinky_pip + ring_dip: pinky_dip + candidate_selection_tasks: + - thumb_mcp_dip_front + directional_zero: true + stable_cross_view_cone_bias: true + cross_view_sources: + middle_mcp_roll: middle_mcp_pitch + index_mcp_roll: index_mcp_pitch + measurements: + thumb_cmc_pitch: + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_cmc + thumb_cmc_roll: + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_cmc + thumb_mcp: + kind: relative_rotation + view: front + parent_role: thumb_cmc + child_role: thumb_mcp + thumb_dip: + kind: relative_rotation + view: front + parent_role: thumb_mcp + child_role: thumb_dip + pose_axis_line_required: false + thumb_cmc_yaw: + kind: relative_rotation + view: top + parent_role: top_base + child_role: thumb_yaw + pinky_mcp_pitch: + kind: relative_rotation + view: side + parent_role: side_base + child_role: pinky_mcp + pinky_pip: + kind: relative_rotation + view: side + parent_role: pinky_mcp + child_role: pinky_pip + pose_axis_line_required: false + pinky_dip: + kind: relative_rotation + view: side + parent_role: pinky_pip + child_role: pinky_dip + pose_axis_line_required: false + middle_mcp_roll: + kind: relative_rotation + view: front + parent_role: front_base + child_role: middle_roll + middle_mcp_pitch: + kind: relative_rotation + view: side + parent_role: side_base + child_role: middle_pip + middle_pip: + kind: relative_rotation + view: side + parent_role: side_base + child_role: middle_pip + middle_dip: + kind: relative_rotation + view: side + parent_role: middle_pip + child_role: middle_dip + pose_axis_line_required: false + index_mcp_roll: + kind: relative_rotation + view: front + parent_role: front_base + child_role: index_roll + index_mcp_pitch: + kind: relative_rotation + view: side + parent_role: side_base + child_role: index_pip + index_pip: + kind: relative_rotation + view: side + parent_role: side_base + child_role: index_pip + index_dip: + kind: relative_rotation + view: side + parent_role: index_pip + child_role: index_dip + pose_axis_line_required: false +zero: + cad_zero_assumptions: + thumb_mcp: User accepts original CAD zero at SDK baseline; absolute zero is assumed, motion is measured. + pinky_pip: User accepts original CAD zero at SDK baseline; absolute zero is assumed, motion is measured. + ring_pip: User accepts recipient CAD zero at SDK baseline; motion is copied from pinky_pip, not independently measured. + index_dip: User accepts original CAD joint zero as the physical baseline; not independently + measured. + middle_dip: User accepts original CAD joint zero as the physical baseline; not independently + measured. + pinky_dip: User accepts original CAD joint zero as the physical baseline; not independently + measured. + ring_dip: User accepts original CAD joint zero as the physical baseline; not independently + measured. + thumb_dip: User accepts original CAD joint zero as the physical baseline; not independently + measured. + active_joints: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - ring_mcp_pitch + - pinky_mcp_pitch + passive_joints: + - thumb_dip + - index_dip + - middle_dip + - ring_pip + - ring_dip + - pinky_pip + - pinky_dip + direct_zero_joints: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - pinky_mcp_pitch + axis_joints: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - pinky_mcp_pitch + - index_dip + - middle_dip + - pinky_pip + mechanical_endpoint_joints: [] + post_solve_endpoint_joints: [] + mimic_source_by_joint: + thumb_dip: thumb_mcp + index_dip: index_pip + middle_dip: middle_pip + ring_pip: ring_mcp_pitch + ring_dip: ring_pip + pinky_pip: pinky_mcp_pitch + pinky_dip: pinky_pip + cad_frozen_joints: + - thumb_mcp + - thumb_dip + - index_dip + - middle_dip + - ring_pip + - ring_dip + - pinky_pip + - pinky_dip + fitted_mimic_joints: + - thumb_dip + - index_dip + - middle_dip + - pinky_pip + - pinky_dip + coupling_model_by_joint: + thumb_dip: linear_mimic + index_dip: linear_mimic + middle_dip: linear_mimic + pinky_pip: linear_mimic + pinky_dip: linear_mimic + transferred_zero_sources: + ring_mcp_pitch: pinky_mcp_pitch + spatial: + base_pose_strategy: thumb_serial + root_anchor_joints: + - thumb_cmc_roll + orientation_anchor_joint: pinky_mcp_pitch + directed_base_axis_joints: + - thumb_cmc_roll + - pinky_mcp_pitch + depth_free_axis_projection: true + accept_validated_zero_in_confidence_interval: true + axis_order: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - pinky_mcp_pitch + - pinky_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - middle_dip + - index_mcp_roll + - index_mcp_pitch + - index_pip + - index_dip + axis_parent_joint: + thumb_cmc_yaw: thumb_cmc_roll + thumb_cmc_pitch: thumb_cmc_yaw + index_mcp_pitch: index_mcp_roll + middle_mcp_pitch: middle_mcp_roll + phase_parent_joint: + thumb_mcp: thumb_cmc_pitch + index_pip: index_mcp_pitch + index_dip: index_pip + middle_pip: middle_mcp_pitch + middle_dip: middle_pip + pinky_pip: pinky_mcp_pitch + offset_observer_joint: + thumb_cmc_roll: thumb_cmc_yaw + thumb_cmc_yaw: thumb_cmc_pitch + thumb_cmc_pitch: thumb_mcp + index_mcp_roll: index_mcp_pitch + index_mcp_pitch: index_pip + index_pip: index_dip + middle_mcp_roll: middle_mcp_pitch + middle_mcp_pitch: middle_pip + middle_pip: middle_dip + pinky_mcp_pitch: pinky_pip +quality: + training_policy: raw_feedback_3_plus_1 + training_cycles: + - 0 + - 1 + - 2 + holdout_cycle: 3 + hard_threshold_keys: + - holdout + isolated_holdout: true +scope: + default_scope: full + calibrate_joints: + full: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - ring_mcp_pitch + - pinky_mcp_pitch + frozen_joints: + full: [] +artifacts: + output_schema_version: 3 + calibration_filename: o12_right_{serial_number}_calibration.json + corrected_urdf_filename: linkerhand_o12_right_{serial_number}_zero_calibrated.urdf + protected_input_fields: + - source_urdf_sha256 + - camera_extrinsics_sha256 + - calibration_config_sha256 + - tag_config_sha256 + - sdk_config_sha256 + - sdk_package_sha256 + - profile_config_sha256 + publication_pointer: latest_passed + session_compatibility_tokens: + - o12_right_16_v1 + - feedback_rad_v1 + - full_sdk_range_v2 + publish_corrected_urdf: true +acquisition: + command_capture_mode: separate + joint_zero_timeout_seconds: 5.0 + policy_version: unified_engine_v8_all_view_images + mapping_probe_maximum_rad: 0.05235987755982989 +urdf: + authorized_fields: + thumb_cmc_roll: + - origin.rpy + - limit.lower + - limit.upper + thumb_cmc_yaw: + - origin.rpy + - limit.lower + - limit.upper + thumb_cmc_pitch: + - origin.rpy + - limit.lower + - limit.upper + thumb_mcp: + - origin.rpy + - limit.lower + - limit.upper + index_mcp_roll: + - origin.rpy + - limit.lower + - limit.upper + index_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + index_pip: + - origin.rpy + - limit.lower + - limit.upper + middle_mcp_roll: + - origin.rpy + - limit.lower + - limit.upper + middle_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + middle_pip: + - origin.rpy + - limit.lower + - limit.upper + ring_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + pinky_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + thumb_dip: + - mimic.multiplier + - mimic.offset + index_dip: + - mimic.multiplier + - mimic.offset + middle_dip: + - mimic.multiplier + - mimic.offset + pinky_pip: + - mimic.multiplier + - mimic.offset + pinky_dip: + - mimic.multiplier + - mimic.offset + ring_pip: + - mimic.multiplier + - mimic.offset + ring_dip: + - mimic.multiplier + - mimic.offset +joint_coverage: + thumb_cmc_roll: measured_static_dynamic + thumb_cmc_yaw: measured_static_dynamic + thumb_cmc_pitch: measured_static_dynamic + thumb_mcp: measured_dynamic_cad_static + thumb_dip: measured_dynamic_cad_static + index_mcp_roll: measured_static_dynamic + index_mcp_pitch: measured_static_dynamic + index_pip: measured_static_dynamic + index_dip: measured_dynamic_cad_static + middle_mcp_roll: measured_static_dynamic + middle_mcp_pitch: measured_static_dynamic + middle_pip: measured_static_dynamic + middle_dip: measured_dynamic_cad_static + ring_mcp_pitch: transferred_static_dynamic + ring_pip: mimic_nominal + ring_dip: mimic_nominal + pinky_mcp_pitch: measured_static_dynamic + pinky_pip: measured_dynamic_cad_static + pinky_dip: measured_dynamic_cad_static diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py new file mode 100644 index 0000000..b5daa82 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py @@ -0,0 +1,38 @@ +"""Legacy result types plus a direct call to the ONE profile fitter. + +Old endpoint/range-centre fallbacks and model-specific refitting are retired. +The legacy result type remains only for reading archived JSON/tool inputs. +""" +from __future__ import annotations +from dataclasses import dataclass, field +from pathlib import Path +from typing import Mapping +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.core.fitting import CouplingFit as MimicFit, fit_coupling_model, curve_travel_rad +from linkerhand_calibration.core.fitting.coupling import LinearMimicFit, RelativeMimicEvidence +from linkerhand_calibration.core.fitting.spatial import ZeroSolveResult +from linkerhand_calibration.core.fitting.session import fit_profile_calibration, compile_spatial_profile +from linkerhand_calibration.profiles import load_bundled_hand_profile + +@dataclass(frozen=True) +class O12FitResult: + curves: Mapping[str, JointCurveFit] + zero_offsets_rad: Mapping[str, float] + travels_rad: Mapping[str, float] + mimic_fits: Mapping[str, LinearMimicFit] + holdout_errors_rad: Mapping[str, tuple[float, ...]] + cross_view_roll_metrics: Mapping[str, Mapping[str, float]] + visual_arc_diagnostics_rad: Mapping[str, float] + feedback_domains_rad: Mapping[str, tuple[float, float]] + zero_method_by_joint: Mapping[str, str] + thumb_root_zero_result: ZeroSolveResult | None + full_hand_zero_result: ZeroSolveResult | None = None + standard_mimic_evidence: Mapping[str, RelativeMimicEvidence] = field(default_factory=dict) + output_mappings: Mapping[str, JointMapping] = field(default_factory=dict) + + +def fit_o12_session(source_urdf, records_by_joint, *, cross_view_records_by_joint=None, require_cross_view=False, require_thumb_root_spatial_zero=False, require_full_hand_spatial_zero=False): + # Historical flags can no longer disable spatial or holdout verification. + return fit_profile_calibration(load_bundled_hand_profile("o12_right_16"), + Path(source_urdf), records_by_joint, cross_view_records=cross_view_records_by_joint) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py new file mode 100644 index 0000000..646c740 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py @@ -0,0 +1,133 @@ +"""O12-specific interpretation of the vendor motor error report. + +The Pro2025 SDK documents ``commu_except`` as a possibly historical flag. It +must therefore be interpreted together with fresh command-triggered feedback; +the other four flags are active motor faults and are never downgraded. +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +ERROR_STALLED = 1 << 0 +ERROR_OVERHEAT = 1 << 1 +ERROR_OVER_CURRENT = 1 << 2 +ERROR_MOTOR_EXCEPTION = 1 << 3 +ERROR_COMMUNICATION_EXCEPTION = 1 << 4 +ACTIVE_MOTOR_FAULT_MASK = ( + ERROR_STALLED + | ERROR_OVERHEAT + | ERROR_OVER_CURRENT + | ERROR_MOTOR_EXCEPTION +) +DOCUMENTED_ERROR_MASK = ACTIVE_MOTOR_FAULT_MASK | ERROR_COMMUNICATION_EXCEPTION + +ERROR_BIT_NAMES = ( + "stalled", + "overheat", + "over_current", + "motor_except", + "commu_except", +) + +HISTORICAL_COMMUNICATION_MINIMUM_REPORTS = 3 +HISTORICAL_COMMUNICATION_CONFIRM_SECONDS = 1.5 +HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS = 0.25 + + +@dataclass(frozen=True) +class O12ErrorAssessment: + codes: tuple[int, ...] + valid: bool + active_fault_channels: tuple[int, ...] + communication_channels: tuple[int, ...] + unknown_bit_channels: tuple[int, ...] + + @property + def clear(self) -> bool: + return self.valid and not any(self.codes) + + @property + def communication_only(self) -> bool: + return bool( + self.valid + and self.communication_channels + and not self.active_fault_channels + and not self.unknown_bit_channels + ) + + +def assess_o12_error_report(values) -> O12ErrorAssessment: + """Decode one complete 12-motor O12 report without hiding any bit.""" + try: + codes = tuple(int(value) for value in values) + except (TypeError, ValueError): + codes = () + valid = len(codes) == 12 and all(0 <= value <= 0xFFFF for value in codes) + if not valid: + return O12ErrorAssessment(codes, False, (), (), ()) + return O12ErrorAssessment( + codes=codes, + valid=True, + active_fault_channels=tuple( + index for index, value in enumerate(codes) + if value & ACTIVE_MOTOR_FAULT_MASK + ), + communication_channels=tuple( + index for index, value in enumerate(codes) + if value & ERROR_COMMUNICATION_EXCEPTION + ), + unknown_bit_channels=tuple( + index for index, value in enumerate(codes) + if value & ~DOCUMENTED_ERROR_MASK + ), + ) + + +def decoded_faults(codes: tuple[int, ...]) -> tuple[dict[str, object], ...]: + return tuple( + { + "motor_index": index, + "code": int(value), + "flags": tuple( + name for bit, name in enumerate(ERROR_BIT_NAMES) + if int(value) & (1 << bit) + ), + } + for index, value in enumerate(codes) + if value + ) + + +def historical_communication_latch_confirmed( + *, + assessment: O12ErrorAssessment, + matching_report_count: int, + observation_seconds: float, + feedback_hz: float, + feedback_age_seconds: float, + minimum_feedback_hz: float, +) -> bool: + """Require repeated reports and a fresh full-rate control feedback stream.""" + return bool( + assessment.communication_only + and matching_report_count >= HISTORICAL_COMMUNICATION_MINIMUM_REPORTS + and observation_seconds >= HISTORICAL_COMMUNICATION_CONFIRM_SECONDS + and feedback_hz >= minimum_feedback_hz + and 0.0 <= feedback_age_seconds <= HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS + ) + + +__all__ = [ + "ACTIVE_MOTOR_FAULT_MASK", + "ERROR_BIT_NAMES", + "ERROR_COMMUNICATION_EXCEPTION", + "HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS", + "HISTORICAL_COMMUNICATION_CONFIRM_SECONDS", + "HISTORICAL_COMMUNICATION_MINIMUM_REPORTS", + "O12ErrorAssessment", + "assess_o12_error_report", + "decoded_faults", + "historical_communication_latch_confirmed", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py new file mode 100644 index 0000000..3ea8b95 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py @@ -0,0 +1,93 @@ +"""O12 vendor kinematics used by calibration and URDF publication. + +The O12 SDK exposes twelve active joint coordinates but the hand URDF has +nineteen revolute joints. The seven passive coordinates are part of the +vendor solver contract; they are not independent quantities that may be +replaced by an arbitrary linear fit of a planar AprilTag pose. + +The coefficients below are the public ``OmniHandPro2025Solver`` coefficients +from the protected SDK input. Keeping this small, deterministic evaluator in +the calibration package makes online and offline publication identical even +when the binary Python wheel is only available inside the vendor overlay. +""" + +from __future__ import annotations + +import math +from typing import Mapping, Sequence + + +# Polynomial order is constant, x, x**2, x**3, x**4. The input is the SDK +# active-joint feedback in radians. Right-thumb MCP/PIP feedback is negative, +# hence its passive DIP result is negative too; ``sdk_to_urdf_sign`` converts +# both to the positive target-URDF convention. +PASSIVE_POLYNOMIAL_BY_JOINT: Mapping[str, tuple[float, ...]] = { + "thumb_dip": (0.0, 0.6359, -0.3539, -0.3066, -0.1240), + "index_dip": (0.0, 1.0630, 0.08942, 0.1845, -0.2169), + "middle_dip": (0.0, 1.1490, -0.2581, 0.6033, -0.3371), + "ring_pip": (0.0, 0.7869, 0.3884, -0.4545, 0.1578), + "ring_dip": (0.0, 0.8990, 0.3138, -0.1728, -0.03666), + "pinky_pip": (0.0, 0.7869, 0.3884, -0.4545, 0.1578), + "pinky_dip": (0.0, 0.8990, 0.3138, -0.1728, -0.03666), +} + +# Polynomial inputs are active SDK coordinates, not necessarily the immediate +# linear URDF mimic source. In particular ring/pinky DIP is solved directly +# from the single active MCP coordinate by the vendor implementation. +PASSIVE_SDK_SOURCE_BY_JOINT: Mapping[str, str] = { + "thumb_dip": "thumb_mcp", + "index_dip": "index_pip", + "middle_dip": "middle_pip", + "ring_pip": "ring_mcp_pitch", + "ring_dip": "ring_mcp_pitch", + "pinky_pip": "pinky_mcp_pitch", + "pinky_dip": "pinky_mcp_pitch", +} + + +def evaluate_vendor_passive_joint( + joint: str, + sdk_feedback_rad: float, + *, + sdk_to_urdf_sign: float, +) -> float: + """Evaluate one passive O12 joint in the target URDF convention.""" + try: + coefficients = PASSIVE_POLYNOMIAL_BY_JOINT[str(joint)] + except KeyError as error: + raise ValueError(f"unknown O12 passive joint: {joint}") from error + x = float(sdk_feedback_rad) + sign = float(sdk_to_urdf_sign) + if not math.isfinite(x) or sign not in {-1.0, 1.0}: + raise ValueError("O12 vendor passive input/sign is invalid") + value = 0.0 + power = 1.0 + for coefficient in coefficients: + value += float(coefficient) * power + power *= x + result = sign * value + if not math.isfinite(result): + raise ValueError(f"O12 vendor passive result is invalid: {joint}") + return result + + +def vendor_passive_curve( + joint: str, + sdk_feedback_rad: Sequence[float], + *, + sdk_to_urdf_sign: float, +) -> tuple[float, ...]: + return tuple( + evaluate_vendor_passive_joint( + joint, value, sdk_to_urdf_sign=sdk_to_urdf_sign + ) + for value in sdk_feedback_rad + ) + + +__all__ = [ + "PASSIVE_POLYNOMIAL_BY_JOINT", + "PASSIVE_SDK_SOURCE_BY_JOINT", + "evaluate_vendor_passive_joint", + "vendor_passive_curve", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py new file mode 100644 index 0000000..270e07a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py @@ -0,0 +1,64 @@ +"""Physical-radian motion helpers for O12 right calibration.""" + +from __future__ import annotations + +from typing import Sequence + +from linkerhand_calibration.core import CalibrationProfile, TaskSpec + + +from linkerhand_calibration.runtime.trajectory import cosine_position_trajectory, cosine_ramp_velocity_trajectory + + +def cosine_position_trajectory_rad(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s): + return cosine_position_trajectory(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s) + + +def cosine_ramp_velocity_trajectory_rad(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s, ramp_seconds): + return cosine_ramp_velocity_trajectory(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s, ramp_seconds) + + +def build_calibration_motion_command( + task: TaskSpec, + command_rad: float, + *, + profile: CalibrationProfile, +) -> list[float]: + values = list(profile.command.baseline_values) + for index, value in task.auxiliary_commands: + values[int(index)] = float(value) + values[int(task.command_index)] = float(command_rad) + return values + + +def build_calibration_preparation_waypoints( + task: TaskSpec, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, +) -> tuple[tuple[float, ...], ...]: + del current_command + return (tuple(build_calibration_motion_command(task, task.start_value, profile=profile)),) + + +def build_calibration_return_waypoints( + target_command: Sequence[float] | None = None, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, + **_: object, +) -> tuple[tuple[float, ...], ...]: + del current_command + target = tuple( + profile.command.baseline_values if target_command is None else target_command + ) + if len(target) != profile.command.command_count: + raise ValueError("O12 return command has the wrong channel count") + return (tuple(float(value) for value in target),) + + +__all__ = [ + "build_calibration_motion_command", "build_calibration_preparation_waypoints", + "build_calibration_return_waypoints", "cosine_position_trajectory_rad", + "cosine_ramp_velocity_trajectory_rad", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/o12_bridge.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/o12_bridge.py new file mode 100644 index 0000000..3f0b84f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/o12_bridge.py @@ -0,0 +1,118 @@ +"""Thin HCAN transport: independent readback, no calibration business logic. + +Only this process opens HCAN. Before the first explicit position message it +calls read methods only; it never moves to a presumed zero to obtain feedback. +""" + +import hashlib +import json +import math +from pathlib import Path +import time + + +ERROR_FIELDS = ("stalled", "overheat", "over_current", "motor_except", "commu_except") + + +def read_health(hand): + modes = tuple(int(v) for v in hand.get_all_control_modes()) + errors = tuple(hand.get_all_error_reports()) + if len(modes) != 12 or len(errors) != 12: + raise ValueError("SDK returned an incomplete health report") + codes = tuple(sum(1 << i for i, field in enumerate(ERROR_FIELDS) if bool(getattr(report, field))) + for report in errors) + return {"position_mode": all(mode == 0 for mode in modes), "modes": modes, + "error_codes": codes, "active_faults": [f"channel={i}:{ERROR_FIELDS[bit]}" + for i, code in enumerate(codes) for bit in range(4) if code & (1 << bit)], + "communication_latches": [i for i, code in enumerate(codes) if code & 16], + "temperature_policy": "vendor_error_bit1_overheat"} + + +def main(args=None): + import rclpy + from rclpy.node import Node + from sensor_msgs.msg import JointState + from std_msgs.msg import String + import yaml + from linkerhand_calibration.runtime.adapters.vendor_package import load_vendor_sdk + + class Bridge(Node): + def __init__(self): + super().__init__("o12_sdk_bridge") + for name, default in {"vendor_config": "", "vendor_config_sha256": "", + "vendor_python_package": "", "vendor_package_sha256": "", "hand_type": "right", + "topic_prefix": "/o12/right", "readback_hz": 50.0}.items(): + self.declare_parameter(name, default) + value = lambda name: self.get_parameter(name).value + config = Path(value("vendor_config")) + if hashlib.sha256(config.read_bytes()).hexdigest() != value("vendor_config_sha256"): + raise ValueError("vendor connection configuration hash mismatch") + sdk = load_vendor_sdk(value("vendor_python_package"), value("vendor_package_sha256")) + side = str(value("hand_type")) + if side not in {"left", "right"}: + raise ValueError("invalid SDK hand side") + connection = yaml.safe_load(config.read_text())["/**"]["ros__parameters"][side+"_hand"] + if connection["connection_type"] != "hcan": + raise ValueError("HCAN adapter received a different transport") + self.hand = sdk.OmniHandPro2025.create_hand_by_hcan( + hand_type=sdk.HandType.LEFT if side == "left" else sdk.HandType.RIGHT, + hand_device_id=int(connection["hand_device_id"]), + canfd_device_id=int(connection["canfd_device_id"]), + canfd_channel_id=int(connection["canfd_channel_id"])) + self.hand.set_request_interval(int(connection.get("request_interval_ms", 0))) + self.hand.set_frame_recv_timeout(int(connection.get("frame_recv_timeout_ms", 100))) + if not self.hand.init(): + raise RuntimeError("HCAN initialization failed") + prefix = str(value("topic_prefix")).rstrip("/") + self.state_pub = self.create_publisher(JointState, prefix+"/joint_states", 10) + self.health_pub = self.create_publisher(String, prefix+"/calibration_health", 10) + self.create_subscription(JointState, prefix+"/joint_cmd", self.command, 1) + self.pending, self.pending_at, self.health_at = None, 0.0, 0.0 + self.health = None + self.invalid_health_since = None + self.create_timer(1.0/float(value("readback_hz")), self.tick) + + def command(self, message): + values = tuple(float(v) for v in message.position) + if len(values) != 12 or not all(math.isfinite(v) for v in values): + self.get_logger().error("rejected malformed SDK position vector") + return + self.pending, self.pending_at = values, time.monotonic() + + def tick(self): + now = time.monotonic() + if now-self.health_at >= .5: + try: + self.health = read_health(self.hand) + self.invalid_health_since = None + except (ValueError, TypeError, AttributeError) as error: + if self.invalid_health_since is None: + self.invalid_health_since = now + if now-self.invalid_health_since >= 1.0: + self.health = {"position_mode": False, "active_faults": ["health_report_unparseable"], "diagnostic": str(error)} + self.health_at = now + if self.health is not None: + report = String() + report.data = json.dumps(self.health) + self.health_pub.publish(report) + if (self.pending is not None and now-self.pending_at <= 1.0 and self.health is not None + and self.health["position_mode"] and not self.health["active_faults"]): + self.hand.set_all_active_joint_angles(list(self.pending)) + self.pending = None + values = tuple(float(v) for v in self.hand.get_all_active_joint_angles()) + if len(values) == 12 and all(math.isfinite(v) for v in values): + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + # Product-fixed order, intentionally no SDK-generated names. + message.position = list(values) + self.state_pub.publish(message) + + rclpy.init(args=args) + node = None + try: + node = Bridge() + rclpy.spin(node) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/o12_hcan_sdk.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/o12_hcan_sdk.py new file mode 100644 index 0000000..bb94434 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/o12_hcan_sdk.py @@ -0,0 +1,90 @@ +"""Physical-angle HCAN adapter used by O12 without leaking into the engine.""" + +from __future__ import annotations + +from typing import Callable, Sequence +import math +import time + +from linkerhand_calibration.core import CommandLayout +from linkerhand_calibration.runtime.adapters.base import HardwareHealth, ProfileSdkAdapter + + +class O12HcanSdkAdapter(ProfileSdkAdapter): + def __init__( + self, + command_layout: CommandLayout, + *, + publish: Callable[[tuple[float, ...]], None], + set_speed_callback: Callable[[int, float], None], + health_callback: Callable[[], HardwareHealth], + clock: Callable[[], float] = time.monotonic, + health_timeout_seconds: float = 1.0, + ) -> None: + if command_layout.unit != "rad": + raise ValueError("O12 HCAN adapter requires a radian command layout") + if not command_layout.feedback_by_index: + raise ValueError("O12 feedback must use the fixed SDK array order") + super().__init__(command_layout) + self._publish = publish + self._set_speed = set_speed_callback + self._health = health_callback + if not math.isfinite(health_timeout_seconds) or health_timeout_seconds <= 0: + raise ValueError("health timeout must be finite and positive") + self._clock = clock + self._health_timeout_seconds = health_timeout_seconds + self._invalid_health_since: float | None = None + self._last_health = HardwareHealth(False, False, diagnostic="waiting") + + def publish_position(self, values: Sequence[float]) -> None: + self._publish(self.validate_command(values)) + + def initial_command(self, feedback): + # Vendor get/set_all_active_joint_angles use the same nominal radian + # coordinates. This initializes a smooth trajectory AFTER Start; + # it is not a baseline command sent during preview, nor a calibration + # correction inferred from command/feedback error. + layout = self.command_layout + if len(feedback) != layout.command_count or any(not math.isfinite(v) or not lo <= v <= hi + for v, lo, hi in zip(feedback, layout.minimum_feedback_values, layout.maximum_feedback_values)): + raise ValueError("cannot initialize motion from invalid physical feedback") + return self.validate_command(tuple(min(hi, max(lo, value)) + for value, lo, hi in zip(feedback, layout.minimum_values, layout.maximum_values))) + + def set_speed(self, command_index: int, speed: float) -> None: + index = int(command_index) + if not 0 <= index < self.command_layout.command_count: + raise ValueError("speed command index is out of range") + if not math.isfinite(float(speed)) or float(speed) <= 0.0: + raise ValueError("physical speed must be positive") + self._set_speed(index, float(speed)) + + def health(self) -> HardwareHealth: + """Retain good health through transient parse errors, using elapsed time. + + Three malformed packets in a single callback burst are not evidence of + a persistent fault. A valid active fault is still returned immediately. + """ + try: + health = self._health() + if not isinstance(health, HardwareHealth): + raise TypeError("SDK health callback returned an invalid report") + except (TypeError, ValueError, IndexError) as error: + now = self._clock() + if self._invalid_health_since is None: + self._invalid_health_since = now + if now - self._invalid_health_since < self._health_timeout_seconds: + return self._last_health + return HardwareHealth( + connected=self._last_health.connected, + position_mode=self._last_health.position_mode, + active_faults=("health_report_unparseable",), + feedback_fresh=self._last_health.feedback_fresh, + diagnostic=str(error), + ) + self._invalid_health_since = None + self._last_health = health + return health + + +__all__ = ["O12HcanSdkAdapter"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py new file mode 100644 index 0000000..b534bef --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py @@ -0,0 +1,17 @@ +"""Legacy offline name for the generic articulated candidate primitive.""" + +from linkerhand_calibration.core.geometry.candidate_selection import ( + MATRIX_SOURCE, POLICY, _relative, _rotation, resolve_chain_observations, +) +from linkerhand_calibration.core.geometry.pnp import solve_square_tag_ippe +from linkerhand_calibration.models.o12.pnp import THUMB_ROLES +from linkerhand_calibration.models.o12.profile import build_typed_profile + + +def resolve_thumb_observations(records, *, projection_override=None): + profile = build_typed_profile() + task = next(task for task in profile.motion.tasks if task.key == 'thumb_mcp_dip_front') + return resolve_chain_observations(records, task_key=task.key, + role_pairs={name: (profile.measurement.measurements[name].parent_role, + profile.measurement.measurements[name].child_role) for name in task.joints}, + projection_override=projection_override, solve_pose=solve_square_tag_ippe) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py new file mode 100644 index 0000000..93360cb --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py @@ -0,0 +1,31 @@ +"""Old O12 journal names at the common finalization boundary. + +All fitting, quality, standard-URDF generation and publication live in the +common finalizer. This adapter only translates old capture field names and +task-relative direction labels, without changing numerical observations. +""" + +from linkerhand_calibration.runtime.acquisition import accepted_joint_records, accepted_secondary_records, load_capture +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session +from linkerhand_calibration.models.o12.profile import build_typed_profile + + +def _journal_rows(records): + return [dict(row, observation_joint=row["model_joint"]) + if row.get("kind") == "o12_roll_cross_view_sample" else dict(row) for row in records] + + +def accepted_records_by_joint(records): + return accepted_joint_records(build_typed_profile(), records) + + +def accepted_roll_cross_view_records(records): + return accepted_secondary_records(build_typed_profile(), _journal_rows(records)) + + +load_o12_raw_samples = load_capture + + +def finalize_o12_session(*, records, **kwargs): + return finalize_profile_session(profile=build_typed_profile(), + records=_journal_rows(records), directions_are_task_relative=True, **kwargs) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py new file mode 100644 index 0000000..dadcdc8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py @@ -0,0 +1,76 @@ +"""O12 thumb-chain branch selection; never alters a measured pose or angle.""" +import math +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.pnp import SquareTagGroupPoseTracker + + +THUMB_ROLES = ('thumb_cmc', 'thumb_mcp', 'thumb_dip') + + +class O12ThumbPoseTracker(SquareTagGroupPoseTracker): + """Use parallel hinge directions, not a CAD zero or passive angle ratio. + + Both relative rotations live in different marker frames. Transport the + follower's rotation vector through the *reference* parent rotation before + comparing axes. Fixed arbitrary marker mounting rotations cancel out. + This is a soft ambiguity discriminator, not a new capture/stop gate. + """ + + def __init__(self): + super().__init__( + roles=THUMB_ROLES, + adjacent_pairs=(THUMB_ROLES[:2], THUMB_ROLES[1:]), + maximum_pose_jump_rad=math.pi, + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=math.radians(5), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=5.0, + initialization_frames=8, + ) + self.reference = None + + def reset(self, *, preserve_task_reference=False): + super().reset(preserve_task_reference=preserve_task_reference) + self.reference = None + + @staticmethod + def relative(poses): + rotations = [Rotation.from_quat(poses[r].quaternion_xyzw) for r in THUMB_ROLES] + return rotations[0].inv()*rotations[1], rotations[1].inv()*rotations[2] + + def _informative_coupled_rotation_costs(self, combinations): + # Reuse the shared group selector's soft-cost extension point without + # installing coupled_rotation_pairs (there is NO angle-ratio prior). + if self.reference is None: + return tuple(0.0 for _ in combinations) + driver_ref, follower_ref = self.reference + residuals = [] + for poses in combinations: + driver, follower = self.relative(poses) + a = (driver*driver_ref.inv()).as_rotvec() + b = driver_ref.apply((follower*follower_ref.inv()).as_rotvec()) + if min(np.linalg.norm(a), np.linalg.norm(b)) < math.radians(5): + residuals.append(None) + continue + axis = a/np.linalg.norm(a) + residuals.append(float(np.linalg.norm(b-axis*np.dot(axis,b)))) + # Insufficient excitation or no geometrically credible candidate: + # keep ordinary visual continuity, rather than inventing a pose or + # rejecting all observations because a prior did not match. + if not any(r is not None and r < math.radians(10) for r in residuals): + return tuple(0.0 for _ in combinations) + return tuple(0.0 if r is None else r/math.radians(3) for r in residuals) + + def select(self, candidates_by_role, *, stamp_ns, **kwargs): + # O12 radians never enter the shared tracker's u8 return-path cache. + if (self._previous_stamp_ns is not None and + not 0 <= stamp_ns-self._previous_stamp_ns <= self.reset_after_ns): + self.reset() + selected, reason = super().select(candidates_by_role, stamp_ns=stamp_ns) + if selected is not None and self.reference is None: + self.reference = self.relative(selected) + return selected, reason diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py new file mode 100644 index 0000000..e5bc8fe --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py @@ -0,0 +1,285 @@ +"""Reviewed O12 right-hand 16-Tag calibration profile.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math + +from linkerhand_calibration.core import CalibrationProfile, ProfileKey +from linkerhand_calibration.models.registry import EngineBindings, RegisteredProfile +from linkerhand_calibration.models.o12.motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) + + +KEY = ProfileKey("O12", "right", "o12_right_16", 1) + +# sensor_msgs/JointState.position active-angle order from API_CPP_O12.md. +COMMAND_NAMES: tuple[str, ...] = ( + "thumb_roll", + "thumb_abad", + "thumb_mcp", + "thumb_pip", + "index_abad", + "index_mcp", + "index_pip", + "middle_abad", + "middle_mcp", + "middle_pip", + "ring_mcp", + "pinky_mcp", +) + +SDK_LOWER_RAD = ( + 0.0, -1.387536755335492, -0.8272860654453121, -1.2915436464758039, + -0.2617993877991494, 0.0, 0.0, -0.2617993877991494, + 0.0, 0.0, 0.0, 0.0, +) +SDK_UPPER_RAD = ( + 0.9424777960769379, 0.0, 0.0, 0.0, + 0.2617993877991494, 1.3526301702956054, 1.530653753999027, + 0.2617993877991494, 1.3578661580515883, 1.8151424220741028, + 1.53588974175501, 1.53588974175501, +) + +# These legacy constants describe SDK coordinates, not measured CAD angles. +# The YAML owns the task domain. Measured output angles must independently +# satisfy the source CAD physical range; a larger SDK value never authorizes +# expanding URDF limits. Remove these aliases with the remaining old runtime. +SAFE_LOWER_RAD = SDK_LOWER_RAD +SAFE_UPPER_RAD = SDK_UPPER_RAD + +# Joint-state feedback can cross a commanded endpoint slightly because it is +# measured after servo tracking and encoder conversion. The envelope remains +# anchored to the reviewed SDK hard range, with an explicit two-degree +# observation allowance that is never used to generate a command. +FEEDBACK_DOMAIN_MARGIN_RAD = math.radians(2.0) +FEEDBACK_LOWER_RAD = tuple( + value - FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_LOWER_RAD +) +FEEDBACK_UPPER_RAD = tuple( + value + FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_UPPER_RAD +) + +SDK_TO_URDF_JOINT: tuple[str, ...] = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", + "middle_mcp_roll", + "middle_mcp_pitch", + "middle_pip", + "ring_mcp_pitch", + "pinky_mcp_pitch", +) + +# Fixed direction contract between vendor channels and the target CAD axes. +# The magnitude remains visually calibrated because O12's 12 active SDK +# coordinates include tendon/solver coordinates that are not one-to-one with +# all 19 physical URDF joints. +SDK_TO_URDF_SIGN: tuple[float, ...] = ( + 1.0, -1.0, -1.0, -1.0, + -1.0, 1.0, 1.0, -1.0, + 1.0, 1.0, 1.0, 1.0, +) + +ACTIVE_JOINTS = SDK_TO_URDF_JOINT +PASSIVE_JOINTS: tuple[str, ...] = ( + "thumb_dip", + "index_dip", + "middle_dip", + "ring_pip", + "ring_dip", + "pinky_pip", + "pinky_dip", +) +CALIBRATED_ACTIVE_JOINTS = frozenset(ACTIVE_JOINTS) - {"ring_mcp_pitch"} +MEASURED_PASSIVE_JOINTS = frozenset( + {"thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip"} +) +TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = { + "ring_mcp_pitch": "pinky_mcp_pitch", +} +MIMIC_SOURCE_BY_JOINT = { + "thumb_dip": "thumb_mcp", + "index_dip": "index_pip", + "middle_dip": "middle_pip", + "ring_pip": "ring_mcp_pitch", + "ring_dip": "ring_pip", + "pinky_pip": "pinky_mcp_pitch", + "pinky_dip": "pinky_pip", +} + +# SDK endpoints measure travel; they have not been surveyed as CAD contact +# datums. In particular CAD.upper - travel is NOT an encoder zero. The root +# thumb axes observe roll/yaw phase; parallel flexion zeros additionally need +# adjacent axis-line observations. The one declared exclusion below remains +# CAD-owned; all other observable active zeros are required for publication. +ROOT_GEOMETRIC_ZERO_JOINTS = frozenset({"thumb_cmc_roll", "thumb_cmc_yaw"}) +# ID2/ID3 are still used to fit and independently validate thumb MCP/DIP +# motion, but the curved-shell ID3 observation is not a reliable absolute +# axis-line datum for the thumb MCP assembly phase. Keep that one static +# origin at immutable source CAD until a rigid downstream fiducial is +# available. This is an explicit O12 profile contract, not an exception-path +# quality bypass; every other observable active zero remains mandatory. +STATIC_ZERO_EXCLUDED_JOINTS = frozenset({"thumb_mcp"}) +GEOMETRIC_ZERO_JOINTS = frozenset( + CALIBRATED_ACTIVE_JOINTS - STATIC_ZERO_EXCLUDED_JOINTS +) +ENDPOINT_ANCHOR_BY_JOINT: dict[str, str] = {} +COMMAND_INDEX_BY_JOINT = { + joint: index for index, joint in enumerate(SDK_TO_URDF_JOINT) +} + +# The outer pair is used only as collision clearance after the pinky's own +# full-range scan has already exercised the same safe endpoint. O12 has only +# one active coordinate on ring/pinky, so partial MCP flexion leaves both long +# passive chains inside the middle/index camera corridor. Park them at the +# reviewed CAD/SDK intersection instead of the former 65% pose. +OUTER_CLEARANCE_FRACTION = 1.0 +PARK_RING_MCP_RAD = OUTER_CLEARANCE_FRACTION * SAFE_UPPER_RAD[10] +PARK_PINKY_MCP_RAD = OUTER_CLEARANCE_FRACTION * SAFE_UPPER_RAD[11] +# Compatibility name for downstream diagnostics; motion code uses the two +# channel-specific values above and never takes their minimum. +PARK_FINGER_RAD = PARK_PINKY_MCP_RAD +PARK_MIDDLE_MCP_RAD = SAFE_UPPER_RAD[8] +PARK_MIDDLE_PIP_RAD = SAFE_UPPER_RAD[9] +# Feedback radians are the quantity being calibrated and need not numerically +# equal the command endpoint. Once a finger has been scanned, clearance uses +# that measured feedback travel as its endpoint reference. Simultaneous MCP +# and PIP flexion may shorten the vendor solver coordinate slightly. The +# unified waypoint rule accepts 80% measured travel after the command +# trajectory ends and feedback settles; +# exact command equality is intentionally not required for tendon mechanisms. +CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION = 0.80 +INDEX_CLEARANCE_RAD = -math.radians(10.0) +# O12's index ABAD/MCP and middle ABAD/MCP are coupled tendon coordinates. +# The vendor solver clamps MCP upward when a large ABAD command is paired +# with MCP=0. These reviewed values keep the requested pose inside the +# solver's feasible set instead of relying on an invisible SDK correction: +# - index ABAD -10 deg requires MCP >= 0.109846 rad; +# - either ABAD roll endpoint (+/-0.26 rad) requires MCP >= 0.163636 rad. +INDEX_CLEARANCE_MCP_RAD = 0.11 +ROLL_CLEARANCE_MCP_RAD = 0.17 +# O12 feedback is continuous radians. Use 64 possible normalized cells and +# require at least 32 of them; using exactly 32 possible cells accidentally +# required both mechanical endpoints despite the separate 90% span gate. +NORMALIZED_SWEEP_BIN_COUNT = 64 +# A feedback curve is the quantity being identified, so its unknown endpoint +# scale must never be judged against the command endpoint as if they were the +# same coordinate. Cycle 0 is normalized over its own measured endpoints and +# therefore establishes a unit-span physical reference. Later cycles must +# reproduce at least 90% of that measured full-stroke reference. +INITIAL_FEEDBACK_SPAN_FRACTION = 0.99 +EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION = 0.90 +# Both FRONT/TOP and FRONT/SIDE are close to orthogonal on the reviewed O12 +# rig. Their common planar checkerboard must be oblique in at least one view, +# so the G20 1.2 px (and O6 1.5 px) batch gates reject highly repeatable O12 +# solutions for image-geometry reasons. At the measured 3700--3800 px focal +# length, 2.0 px is about 0.030 degrees. Treat RMS as a gross-error gate and +# keep the independent 0.3 degree and 1.5 mm pose-repeatability gates as the +# final geometric consistency evidence. +MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX = 2.0 + + +@dataclass(frozen=True) +class RollCrossViewObserver: + """O12-only side-camera contract for one front-camera roll sweep.""" + + task_name: str + view: str + parent_role: str + child_role: str + source_joint: str + model_joint: str + coupled_channel_index: int + maximum_coupled_motion_rad: float + + +# The downstream PIP Tag is rigidly carried by MCP roll, so it supplies an +# independent side-view roll curve while the front Tag supplies the published +# curve. O12's tendon solver also has a small, deterministic PIP back-drive +# around centred ABAD: joint->actuator->joint reaches about 0.0124 rad even +# when PIP is commanded to zero. The reviewed 0.020 rad envelope includes +# encoder quantisation and applies only to roll-task cross-view integrity. +ROLL_CROSS_VIEW_OBSERVERS: tuple[RollCrossViewObserver, ...] = ( + RollCrossViewObserver( + "middle_roll_front", "side", "side_base", "middle_pip", + "middle_mcp_roll_side", "middle_mcp_roll", 9, 0.020, + ), + RollCrossViewObserver( + "index_roll_front", "side", "side_base", "index_pip", + "index_mcp_roll_side", "index_mcp_roll", 6, 0.020, + ), +) +ROLL_CROSS_VIEW_BY_TASK = { + observer.task_name: observer for observer in ROLL_CROSS_VIEW_OBSERVERS +} + +# Fast-calibration caps by fixed SDK channel. The thumb CMC pitch cap keeps +# explicit margin below its unusually low 0.11 rad/s source-URDF limit; the +# remaining caps are deliberately far below their corresponding CAD limits +# while keeping one visual sweep long enough for dense 30 Hz observations. +FORMAL_SPEED_CAP_RAD_S: tuple[float, ...] = ( + 0.16, 0.16, 0.10, 0.32, + # Index/middle ABAD at 0.16 rad/s repeatedly returned only 89.2--89.5% + # feedback coverage; 0.12 rad/s measured 91.2% on the physical hand. + 0.12, 0.32, 0.32, 0.12, + 0.32, 0.32, 0.32, 0.32, +) + + +def build_typed_profile() -> CalibrationProfile: + """Compatibility entry; the complete typed contract lives only in YAML.""" + from linkerhand_calibration.profiles.loader import load_bundled_hand_profile + return load_bundled_hand_profile("o12_right_16") + + +def _run_cli(args: list[str] | None = None) -> None: + from linkerhand_calibration.models.o12.runner import main + main(args) + + +def _run_node(args: list[str] | None = None) -> None: + from linkerhand_calibration.runtime.ros.calibration_node import run_profile_node + run_profile_node(build_typed_profile(), args) + + +def build_profile() -> RegisteredProfile: + typed = build_typed_profile() + return RegisteredProfile( + typed, + 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=_run_cli, + node_main=_run_node, + ), + ) + + +__all__ = [ + "ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT", + "COMMAND_NAMES", + "GEOMETRIC_ZERO_JOINTS", "INDEX_CLEARANCE_RAD", "KEY", + "MEASURED_PASSIVE_JOINTS", + "EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION", + "INITIAL_FEEDBACK_SPAN_FRACTION", "MIMIC_SOURCE_BY_JOINT", + "NORMALIZED_SWEEP_BIN_COUNT", "OUTER_CLEARANCE_FRACTION", + "PARK_FINGER_RAD", "PARK_MIDDLE_MCP_RAD", "PARK_PINKY_MCP_RAD", + "PARK_RING_MCP_RAD", "CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION", + "PARK_MIDDLE_PIP_RAD", "PASSIVE_JOINTS", "SAFE_LOWER_RAD", "SAFE_UPPER_RAD", + "ROLL_CROSS_VIEW_BY_TASK", "ROLL_CROSS_VIEW_OBSERVERS", + "RollCrossViewObserver", + "SDK_LOWER_RAD", "SDK_TO_URDF_JOINT", "SDK_UPPER_RAD", + "STATIC_ZERO_EXCLUDED_JOINTS", + "TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py new file mode 100644 index 0000000..07ed869 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py @@ -0,0 +1,75 @@ +"""O12 schema-v7 compatibility facade for the common data gate. + +New runtime code calls :class:`runtime.engine.CalibrationEngine` directly. +The legacy function remains because deployed schema-v7 diagnostics and tests +import it; it delegates to the same model-independent evaluator. +""" + +from __future__ import annotations + +from typing import Any, Mapping, Sequence + +from linkerhand_calibration.runtime.engine import evaluate_observability + +QUALITY_POLICY_VERSION = 5 +MAXIMUM_UNOBSERVED_BIN_FRACTION = 1.0 / 16.0 + + +def evaluate_o12_observation_quality( + normalized_feedback: Sequence[float], + observation: Mapping[str, Any], + *, + normalized_bin_count: int, + required_feedback_span: float, + minimum_sweep_frames: int, + minimum_sweep_bins: int, + minimum_joint_frame_rate: float, + minimum_feedback_hz: float, + feedback_hz: float, + target_detection_rate: float, + target_maximum_bin_gap: int, +) -> dict[str, Any]: + """Return schema-v7 fields derived from the common acceptance policy.""" + del minimum_joint_frame_rate, minimum_feedback_hz + quality = evaluate_observability( + normalized_feedback, + minimum_span=required_feedback_span, + minimum_valid_samples=minimum_sweep_frames, + minimum_bins=minimum_sweep_bins, + maximum_unobserved_fraction=MAXIMUM_UNOBSERVED_BIN_FRACTION, + total_frames=int(observation.get("total_frames", 0)), + joint_frame_rate=float(observation.get("joint_frame_rate", 0.0)), + feedback_hz=feedback_hz, + detection_rate=float(observation.get("tag_detection_rate", 0.0)), + bin_count=normalized_bin_count, + include_endpoint_gaps=False, + ) + warnings = list(quality.warnings) + maximum_gap = int(quality.metrics["maximum_bin_gap"]) + detection_rate = float(observation.get("tag_detection_rate", 0.0)) + if detection_rate < float(target_detection_rate): + role_rates = observation.get("tag_detection_rate_by_role", {}) + worst_role = min(role_rates, key=role_rates.get, default="unknown") + target_warning = f"tag_rate_target[{worst_role}]={detection_rate:.3f}" + if target_warning not in warnings: + warnings.append(target_warning) + if maximum_gap > int(target_maximum_bin_gap): + warnings.append(f"maximum_gap_target={maximum_gap}") + return { + "quality_policy_version": QUALITY_POLICY_VERSION, + "decision_basis": "normalized_fit_observability", + **dict(quality.metrics), + "required_feedback_span": round(float(required_feedback_span), 9), + "maximum_unobserved_bin_fraction": MAXIMUM_UNOBSERVED_BIN_FRACTION, + "target_detection_rate": float(target_detection_rate), + "target_maximum_bin_gap": int(target_maximum_bin_gap), + "warnings": warnings, + "failures": list(quality.failures), + "passed": quality.passed, + } + +__all__ = [ + "MAXIMUM_UNOBSERVED_BIN_FRACTION", + "QUALITY_POLICY_VERSION", + "evaluate_o12_observation_quality", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py new file mode 100644 index 0000000..ab09480 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py @@ -0,0 +1,5 @@ +"""Legacy import path; all online work uses the common product runner.""" + +from linkerhand_calibration.runtime.runner import main +from linkerhand_calibration.runtime.runner_support import log_exception_summary as _log_exception_summary +from linkerhand_calibration.compat.legacy_diagnostic_tools.progress import render_o12_progress_zh diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py new file mode 100644 index 0000000..44cf112 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py @@ -0,0 +1,94 @@ +"""O12 result adapter for the common standard-URDF correction engine.""" + +from __future__ import annotations + +from dataclasses import dataclass +from datetime import datetime +import hashlib +import math +from pathlib import Path +import re +from typing import Mapping + +from linkerhand_calibration.core.urdf.plan import StandardUrdfPlan, build_standard_correction_plan +from linkerhand_calibration.runtime.artifacts.publisher import StagedRelease +from linkerhand_calibration.models.o12.fitting import O12FitResult +from linkerhand_calibration.models.o12.profile import ( + ACTIVE_JOINTS, CALIBRATED_ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, + MEASURED_PASSIVE_JOINTS, SDK_TO_URDF_SIGN, STATIC_ZERO_EXCLUDED_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, build_typed_profile, +) + + +def o12_active_ranges(result: O12FitResult) -> dict[str, tuple[float, float]]: + if set(result.output_mappings) != set(ACTIVE_JOINTS): + raise ValueError("O12 fit is missing frozen native output mappings") + return { + name: result.output_mappings[name].bounds_rad + for name in sorted(CALIBRATED_ACTIVE_JOINTS) + } + + +def o12_correction_plan(source_urdf: str | Path, result: O12FitResult) -> StandardUrdfPlan: + """Translate measured parameters; never invent endpoint mimics or limits.""" + if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS | set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT): + raise ValueError("O12 travel result has the wrong active joint set") + if set(result.standard_mimic_evidence) != MEASURED_PASSIVE_JOINTS: + raise ValueError("O12 standard mimic requires independent visual training/holdout evidence") + for name in STATIC_ZERO_EXCLUDED_JOINTS: + if not math.isclose(float(result.zero_offsets_rad.get(name, math.nan)), 0., abs_tol=1e-12) or result.zero_method_by_joint.get(name) != "source_cad_zero_profile_excluded": + raise ValueError(f"O12 {name} must retain immutable source-CAD static zero") + for name, evidence in result.standard_mimic_evidence.items(): + fit = evidence.fit + if fit.target_joint != name or not evidence.training_sample_ids or not evidence.holdout_sample_ids or set(evidence.training_sample_ids) & set(evidence.holdout_sample_ids): + raise ValueError(f"O12 mimic evidence is not independent: {name}") + from linkerhand_calibration.core.urdf.acceptance import angular_metrics + if not angular_metrics(evidence.holdout_errors_rad).passed: + raise ValueError(f"standard_urdf_mimic_not_expressive:{name}") + source = Path(source_urdf) + return build_standard_correction_plan(source_urdf=source, + source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), + zero_offsets_rad=result.zero_offsets_rad, + measured_ranges_output_rad=o12_active_ranges(result), + fitted_mimics_cad={name: evidence.fit for name, evidence in result.standard_mimic_evidence.items()}, + transferred_zero_sources=TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + authorized_fields=build_typed_profile().urdf_authorized_fields) + + +@dataclass(frozen=True) +class O12UrdfCorrection: + path: Path + origin_offsets_rad: Mapping[str, float] + corrected_limits_rad: Mapping[str, tuple[float, float]] + mimic_multipliers: Mapping[str, float] + preserved_ring_fields: tuple[str, ...] + staged_release: StagedRelease | None = None + + +def write_o12_corrected_urdf( + *, source_urdf: str | Path, output_directory: str | Path, + serial_number: str, result: O12FitResult, timestamp: str | None = None, +) -> O12UrdfCorrection: + """Write authorized standard coordinates using the shared patch engine.""" + source = Path(source_urdf).expanduser().resolve() + if "calibrated" in source.stem.lower(): + raise ValueError("O12 source URDF must be immutable original CAD") + plan = o12_correction_plan(source, result) + stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") + if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: + raise ValueError("O12 URDF timestamp must use YYYYMMDD_HHMMSS") + serial = "".join(c if c.isalnum() or c in "_.-" else "_" for c in str(serial_number)) + if not serial: + raise ValueError("serial number must not be empty") + destination = Path(output_directory).expanduser().resolve() / f"{source.stem}_calibrated_{serial}_{stamp}.urdf" + plan.write(source, destination) + return O12UrdfCorrection(destination, + {name: float(result.zero_offsets_rad[name]) for name in ACTIVE_JOINTS}, + plan.limits_output_rad, + {name: value[1] for name, value in plan.mimic_output.items()}, + ("ring_mcp_pitch.origin.xyz", "ring_mcp_pitch.physical_range", + "ring_pip.origin", "ring_pip.limit", "ring_pip.mimic.multiplier", + "ring_dip.origin", "ring_dip.limit", "ring_dip.mimic.multiplier")) + + +__all__ = ["O12UrdfCorrection", "o12_active_ranges", "o12_correction_plan", "write_o12_corrected_urdf"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/linkerhand_o12_t3_left_urdf-0703.urdf b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/linkerhand_o12_t3_left_urdf-0703.urdf new file mode 100644 index 0000000..bca7a82 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/linkerhand_o12_t3_left_urdf-0703.urdf @@ -0,0 +1,1231 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/base_link.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/base_link.STL new file mode 100644 index 0000000..e00e9e0 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/base_link.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_distal.STL new file mode 100644 index 0000000..110c1f8 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_metacarpals.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_metacarpals.STL new file mode 100644 index 0000000..091350d Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_metacarpals.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_middle.STL new file mode 100644 index 0000000..1c6cf46 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_proximal.STL new file mode 100644 index 0000000..b09c29e Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/index_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_distal.STL new file mode 100644 index 0000000..2759f69 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_metacarpals.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_metacarpals.STL new file mode 100644 index 0000000..7779af0 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_metacarpals.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_middle.STL new file mode 100644 index 0000000..cccbf5e Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_proximal.STL new file mode 100644 index 0000000..2398b8b Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/middle_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_distal.STL new file mode 100644 index 0000000..4c6b56c Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_middle.STL new file mode 100644 index 0000000..a7b4a5d Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_proximal.STL new file mode 100644 index 0000000..4977b8f Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_distal.STL new file mode 100644 index 0000000..f9b958b Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_middle.STL new file mode 100644 index 0000000..b58f4af Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_proximal.STL new file mode 100644 index 0000000..2647d64 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/ring_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_distal.STL new file mode 100644 index 0000000..2b53c56 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals.STL new file mode 100644 index 0000000..bd3c424 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals_base1.STL new file mode 100644 index 0000000..21bd7eb Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals_base1.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals_base2.STL new file mode 100644 index 0000000..5cd144a Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_metacarpals_base2.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_proximal.STL new file mode 100644 index 0000000..90aca3e Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/left/meshes/thumb_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/linkerhand_o12_t3_right-0703.urdf b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/linkerhand_o12_t3_right-0703.urdf new file mode 100644 index 0000000..a668f76 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/linkerhand_o12_t3_right-0703.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/base_link.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/base_link.STL new file mode 100644 index 0000000..42541fa Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/base_link.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_distal.STL new file mode 100644 index 0000000..56a9b93 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_metacarpals.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_metacarpals.STL new file mode 100644 index 0000000..5233fa7 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_metacarpals.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_middle.STL new file mode 100644 index 0000000..53a06c8 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_proximal.STL new file mode 100644 index 0000000..e35090d Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/index_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_distal.STL new file mode 100644 index 0000000..6daf9f7 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_metacarpals.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_metacarpals.STL new file mode 100644 index 0000000..2a6cfd1 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_metacarpals.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_middle.STL new file mode 100644 index 0000000..5fbae37 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_proximal.STL new file mode 100644 index 0000000..5e7d424 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/middle_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_distal.STL new file mode 100644 index 0000000..ae565d3 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_middle.STL new file mode 100644 index 0000000..b41bac3 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_proximal.STL new file mode 100644 index 0000000..babac60 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_distal.STL new file mode 100644 index 0000000..1d9ba0c Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_middle.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_middle.STL new file mode 100644 index 0000000..bb1411e Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_middle.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_proximal.STL new file mode 100644 index 0000000..2733aff Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/ring_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_distal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_distal.STL new file mode 100644 index 0000000..e1f8819 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_distal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals.STL new file mode 100644 index 0000000..aa9f94b Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals_base1.STL new file mode 100644 index 0000000..2ba4a8b Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals_base1.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals_base2.STL new file mode 100644 index 0000000..cbf82a6 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_metacarpals_base2.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_proximal.STL b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_proximal.STL new file mode 100644 index 0000000..22c9357 Binary files /dev/null and b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf/right/meshes/thumb_proximal.STL differ diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py new file mode 100644 index 0000000..66e634b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py @@ -0,0 +1,146 @@ +"""O12 observation graph for the shared G20 spatial-zero solver. + +Parallel flexion axes observe phase through their line positions, not their +directions. SDK travel endpoints are deliberately not treated as CAD datums. +Passive joints supply observations but never independently fitted static zeros. +""" + +from dataclasses import asdict, replace +import math +from pathlib import Path +from typing import Any, Mapping, Sequence + +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.fitting.session import compile_spatial_profile +from linkerhand_calibration.core.fitting.spatial import ( + ZeroCalibrationProfile, ZeroSolveResult, + fit_joint_axis_measurement, solve_urdf_zero_offsets, + with_depth_free_axis_projection, +) +from linkerhand_calibration.models.o12.profile import ( + CALIBRATED_ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, + SDK_TO_URDF_SIGN, + build_typed_profile, +) +from linkerhand_calibration.models.o12.kinematics import PASSIVE_SDK_SOURCE_BY_JOINT + +SPATIAL_ZERO_POLICY = ( + "o12_full_hand_spatial_v5_thumb_mcp_cad_static_stable_pnp_bias" +) + +# Historical diagnostic imports read the same protected YAML as production. +_SPATIAL = build_typed_profile().zero.spatial +AXIS_PARENT = dict(_SPATIAL["axis_parent_joint"]) +PHASE_PARENT = dict(_SPATIAL["phase_parent_joint"]) +ZERO_OBSERVER = dict(_SPATIAL["offset_observer_joint"]) +AXIS_JOINTS = tuple(_SPATIAL["axis_order"]) + + +def motor_index(joint: str) -> int: + return COMMAND_INDEX_BY_JOINT[PASSIVE_SDK_SOURCE_BY_JOINT.get(joint, joint)] + + +def full_hand_zero_profile() -> ZeroCalibrationProfile: + return compile_spatial_profile(build_typed_profile()) + + +class O12SpatialZeroError(ValueError): + """Unpublishable geometry, with machine-readable per-joint evidence.""" + + def __init__(self, message: str, diagnostics: Mapping[str, Any]) -> None: + super().__init__(message) + self.diagnostics = diagnostics + # Populated only after the full dynamic fit and an identifiable + # spatial estimate exist. Never a substitute for a passed result. + self.review_fit = None + + +def solve_full_hand_zero( + source_urdf: str | Path, + virtual_records: Mapping[str, Sequence[Mapping[str, Any]]], + solver_curves: Mapping[str, JointCurveFit], +) -> ZeroSolveResult: + profile = full_hand_zero_profile() + tasks = {joint: task for task in build_typed_profile().motion.tasks for joint in task.joints} + measurements = [] + for cycle in range(4): + by_joint = {} + for name in AXIS_JOINTS: + rows = virtual_records[name] + # Relative passive motion is measured in its moving parent Tag + # frame. Its common-frame line is evaluated at the same baseline + # condition, just as in G20; it is not a circle of the entire chain. + parent = PHASE_PARENT.get(name) + # PHASE_PARENT declares parallel physical flexion axes, including + # ACTIVE PIP/MCP axes. Independent planar-PnP direction errors must + # not turn those into skew lines and then appear as encoder zeros. + # The task profile holds nonparallel upstream axes at the same + # pose for these paired captures. Preserve each child's measured + # scalar rotation; constrain only the shared physical direction. + constraint = by_joint[parent].axis_common_xyz if parent in by_joint else None + try: + item = fit_joint_axis_measurement( + name, rows, cycle=cycle, + zero_command_u8=tasks[name].start_value if "input_value" in rows[0] else 255, + input_to_joint_direction=int(SDK_TO_URDF_SIGN[motor_index(name)]), + constrained_circle_joints=profile.constrained_circle_joints, + view_normal_common_xyz=rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + axis_common_constraint=constraint, + separate_axial_residual=True, + ) + except ValueError as error: + raise O12SpatialZeroError( + f"O12 spatial zero observation failed:{name}:cycle={cycle}:{error}", + {"passed": False, "stage": "axis_observation", "joint": name, + "cycle": cycle, "reason": str(error)}, + ) from error + item = with_depth_free_axis_projection(item, rows[0]["camera_center_common_xyz_m"]) + measurements.append(item) + by_joint[name] = item + try: + result = solve_urdf_zero_offsets( + source_urdf=source_urdf, measurements=measurements, curves=solver_curves, + motor_by_joint={name: motor_index(name) for name in AXIS_JOINTS}, + training_cycles=(0, 1, 2), validation_cycle=3, + maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20), + maximum_cycle_difference_rad=math.radians(.75), + minimum_applied_offset_rad=math.radians(.1), + maximum_validation_mae_rad=math.radians(1), + maximum_validation_p95_rad=math.radians(2), + maximum_validation_error_rad=math.radians(3), + maximum_confidence_half_width_rad=math.radians(1), + # Match G20's established treatment of a repeatable planar-Tag + # direction bias: 5 deg remains the normal cone target, while a + # stable (<=0.75 deg cycle spread) bias up to 15 deg is retained + # as an explicit diagnostic instead of corrupting/rejecting an + # otherwise observable encoder zero. + maximum_systematic_axis_cone_bias_rad=math.radians(15), + maximum_pose_axis_line_rms_m=.0015, + hand_type="right", tag_layout="o12_right_16", zero_profile=profile, + ) + except ValueError as error: + raise O12SpatialZeroError( + "O12 full-hand spatial zero geometry is invalid:" + str(error), + {"passed": False, "stage": "spatial_solve", "reason": str(error), + "axis_measurements": [asdict(item) for item in measurements]}, + ) from error + result = replace(result, axis_residual_diagnostics={ + f"{item.joint}:cycle{item.cycle}": { + "axial_component_separated": item.axis_point_axial_component_separated, + "fit_rms_m": item.pose_axis_line_rms_m, + "raw_rms_m": item.pose_axis_line_raw_rms_m, + "axial_rms_m": item.pose_axis_line_axial_rms_m, + "transverse_rms_m": item.pose_axis_line_transverse_rms_m, + } for item in measurements + }) + if not result.passed: + details = ",".join(f"{n}={r}" for n, r in sorted(result.failure_reasons.items())) + raise O12SpatialZeroError( + "O12 full-hand spatial zero solve failed:" + details, + {"passed": False, "stage": "spatial_solve", "policy": SPATIAL_ZERO_POLICY, + "result": asdict(result), + "axis_measurements": [asdict(item) for item in measurements]}, + ) + return result