Import O12 calibration model assets

This commit is contained in:
sunxianghui
2026-10-29 14:21:57 +08:00
parent be9039ec31
commit 55ff0a52db
64 changed files with 5461 additions and 0 deletions
@@ -0,0 +1,5 @@
# O12 标定
本目录存放 O12 的专属程序、配置和左右手 URDF。 `core/`、`runtime/` 和 `profiles/` 中的通用程序仍由各型号共享。
旧的 `config/`、`urdf/` 路径及 `compat/legacy_diagnostic_tools/models/` 导入路径保留兼容;现有产品配置与历史采集记录中的资源内容和哈希不变。
@@ -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"]
@@ -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",
]
@@ -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]
@@ -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
@@ -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
@@ -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
@@ -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)
@@ -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",
]
@@ -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",
]
@@ -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",
]
@@ -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()
@@ -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"]
@@ -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)
@@ -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)
@@ -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
@@ -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",
]
@@ -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",
]
@@ -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
@@ -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"]
@@ -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