Import O12 calibration model assets
This commit is contained in:
@@ -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",
|
||||
]
|
||||
+41
@@ -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]
|
||||
+47
@@ -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
|
||||
+53
@@ -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
|
||||
+1029
File diff suppressed because it is too large
Load Diff
@@ -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"]
|
||||
+1231
File diff suppressed because it is too large
Load Diff
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
+1234
File diff suppressed because it is too large
Load Diff
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -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
|
||||
Reference in New Issue
Block a user