Complete calibration package import

This commit is contained in:
sunxianghui
2026-10-29 14:21:57 +08:00
parent 55ff0a52db
commit 22f0aaf1ba
364 changed files with 54837 additions and 0 deletions
@@ -0,0 +1,10 @@
# Python 程序目录
- `models/`:各手型的专属代码、配置、源 URDF 和型号文档。
- `core/`:共享的数据结构、几何、拟合和 URDF 处理。
- `runtime/`:共享的采集状态机、ROS/SDK 接线、离线收尾与发布。
- `profiles/`:型号 YAML 的加载与检查。
- `compat/`:历史数据格式与旧入口。
- `tools/`:独立诊断和辅助命令。
本目录根层仍有两类文件:`product.py`、`acquisition.py`、`extrinsics.py` 等共享入口;`full_hand.py`、`trajectory.py`、`pnp.py` 等历史导入兼容文件。新增型号专属实现应放在 `models/<型号>/`,新增共享实现应放在对应的 `core/` 或 `runtime/` 子目录,不继续堆在根层。
@@ -0,0 +1,5 @@
"""Profile-driven LinkerHand calibration and validated URDF correction."""
from .core import CalibrationProfile, ProfileKey
__all__ = ["CalibrationProfile", "ProfileKey"]
@@ -0,0 +1,748 @@
"""Hardware-independent point acquisition state."""
from __future__ import annotations
from bisect import bisect_left
from collections import deque
from dataclasses import dataclass, field
from typing import Any, Mapping, Sequence
import numpy as np
from .core import robust_rotation_summary
from .pnp import SquareTagPose
TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
"t0_t3": ("t0", "t3"),
"t3_t4": ("t3", "t4"),
"t4_t5": ("t4", "t5"),
}
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
@dataclass(frozen=True)
class TagQuality:
hamming: int
decision_margin: float
edge_pixels: float
reprojection_error_px: float | None = None
def tag_quality_is_valid(
quality: TagQuality,
*,
maximum_hamming: int,
minimum_decision_margin: float,
minimum_edge_pixels: float,
maximum_reprojection_error_px: float | None = None,
) -> bool:
detection_valid = (
quality.hamming <= maximum_hamming
and quality.decision_margin >= minimum_decision_margin
and quality.edge_pixels >= minimum_edge_pixels
)
if not detection_valid:
return False
if maximum_reprojection_error_px is None:
return True
return (
quality.reprojection_error_px is not None
and quality.reprojection_error_px <= maximum_reprojection_error_px
)
def update_pnp_reset_watchdog(
*,
detection_good: bool,
pnp_valid: bool,
now: float,
invalid_since: float | None,
reset_after_seconds: float,
) -> tuple[float | None, bool]:
"""Track continuous PnP-only failures and request a throttled reset."""
reset_after = float(reset_after_seconds)
if reset_after <= 0.0:
raise ValueError("reset_after_seconds must be positive")
if not detection_good or pnp_valid:
return None, False
since = float(now) if invalid_since is None else float(invalid_since)
if float(now) - since >= reset_after:
# Start a new interval so a permanently bad view is not reset on every
# frame. The next valid frame clears the interval.
return float(now), True
return since, False
def required_resume_views(active_view: str | None) -> tuple[str, ...]:
"""Require only the active view on resume; start still checks all views."""
all_views = ("front", "side", "top")
if active_view is None:
return all_views
view = str(active_view)
if view not in all_views:
raise ValueError(f"unknown calibration view: {view}")
return (view,)
@dataclass(frozen=True)
class Observation:
stamp_ns: int
received_at: float
relative_quaternion_xyzw: Mapping[str, tuple[float, float, float, float]]
tag_quality: Mapping[str, TagQuality]
state_u8: tuple[float, ...] = ()
state_stamp_ns: int | None = None
state_sync_error_ns: int | None = None
tag_quaternion_xyzw: Mapping[
str, tuple[float, float, float, float]
] = field(default_factory=dict)
tag_translation_xyz_m: Mapping[
str, tuple[float, float, float]
] = field(default_factory=dict)
tag_pose_candidates: Mapping[
str, tuple[SquareTagPose, ...]
] = field(default_factory=dict)
@dataclass(frozen=True)
class StateSample:
stamp_ns: int
position_u8: tuple[float, ...]
channel_stamps_ns: tuple[int, ...] = ()
def interpolate_state_u8(
samples: Sequence[StateSample],
stamp_ns: int,
*,
maximum_skew_ns: int,
) -> tuple[tuple[float, ...], int] | None:
"""Interpolate a profile-sized hand state at an image timestamp.
The SDK publishes state independently from the camera. Continuous
calibration must therefore use the image timestamp instead of whichever
state happened to arrive most recently in the ROS callback thread.
"""
if maximum_skew_ns < 0:
raise ValueError("maximum_skew_ns must be non-negative")
if not samples:
return None
if any(sample.channel_stamps_ns for sample in samples):
# A multi-frame SDK does not observe all motors at the same instant.
# De-duplicate each channel by its CAN receipt, then interpolate it
# independently. Repeated publication never creates extra support.
samples = tuple(sample for sample in samples if sample.stamp_ns >= stamp_ns-maximum_skew_ns
and sample.channel_stamps_ns and min(sample.channel_stamps_ns) <= stamp_ns+maximum_skew_ns)
if not samples:
return None
count = len(samples[0].position_u8)
channels = [dict() for _ in range(count)]
for sample in samples:
if len(sample.position_u8) != count or len(sample.channel_stamps_ns) != count:
return None
for index, (stamp, value) in enumerate(zip(sample.channel_stamps_ns, sample.position_u8)):
if stamp in channels[index] and channels[index][stamp] != value:
return None
channels[index][stamp] = value
matches = [interpolate_state_u8(tuple(StateSample(stamp, (value,))
for stamp, value in sorted(channel.items())), stamp_ns, maximum_skew_ns=maximum_skew_ns)
for channel in channels]
if any(match is None for match in matches):
return None
return tuple(match[0][0] for match in matches), max(match[1] for match in matches)
stamps = [int(sample.stamp_ns) for sample in samples]
index = bisect_left(stamps, int(stamp_ns))
if index < len(samples) and stamps[index] == int(stamp_ns):
state = samples[index].position_u8
return (tuple(float(value) for value in state), 0)
before = samples[index - 1] if index > 0 else None
after = samples[index] if index < len(samples) else None
if before is not None and after is not None:
before_gap = int(stamp_ns) - int(before.stamp_ns)
after_gap = int(after.stamp_ns) - int(stamp_ns)
nearest_gap = min(before_gap, after_gap)
if nearest_gap > maximum_skew_ns:
return None
denominator = int(after.stamp_ns) - int(before.stamp_ns)
if denominator <= 0:
return (
tuple(float(value) for value in before.position_u8),
nearest_gap,
)
fraction = before_gap / denominator
before_values = np.asarray(before.position_u8, dtype=float)
after_values = np.asarray(after.position_u8, dtype=float)
if (
before_values.ndim != 1
or before_values.size == 0
or after_values.shape != before_values.shape
):
return None
interpolated = before_values + fraction * (after_values - before_values)
return (
tuple(float(value) for value in interpolated),
nearest_gap,
)
nearest = before if before is not None else after
if nearest is None:
return None
gap = abs(int(stamp_ns) - int(nearest.stamp_ns))
if gap > maximum_skew_ns or not nearest.position_u8:
return None
return (tuple(float(value) for value in nearest.position_u8), gap)
# Physical-angle profiles use the same timestamp interpolation. Keep the old
# public name for compatibility and offer a unit-neutral spelling to new code.
interpolate_state = interpolate_state_u8
class ContinuousSweepCollector:
"""Collect timestamp-synchronised observations during one end-to-end move."""
def __init__(
self,
*,
endpoint_tolerance_u8: float = 2.0,
endpoint_hold_seconds: float = 1.0,
timeout_seconds: float = 90.0,
invalid_timeout_seconds: float = 2.0,
minimum_valid_frames: int = 40,
minimum_state_span_u8: float = 240.0,
) -> None:
if endpoint_tolerance_u8 < 0.0:
raise ValueError("endpoint_tolerance_u8 must be non-negative")
if endpoint_hold_seconds <= 0.0:
raise ValueError("endpoint_hold_seconds must be positive")
if timeout_seconds <= 0.0 or invalid_timeout_seconds <= 0.0:
raise ValueError("sweep timeouts must be positive")
if minimum_valid_frames < 3:
raise ValueError("minimum_valid_frames must be at least 3")
if minimum_state_span_u8 <= 0.0:
raise ValueError("minimum_state_span_u8 must be positive")
self.endpoint_tolerance_u8 = float(endpoint_tolerance_u8)
self.endpoint_hold_seconds = float(endpoint_hold_seconds)
self.timeout_seconds = float(timeout_seconds)
self.invalid_timeout_seconds = float(invalid_timeout_seconds)
self.minimum_valid_frames = int(minimum_valid_frames)
self.minimum_state_span_u8 = float(minimum_state_span_u8)
self.observations: list[Observation] = []
self.motor_index = 0
self.start_u8 = 255.0
self.target_u8 = 0.0
self.started_at: float | None = None
self.last_valid_at: float | None = None
self.endpoint_since: float | None = None
self.state = "idle"
self.reason = ""
def start(
self,
now: float,
*,
motor_index: int,
start_u8: int,
target_u8: int,
) -> None:
if motor_index not in (0, 15):
raise ValueError("continuous thumb sweep only permits motor 0 or 15")
if {int(start_u8), int(target_u8)} != {0, 255}:
raise ValueError("continuous sweep endpoints must be 0 and 255")
self.observations.clear()
self.motor_index = int(motor_index)
self.start_u8 = float(start_u8)
self.target_u8 = float(target_u8)
self.started_at = float(now)
self.last_valid_at = float(now)
self.endpoint_since = None
self.state = "collecting"
self.reason = ""
@property
def active(self) -> bool:
return self.state == "collecting"
@property
def valid_frames_seen(self) -> int:
return len(self.observations)
@property
def state_span_u8(self) -> float:
if not self.observations:
return 0.0
values = [
float(observation.state_u8[self.motor_index])
for observation in self.observations
]
return float(max(values) - min(values))
def add(
self, observation: Observation, now: float
) -> list[Observation] | None:
if not self.active:
return None
if (
len(observation.state_u8) != 20
or observation.state_sync_error_ns is None
):
return None
value = float(observation.state_u8[self.motor_index])
if not np.isfinite(value) or not -3.0 <= value <= 258.0:
return None
now = float(now)
self.observations.append(observation)
self.last_valid_at = now
if abs(value - self.target_u8) <= self.endpoint_tolerance_u8:
if self.endpoint_since is None:
self.endpoint_since = now
else:
self.endpoint_since = None
enough_endpoint_hold = (
self.endpoint_since is not None
and now - self.endpoint_since >= self.endpoint_hold_seconds
)
if (
enough_endpoint_hold
and len(self.observations) >= self.minimum_valid_frames
and self.state_span_u8 >= self.minimum_state_span_u8
):
self.state = "complete"
return list(self.observations)
return None
def poll(self, now: float) -> None:
if not self.active:
return
now = float(now)
if now - float(self.started_at) > self.timeout_seconds:
self.state = "failed"
self.reason = "sweep_timeout"
elif now - float(self.last_valid_at) > self.invalid_timeout_seconds:
self.state = "failed"
self.reason = "synchronised_tag_state_timeout"
def aggregate_sweep_observations(
observations: Sequence[Observation],
*,
motor_index: int,
start_u8: int,
target_u8: int,
endpoint_tolerance_u8: float,
) -> dict[int, dict[str, Any]]:
"""Robustly aggregate continuous observations into integer motor bins."""
if not observations:
raise ValueError("cannot aggregate an empty continuous sweep")
bins: dict[int, list[Observation]] = {}
for observation in observations:
if len(observation.state_u8) != 20:
continue
value = float(observation.state_u8[motor_index])
if abs(value - float(start_u8)) <= endpoint_tolerance_u8:
command = int(start_u8)
elif abs(value - float(target_u8)) <= endpoint_tolerance_u8:
command = int(target_u8)
else:
command = int(np.clip(np.rint(value), 0, 255))
bins.setdefault(command, []).append(observation)
return {
command: aggregate_observations(values)
for command, values in sorted(bins.items())
}
class PointCollector:
"""Wait for a stable pose, then aggregate a fixed number of frames."""
def __init__(
self,
*,
stable_frames: int = 15,
capture_frames: int = 30,
minimum_settle_seconds: float = 0.4,
maximum_stable_spread_rad: float = np.deg2rad(0.3),
stability_mode: str = "rotation",
maximum_stable_translation_spread_m: float = 0.003,
settle_timeout_seconds: float = 5.0,
capture_timeout_seconds: float = 5.0,
) -> None:
if stable_frames < 3 or capture_frames < 3:
raise ValueError("stable_frames and capture_frames must be at least 3")
self.stable_frames = int(stable_frames)
self.capture_frames = int(capture_frames)
self.minimum_settle_seconds = float(minimum_settle_seconds)
self.maximum_stable_spread_rad = float(maximum_stable_spread_rad)
self.stability_mode = str(stability_mode)
self.maximum_stable_translation_spread_m = float(
maximum_stable_translation_spread_m
)
if self.stability_mode not in {"rotation", "translation"}:
raise ValueError(
"stability_mode must be rotation or translation"
)
if self.maximum_stable_translation_spread_m <= 0.0:
raise ValueError(
"maximum_stable_translation_spread_m must be positive"
)
self.settle_timeout_seconds = float(settle_timeout_seconds)
self.capture_timeout_seconds = float(capture_timeout_seconds)
self._stable: deque[Observation] = deque(maxlen=self.stable_frames)
self._captured: list[Observation] = []
self._consecutive_invalid_frames = 0
self.started_at: float | None = None
self.capture_started_at: float | None = None
self.state = "idle"
self.reason = ""
self.stable_spread_rad: dict[str, float] = {}
self.stable_spread_m: dict[str, float] = {}
self.required_state_index: int | None = None
self.required_state_u8: float | None = None
self.maximum_state_error_u8: float | None = None
def start(
self,
now: float,
*,
required_state_index: int | None = None,
required_state_u8: float | None = None,
maximum_state_error_u8: float | None = None,
) -> None:
state_constraints = (
required_state_index,
required_state_u8,
maximum_state_error_u8,
)
if any(value is not None for value in state_constraints) and not all(
value is not None for value in state_constraints
):
raise ValueError(
"point state constraint parameters must be provided together"
)
if required_state_index is not None:
if not 0 <= int(required_state_index) < 20:
raise ValueError("required_state_index must be in [0, 19]")
if not np.isfinite(float(required_state_u8)):
raise ValueError("required_state_u8 must be finite")
if float(maximum_state_error_u8) < 0.0:
raise ValueError(
"maximum_state_error_u8 must be non-negative"
)
self._stable.clear()
self._captured.clear()
self._consecutive_invalid_frames = 0
self.started_at = float(now)
self.capture_started_at = None
self.state = "settling"
self.reason = ""
self.stable_spread_rad = {}
self.stable_spread_m = {}
self.required_state_index = (
None
if required_state_index is None
else int(required_state_index)
)
self.required_state_u8 = (
None if required_state_u8 is None else float(required_state_u8)
)
self.maximum_state_error_u8 = (
None
if maximum_state_error_u8 is None
else float(maximum_state_error_u8)
)
@property
def active(self) -> bool:
return self.state in {"settling", "capturing"}
@property
def stable_frames_seen(self) -> int:
return len(self._stable)
@property
def capture_frames_seen(self) -> int:
return len(self._captured)
def _window_is_stable(self) -> bool:
if len(self._stable) < self.stable_frames:
return False
if self.stability_mode == "translation":
spreads: dict[str, float] = {}
for pair, (parent, child) in TAG_PAIR_ROLES.items():
if any(
parent not in observation.tag_translation_xyz_m
or child not in observation.tag_translation_xyz_m
for observation in self._stable
):
self.reason = f"{pair}_translation_missing"
return False
vectors = np.asarray(
[
np.asarray(
observation.tag_translation_xyz_m[child],
dtype=float,
)
- np.asarray(
observation.tag_translation_xyz_m[parent],
dtype=float,
)
for observation in self._stable
],
dtype=float,
)
reference = np.median(vectors, axis=0)
spreads[pair] = float(
np.max(np.linalg.norm(vectors - reference, axis=1))
)
self.stable_spread_m = spreads
for pair, spread in spreads.items():
if spread > self.maximum_stable_translation_spread_m:
self.reason = f"{pair}_not_stable"
return False
return True
spreads: dict[str, float] = {}
for pair in PAIR_NAMES:
quaternions = [
observation.relative_quaternion_xyzw[pair]
for observation in self._stable
]
_, spread = robust_rotation_summary(quaternions)
spreads[pair] = float(spread)
self.stable_spread_rad = spreads
for pair, spread in spreads.items():
if spread > self.maximum_stable_spread_rad:
self.reason = f"{pair}_not_stable"
return False
return True
def _state_is_acceptable(self, observation: Observation) -> bool:
if self.required_state_index is None:
return True
if (
len(observation.state_u8) != 20
or observation.state_sync_error_ns is None
):
return False
value = float(observation.state_u8[self.required_state_index])
return bool(
np.isfinite(value)
and abs(value - float(self.required_state_u8))
<= float(self.maximum_state_error_u8)
)
def _return_to_settling(self, reason: str) -> None:
self._stable.clear()
self._captured.clear()
self._consecutive_invalid_frames = 0
self.capture_started_at = None
self.state = "settling"
self.reason = str(reason)
self.stable_spread_rad = {}
self.stable_spread_m = {}
def add(
self, observation: Observation, now: float
) -> dict[str, Any] | None:
if not self.active:
return None
self._consecutive_invalid_frames = 0
now = float(now)
if not self._state_is_acceptable(observation):
self._return_to_settling("motor_position_out_of_tolerance")
return None
if self.state == "settling":
self._stable.append(observation)
elapsed = now - float(self.started_at)
if elapsed >= self.minimum_settle_seconds and self._window_is_stable():
self.state = "capturing"
self.capture_started_at = now
self._captured.clear()
self.reason = ""
return None
self._captured.append(observation)
if len(self._captured) < self.capture_frames:
return None
# Validate continuity across the boundary as well as inside the
# capture block. A planar branch can switch immediately after the
# stable window and then look perfectly stable for every capture
# frame; checking only the captured frames would accept that jump.
stability_aggregate = aggregate_observations(
[*self._stable, *self._captured]
)
if self.stability_mode == "translation":
unstable_pairs = [
pair
for pair, spread in stability_aggregate[
"maximum_translation_spread_m"
].items()
if float(spread)
> self.maximum_stable_translation_spread_m
]
else:
unstable_pairs = [
pair
for pair, spread in stability_aggregate[
"maximum_spread_rad"
].items()
if float(spread) > self.maximum_stable_spread_rad
]
if unstable_pairs:
self._return_to_settling(
f"{unstable_pairs[0]}_capture_not_stable"
)
return None
self.state = "complete"
return aggregate_observations(self._captured)
def poll(self, now: float) -> None:
if not self.active:
return
now = float(now)
if self.state == "settling":
if now - float(self.started_at) > self.settle_timeout_seconds:
self.state = "failed"
self.reason = self.reason or "settle_timeout"
elif self.state == "capturing":
if now - float(self.capture_started_at) > self.capture_timeout_seconds:
self.state = "failed"
self.reason = "capture_timeout"
def mark_invalid_frame(self) -> None:
"""Skip one invalid frame while retaining the recent valid window."""
if self.state == "capturing":
self._return_to_settling("invalid_tag_frame")
return
if self.state == "settling":
self._consecutive_invalid_frames += 1
if self._consecutive_invalid_frames >= 3:
self._stable.clear()
self.reason = "invalid_tag_frame"
def aggregate_observations(
observations: Sequence[Observation],
) -> dict[str, Any]:
if not observations:
raise ValueError("cannot aggregate an empty observation sequence")
relative: dict[str, list[float]] = {}
spread: dict[str, float] = {}
for pair in PAIR_NAMES:
quaternion, maximum = robust_rotation_summary(
[
observation.relative_quaternion_xyzw[pair]
for observation in observations
]
)
relative[pair] = [float(value) for value in quaternion]
spread[pair] = float(maximum)
quality: dict[str, dict[str, float]] = {}
tag_names = sorted(observations[0].tag_quality)
for tag_name in tag_names:
values = [
observation.tag_quality[tag_name] for observation in observations
]
quality[tag_name] = {
"minimum_decision_margin": float(
min(value.decision_margin for value in values)
),
"minimum_edge_pixels": float(min(value.edge_pixels for value in values)),
"maximum_hamming": int(max(value.hamming for value in values)),
}
reprojection_errors = [
float(value.reprojection_error_px)
for value in values
if value.reprojection_error_px is not None
]
if reprojection_errors:
quality[tag_name]["maximum_reprojection_error_px"] = float(
max(reprojection_errors)
)
states = [
observation.state_u8
for observation in observations
if len(observation.state_u8) == 20
]
state_median: list[float] = []
if states:
state_median = [
float(value)
for value in np.median(np.asarray(states, dtype=float), axis=0)
]
sync_errors = [
int(observation.state_sync_error_ns)
for observation in observations
if observation.state_sync_error_ns is not None
]
tag_translations: dict[str, list[float]] = {}
translation_spread: dict[str, float] = {}
translation_roles = sorted(
set.intersection(
*(
set(observation.tag_translation_xyz_m)
for observation in observations
)
)
if observations
else set()
)
for role in translation_roles:
values = np.asarray(
[
observation.tag_translation_xyz_m[role]
for observation in observations
],
dtype=float,
)
if values.shape == (len(observations), 3) and np.all(
np.isfinite(values)
):
tag_translations[role] = [
float(value)
for value in np.median(values, axis=0)
]
for pair, (parent, child) in TAG_PAIR_ROLES.items():
if parent not in translation_roles or child not in translation_roles:
continue
vectors = np.asarray(
[
np.asarray(
observation.tag_translation_xyz_m[child],
dtype=float,
)
- np.asarray(
observation.tag_translation_xyz_m[parent],
dtype=float,
)
for observation in observations
],
dtype=float,
)
reference = np.median(vectors, axis=0)
translation_spread[pair] = float(
np.max(np.linalg.norm(vectors - reference, axis=1))
)
return {
"stamp_start_ns": int(observations[0].stamp_ns),
"stamp_end_ns": int(observations[-1].stamp_ns),
"valid_frames": len(observations),
"relative_quaternion_xyzw": relative,
"maximum_spread_rad": spread,
"tag_quality": quality,
"state_u8_median": state_median,
"tag_translation_xyz_m": tag_translations,
"maximum_translation_spread_m": translation_spread,
"maximum_state_sync_error_ms": (
None
if not sync_errors
else float(max(sync_errors)) / 1_000_000.0
),
}
@@ -0,0 +1,4 @@
"""Compatibility import for G20 alignment_view."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.alignment_view")
@@ -0,0 +1,377 @@
"""Map explicit SDK commands or feedback to corrected URDF joint coordinates.
Unified artifacts load their certified manifest and standard URDF. The old
readers below are used only when a historical payload is explicitly supplied.
The static encoder-zero corrections in ``zero_angles`` are already baked into
the corrected URDF joint origins. This bridge therefore publishes only the
dynamic ``angle_rad`` values and never adds the static offsets a second time.
Schema-v5 trajectories are fitted against timestamp-synchronised hardware
feedback, not controller set-points. They must therefore be queried with the
SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for
backwards compatibility only.
"""
from __future__ import annotations
import json
import math
from pathlib import Path
from typing import Any, Mapping, Sequence
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
G20_COMMAND_NAMES: tuple[str, ...] = (
"thumb_cmc_pitch",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
"thumb_cmc_roll",
"index_mcp_roll",
"middle_mcp_roll",
"ring_mcp_roll",
"pinky_mcp_roll",
"thumb_cmc_yaw",
"reserved_11",
"reserved_12",
"reserved_13",
"reserved_14",
"thumb_mcp",
"index_pip",
"middle_pip",
"ring_pip",
"pinky_pip",
)
# Match the stable ordering used by the existing MuJoCo bridge. JointState
# consumers must use names, but retaining the ordering also keeps logs and
# direct comparisons deterministic.
G20_URDF_JOINT_NAMES: tuple[str, ...] = (
"index_dip",
"index_mcp_pitch",
"index_mcp_roll",
"index_pip",
"middle_dip",
"middle_mcp_pitch",
"middle_mcp_roll",
"middle_pip",
"pinky_dip",
"pinky_mcp_pitch",
"pinky_mcp_roll",
"pinky_pip",
"ring_dip",
"ring_mcp_pitch",
"ring_mcp_roll",
"ring_pip",
"thumb_cmc_pitch",
"thumb_cmc_roll",
"thumb_cmc_yaw",
"thumb_ip",
"thumb_mcp",
)
class CalibratedCommandMapper:
"""Validated, profile-specific lookup from SDK u8 values to URDF radians."""
def __init__(
self, payload: Mapping[str, Any], *, expected_side: str | None = None
) -> None:
from .full_hand import get_hand_calibration_profile, infer_compact_payload_layout, validate_compact_payload
from .compat.legacy_diagnostic_tools.models import get_default_registry, validate_schema_v6_runtime_payload
from .compat.legacy_diagnostic_tools.models.o12.artifacts import validate_o12_runtime_payload
from .core import ProfileKey
schema_version = int(payload["schema_version"])
if schema_version == 7:
validate_o12_runtime_payload(payload)
elif schema_version == 6:
validate_schema_v6_runtime_payload(payload)
else:
validate_compact_payload(payload)
side = str(payload["side"]).lower()
if expected_side is not None and side != str(expected_side).lower():
raise ValueError(
f"calibration side {side!r} does not match requested side "
f"{str(expected_side).lower()!r}"
)
quality = payload["quality"]
if quality.get("passed") is not True:
raise ValueError("calibration quality.passed must be true")
layout_id = (
str(payload["layout_id"])
if schema_version in {6, 7}
else infer_compact_payload_layout(payload)
)
self.side = side
self.layout_id = layout_id
self.model = str(payload["model"]).upper()
self.profile_id = str(
payload.get("profile_id", f"G20/{side}/{layout_id}/v1")
)
self.serial_number = str(payload["serial_number"])
self.input_domain = str(
payload.get(
"curve_input_domain",
"command_u8" if schema_version == 4 else "",
)
)
if self.input_domain not in {"command_u8", "feedback_u8", "feedback_rad"}:
raise ValueError("calibration curve_input_domain is invalid")
if schema_version in {6, 7}:
self.command_names = tuple(str(value) for value in payload["command_names"])
self.urdf_joint_names = tuple(str(name) for name in payload["joints"])
self._motor_by_joint = {
name: int(payload["joints"][name]["motor_index"])
for name in self.urdf_joint_names
}
registered = get_default_registry().get(
ProfileKey.parse(self.profile_id)
)
self.feedback_name_aliases = dict(
registered.profile.command.feedback_name_aliases
)
self.feedback_by_index = bool(
registered.profile.command.feedback_by_index
)
else:
profile = get_hand_calibration_profile(side, layout_id)
self.command_names = G20_COMMAND_NAMES
self.urdf_joint_names = G20_URDF_JOINT_NAMES
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in self.urdf_joint_names
}
self.feedback_name_aliases = {}
self.feedback_by_index = False
self._curves = {
name: tuple(
float(value)
for value in payload["joints"][name]["angle_rad"]
)
for name in self.urdf_joint_names
}
self._decreasing_curves = {
name: tuple(
float(value)
for value in payload["joints"][name].get(
"decreasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in self.urdf_joint_names
}
self._increasing_curves = {
name: tuple(
float(value)
for value in payload["joints"][name].get(
"increasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in self.urdf_joint_names
}
self._previous_by_motor: dict[int, float] = {}
self._direction_by_motor: dict[int, str] = {}
self.direction_deadband_u8 = 0.002 if schema_version == 7 else 0.5
self._knots = {
name: tuple(float(value) for value in payload["joints"][name].get(
"curve_input_knots_rad", ()
))
for name in self.urdf_joint_names
}
self._raw_increasing_branch = {
name: str(payload["joints"][name].get(
"raw_increasing_curve_branch", "increasing"
))
for name in self.urdf_joint_names
}
@staticmethod
def _command_index(value: float) -> int:
command = float(value)
if not math.isfinite(command):
raise ValueError("calibrated command positions must be finite")
return max(0, min(255, int(math.floor(command + 0.5))))
def map_positions(
self, positions: Sequence[float], names: Sequence[str] = ()
) -> tuple[float, ...]:
values = tuple(float(value) for value in positions)
if names and not self.feedback_by_index:
if len(names) != len(values):
raise ValueError(
"JointState names and positions must have equal length"
)
if len(set(names)) != len(names):
raise ValueError("JointState names must be unique")
by_name = dict(zip((str(name) for name in names), values))
for alias, canonical in self.feedback_name_aliases.items():
if alias in by_name and canonical not in by_name:
by_name[canonical] = by_name[alias]
missing = [name for name in self.command_names if name not in by_name]
if missing:
raise ValueError(
f"{self.model} feedback is missing named channels: "
+ ",".join(missing)
)
command = tuple(by_name[name] for name in self.command_names)
else:
if len(values) != len(self.command_names):
raise ValueError(
f"unnamed {self.model} feedback must contain exactly "
f"{len(self.command_names)} positions"
)
command = values
indices = (
() if self.input_domain == "feedback_rad"
else tuple(self._command_index(value) for value in command)
)
direction_by_motor: dict[int, str | None] = {}
for motor, value in enumerate(command):
previous = self._previous_by_motor.get(motor)
direction = self._direction_by_motor.get(motor)
if previous is not None:
if value > previous + self.direction_deadband_u8:
direction = "increasing"
elif value < previous - self.direction_deadband_u8:
direction = "decreasing"
direction_by_motor[motor] = direction
result: list[float] = []
for name in self.urdf_joint_names:
motor = self._motor_by_joint[name]
direction = direction_by_motor[motor]
if self.input_domain == "feedback_rad" and direction is not None:
raw_increasing = self._raw_increasing_branch[name]
direction = (
raw_increasing
if direction == "increasing"
else "increasing" if raw_increasing == "decreasing" else "decreasing"
)
curves = (
self._increasing_curves
if direction == "increasing"
else self._decreasing_curves
if direction == "decreasing"
else self._curves
)
if self.input_domain == "feedback_rad":
result.append(float(np.interp(command[motor], self._knots[name], curves[name])))
else:
result.append(curves[name][indices[motor]])
for motor, value in enumerate(command):
self._previous_by_motor[motor] = value
direction = direction_by_motor[motor]
if direction is not None:
self._direction_by_motor[motor] = direction
return tuple(result)
def load_calibrated_command_mapper(
calibration_file: str | Path, *, expected_side: str | None = None, input_kind="command"
) -> CalibratedCommandMapper:
path = Path(calibration_file).expanduser().resolve()
if not path.is_file():
raise ValueError(f"calibration JSON does not exist: {path}")
payload = json.loads(path.read_text(encoding="utf-8"))
if path.name == "release_manifest.json" or payload.get("format") in {"unified_calibration_v1", "unified_calibration_v2", "unified_calibration_v3"}:
from .runtime.artifacts.reader import load_unified_mapper
return load_unified_mapper(path, expected_side=expected_side, input_kind=input_kind)
return CalibratedCommandMapper(payload, expected_side=expected_side)
def default_input_topic(
hand_type: str, input_domain: str, model: str = "G20"
) -> str:
side = str(hand_type).lower()
if side not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
if input_domain == "feedback_u8":
return f"/{str(model).lower()}/cb_{side}_hand_state"
if input_domain == "command_u8":
return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
if input_domain == "feedback_rad":
return f"/{str(model).lower()}/{side}/joint_states"
if input_domain == "command_rad":
return f"/{str(model).lower()}/{side}/joint_cmd"
raise ValueError("calibration curve_input_domain is invalid")
class CalibratedJointStateBridge(Node):
def __init__(self) -> None:
super().__init__("calibrated_joint_state_bridge")
self.declare_parameter("hand_type", "right")
self.declare_parameter("calibration_file", "")
self.declare_parameter("input_topic", "")
self.declare_parameter("output_topic", "")
self.declare_parameter("input_kind", "command")
hand_type = str(self.get_parameter("hand_type").value).lower()
if hand_type not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
calibration_file = str(self.get_parameter("calibration_file").value)
if not calibration_file:
raise ValueError("calibration_file is required")
self.mapper = load_calibrated_command_mapper(
calibration_file, expected_side=hand_type, input_kind=str(self.get_parameter("input_kind").value)
)
input_topic = str(self.get_parameter("input_topic").value).strip()
output_topic = str(self.get_parameter("output_topic").value).strip()
self.input_topic = input_topic or default_input_topic(
hand_type, self.mapper.input_domain, self.mapper.model
)
self.output_topic = (
output_topic
or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state"
)
if self.output_topic in {self.input_topic, default_input_topic(hand_type,
"feedback_rad" if self.mapper.input_domain.endswith("rad") else "feedback_u8", self.mapper.model)}:
raise ValueError("mapped output must not overwrite the real SDK command/feedback topic")
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
self.subscription = self.create_subscription(
JointState, self.input_topic, self._command_callback, 10
)
self._last_error = ""
self.get_logger().info(
f"loaded {self.mapper.profile_id} calibration for "
f"{self.mapper.serial_number}: "
f"{self.input_topic} ({self.mapper.input_domain}) -> "
f"{self.output_topic}"
)
def _command_callback(self, command: JointState) -> None:
try:
positions = self.mapper.map_positions(command.position, command.name)
except ValueError as error:
message = str(error)
if message != self._last_error:
self.get_logger().error(message)
self._last_error = message
return
self._last_error = ""
result = JointState()
result.header = command.header
result.name = list(self.mapper.urdf_joint_names)
result.position = list(positions)
self.publisher.publish(result)
def main(args: Sequence[str] | None = None) -> None:
rclpy.init(args=args)
node: CalibratedJointStateBridge | None = None
try:
node = CalibratedJointStateBridge()
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":
main()
@@ -0,0 +1,217 @@
"""Map camera frame clocks to host time before pairing images with feedback.
The MVS frame counter timestamps exposure start. USB delivery/publication is
later. Clock latches bracket the device/host correspondence independently of
hand motion, images, or fitted calibration curves.
"""
from dataclasses import asdict, dataclass
import json
import math
from pathlib import Path
import time
CAMERA_TIMING_POLICY = "mvs_latched_exposure_time_v1"
CLOCK_DRIFT_TOLERANCE_NS = 1_000_000
@dataclass(frozen=True)
class ClockLatch:
device_tick: int
before_ns: int
after_ns: int
raw_before_ns: int | None = None
raw_after_ns: int | None = None
@property
def midpoint_ns(self):
return (self.before_ns+self.after_ns)//2
@property
def half_width_ns(self):
return (self.after_ns-self.before_ns)/2
class DeviceFrameClock:
"""A nominal device clock with short, measured host-time brackets.
A new latch is checked against the previous one before it can change the
mapping. This rejects wrong counter units, resets and host clock jumps.
Periodic updates bound accumulated drift; none depend on image delivery.
"""
def __init__(self, ticks_per_second):
if not math.isfinite(ticks_per_second) or ticks_per_second <= 0:
raise ValueError("invalid camera clock frequency")
self.ticks_per_second = ticks_per_second
self.anchor = None
self.last_frame_tick = None
self.last_stamp_ns = None
self.refresh_interval_seconds = .5
def elapsed_ns(self, ticks):
return round(ticks*1_000_000_000/self.ticks_per_second)
def update(self, latches):
if not latches or any(x.device_tick <= 0 or x.before_ns <= 0
or not 0 <= x.after_ns-x.before_ns <= 2_000_000 for x in latches):
raise ValueError("camera clock latch is invalid or too uncertain")
previous = self.anchor or latches[0]
if self.anchor is None and latches[-1].before_ns-latches[0].after_ns < 50_000_000:
raise ValueError("camera clock units need an independent elapsed-time check")
for current in latches:
elapsed = self.elapsed_ns(current.device_tick-previous.device_tick)
host_elapsed = current.midpoint_ns-previous.midpoint_ns
tolerance = current.half_width_ns+previous.half_width_ns+CLOCK_DRIFT_TOLERANCE_NS
if elapsed < 0 or abs(elapsed-host_elapsed) > tolerance:
raise ValueError("camera/host clock discontinuity or counter unit mismatch")
# Refresh before measured phase drift spends the existing 1 ms
# continuity allowance. A fixed 0.5 s period repeatedly invalidates a
# smooth slewing host clock. Scheduling changes; admission never does.
current = max(latches, key=lambda x: x.midpoint_ns)
host_elapsed = current.midpoint_ns-previous.midpoint_ns
if host_elapsed >= 50_000_000:
drift = abs(host_elapsed-self.elapsed_ns(current.device_tick-previous.device_tick))
rate = drift/host_elapsed
self.refresh_interval_seconds = max(.02, min(.5,
.25*CLOCK_DRIFT_TOLERANCE_NS/1e9/max(rate, 1e-12)))
self.anchor = min(latches, key=lambda x: x.half_width_ns)
def timestamp(self, tick, *, received_ns, exposure_us):
if (self.anchor is None or not 0 <= received_ns-self.anchor.midpoint_ns <= 2_000_000_000
or not math.isfinite(exposure_us) or exposure_us < 0):
raise ValueError("camera frame has no fresh clock or valid exposure")
if self.last_frame_tick is not None and tick <= self.last_frame_tick:
raise ValueError("camera frame counter reset or duplicate frame")
stamp = self.anchor.midpoint_ns+self.elapsed_ns(tick-self.anchor.device_tick)+round(exposure_us*500)
if stamp > received_ns+2_000_000 or (self.last_stamp_ns is not None and stamp <= self.last_stamp_ns):
raise ValueError("camera image time is future or out of order")
self.last_frame_tick, self.last_stamp_ns = tick, stamp
return stamp
class MvsCameraTiming:
"""The camera-specific clock latch and its optional append-only journal."""
def __init__(self, camera, mvs, clock_ns, *, exposure_us, journal_path="", raw_clock_ns=None):
self.camera, self.mvs, self.clock_ns = camera, mvs, clock_ns
# CLOCK_MONOTONIC_RAW is independent of wall-clock synchronization.
# It is diagnostic evidence only, never a replacement ROS timestamp.
self.raw_clock_ns = raw_clock_ns or (lambda: time.clock_gettime_ns(time.CLOCK_MONOTONIC_RAW))
self.exposure_us = exposure_us
self.frame_clock = None
self.last_stamp_ns = None
self.next_refresh = 0.
self.journal = None
if journal_path:
path = Path(journal_path)
path.parent.mkdir(parents=True, exist_ok=True)
self.journal = path.open("a", encoding="utf-8")
def _integer(self, name):
value = self.mvs.MVCC_INTVALUE_EX()
status = self.camera.MV_CC_GetIntValueEx(name, value)
if status != 0:
raise ValueError(f"camera clock cannot read {name}:0x{status:08x}")
return int(value.nCurValue)
def _latch(self):
raw_before = self.raw_clock_ns()
before = self.clock_ns()
status = self.camera.MV_CC_SetCommandValue("DeviceTimestampLatch")
if status != 0:
raise ValueError(f"camera clock latch failed:0x{status:08x}")
tick = self._integer("DeviceTimestamp")
after = self.clock_ns()
return ClockLatch(tick, before, after, raw_before, self.raw_clock_ns())
def _write(self, row):
if self.journal is not None:
self.journal.write(json.dumps(row, separators=(",", ":"))+"\n")
def start(self):
# The installed USB camera firmware returns ticks/second for this
# feature (despite its XML unit label). Verify that interpretation
# against elapsed host time; do not assume it for another firmware.
self.frame_clock = DeviceFrameClock(self._integer("DeviceTimestampIncrement"))
latches = []
for index in range(40):
if index:
time.sleep(.02)
latches.append(self._latch())
valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000]
if len(valid) >= 8 and valid[-1].before_ns-valid[0].after_ns >= 50_000_000:
self._update(latches)
return
raise ValueError("camera clock has no bounded-latency startup latches")
def _update(self, latches):
valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000]
if not valid:
# A delayed USB control transaction provides no new clock evidence.
# Keep the original two-second freshness bound while trying again.
self._write(dict(kind="camera_clock_latch_rejected", latches=[asdict(x) for x in latches]))
self.next_refresh = time.monotonic()+.02
return
try:
self.frame_clock.update(valid)
except ValueError as error:
# Invalidation alone loses the transaction that failed. Preserve
# both clocks and the old anchor so drift, jumps and bad device
# counters can be distinguished without weakening the check.
self._write(dict(kind="camera_clock_sync_rejected", policy=CAMERA_TIMING_POLICY,
reason=str(error), ticks_per_second=self.frame_clock.ticks_per_second,
previous_anchor=asdict(self.frame_clock.anchor) if self.frame_clock.anchor else None,
latches=[asdict(x) for x in latches]))
if self.journal is not None:
self.journal.flush()
raise
self._write(dict(kind="camera_clock_sync", policy=CAMERA_TIMING_POLICY,
ticks_per_second=self.frame_clock.ticks_per_second,
refresh_interval_seconds=self.frame_clock.refresh_interval_seconds,
latches=[asdict(x) for x in latches], anchor=asdict(self.frame_clock.anchor)))
if self.journal is not None:
self.journal.flush()
# Startup may select a precise latch near the beginning of its window.
# That anchor has already spent part of the refresh budget.
anchor_age = (max(x.after_ns for x in latches)-self.frame_clock.anchor.midpoint_ns)/1e9
self.next_refresh = time.monotonic()+max(0., self.frame_clock.refresh_interval_seconds-anchor_age)
def refresh_if_due(self):
if self.frame_clock is None:
self.start()
elif time.monotonic() >= self.next_refresh:
self._update([self._latch() for _ in range(3)])
def invalidate(self, reason):
"""Stop using an uncertain mapping until a new startup check succeeds."""
self.frame_clock = None
self.next_refresh = 0.
self._write(dict(kind="camera_clock_invalidated", reason=str(reason),
last_stamp_ns=self.last_stamp_ns))
if self.journal is not None:
self.journal.flush()
def timestamp(self, info, received_ns):
if self.frame_clock is None:
raise ValueError("camera frame has no synchronized clock")
tick = (int(info.nDevTimeStampHigh)<<32)|int(info.nDevTimeStampLow)
# Auto-exposure preview uses the exact exposure-start timestamp when
# the SDK supplies no per-frame duration. Calibration fixes exposure.
measured_exposure = float(info.fExposureTime)
exposure = measured_exposure if measured_exposure > 0 else self.exposure_us
stamp = self.frame_clock.timestamp(tick, received_ns=received_ns, exposure_us=exposure)
if self.last_stamp_ns is not None and stamp <= self.last_stamp_ns:
raise ValueError("camera exposure time is not increasing after clock synchronization")
self.last_stamp_ns = stamp
self._write(dict(kind="camera_frame_time", frame_number=int(info.nFrameNum),
device_tick=tick, sdk_host_stamp_ms=int(info.nHostTimeStamp),
received_ns=received_ns, stamp_ns=stamp, exposure_us=exposure,
timestamp_reference="exposure_midpoint" if exposure else "exposure_start"))
return stamp
def close(self):
if self.journal is not None:
self.journal.close()
self.journal = None
@@ -0,0 +1,21 @@
"""Compatibility adapters for one-release calibration migrations."""
from .config_v1 import (
legacy_default_profile_key,
product_profile_key,
resolve_legacy_profile_alias,
)
from .defaults import (
default_product_config_path,
default_three_camera_config_path,
)
from .paths import resolve_renamed_package_path
__all__ = [
"default_product_config_path",
"default_three_camera_config_path",
"legacy_default_profile_key",
"product_profile_key",
"resolve_legacy_profile_alias",
"resolve_renamed_package_path",
]
@@ -0,0 +1,47 @@
"""Identity migration for deployed product configuration schemas."""
from __future__ import annotations
from typing import Any, Mapping
from ..core import ProfileKey
def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
version = int(raw.get("schema_version", -1))
if version in {2, 3}:
key = ProfileKey.parse(str(raw.get("profile_id", "")))
for field, actual in (
("model", key.model),
("side", key.side),
("tag_layout", key.layout),
):
configured = str(raw.get(field, "")).strip()
if configured and configured.lower() != actual.lower():
raise ValueError(f"{field} differs from profile_id")
return key
if version != 1:
raise ValueError("product config schema_version must be 1, 2 or 3")
model = str(raw.get("model", "")).strip().upper()
side = str(raw.get("side", "")).strip().lower()
layout = str(raw.get("tag_layout", "")).strip().lower()
if not layout and (model, side) == ("G20", "right"):
layout = "g20_right_19"
return ProfileKey(model, side, layout, 1)
def legacy_default_profile_key() -> ProfileKey:
"""Preserve the former no-argument executable for one release."""
return ProfileKey("G20", "right", "g20_right_19", 1)
def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey:
"""Map retired layout identifiers to their reviewed physical profile."""
if (
key.model == "G20"
and key.side == "right"
and key.layout == "g20_right_15"
and key.revision == 1
):
return ProfileKey("G20", "right", "g20_right_19", 1)
return key
@@ -0,0 +1,16 @@
"""One-release default selection for invocations without ``--config``."""
from pathlib import Path
from ament_index_python.packages import get_package_share_directory
def default_product_config_path() -> Path:
share = Path(get_package_share_directory("linkerhand_calibration"))
return share / "config/g20_right_product.yaml"
def default_three_camera_config_path() -> Path:
"""Resolve the installed calibration defaults through the ROS index."""
share = Path(get_package_share_directory("linkerhand_calibration"))
return share / "config/three_camera_calibration.yaml"
@@ -0,0 +1,4 @@
"""Legacy single-camera algorithms retained for one compatibility release."""
from .session_v1 import uses_coupled_full_hand_zero_solver
__all__ = ["uses_coupled_full_hand_zero_solver"]
@@ -0,0 +1,24 @@
"""Version selection for replaying durable pre-v3 hardware sessions."""
from __future__ import annotations
from typing import Any, Mapping
def uses_coupled_full_hand_zero_solver(
session_start: Mapping[str, Any],
) -> bool:
"""Return the solver contract recorded by the legacy session header.
Capabilities are not consulted by the live runtime. This adapter reads
the durable v1 header only so offline replay can reproduce an artifact
created before the independent thumb solver was introduced.
"""
capabilities = {
str(value) for value in session_start.get("capabilities", ())
}
return (
int(session_start.get("sample_schema_version", 1)) == 1
and "palm_axis_side_channel_v2" in capabilities
and "palm_axis_relative_motion_v3" not in capabilities
)
@@ -0,0 +1,638 @@
"""Pure calibration math and command helpers.
This module deliberately has no ROS imports so the geometry, fitting, and
output schema can be tested without a camera or a connected hand.
"""
from __future__ import annotations
from dataclasses import dataclass, field
import math
from typing import Any, Iterable, Mapping, Sequence
import numpy as np
from scipy.spatial.transform import Rotation
COMMAND_NAMES: tuple[str, ...] = (
"thumb_cmc_pitch",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
"thumb_cmc_roll",
"index_mcp_roll",
"middle_mcp_roll",
"ring_mcp_roll",
"pinky_mcp_roll",
"thumb_cmc_yaw",
"reserved_11",
"reserved_12",
"reserved_13",
"reserved_14",
"thumb_mcp",
"index_pip",
"middle_pip",
"ring_pip",
"pinky_pip",
)
BASELINE_COMMAND: tuple[int, ...] = (
255,
255,
255,
255,
255,
255,
193,
148,
105,
42,
245,
255,
255,
255,
255,
255,
255,
255,
255,
255,
)
PAIR_ROOT = "t0_t3"
PAIR_MCP = "t3_t4"
PAIR_IP = "t4_t5"
PAIR_NAMES: tuple[str, ...] = (PAIR_ROOT, PAIR_MCP, PAIR_IP)
DIRECTION_DECREASING = "decreasing"
DIRECTION_INCREASING = "increasing"
DIRECTIONS: tuple[str, ...] = (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
)
PHASE_ROOT = "root"
PHASE_TIP = "tip"
JOINT_SPECS: dict[str, tuple[str, str, int]] = {
"thumb_cmc_pitch": (PHASE_ROOT, PAIR_ROOT, 0),
"thumb_mcp": (PHASE_TIP, PAIR_MCP, 15),
"thumb_ip": (PHASE_TIP, PAIR_IP, 15),
}
def build_command(
motor_index: int,
command_u8: int,
baseline: Sequence[int] = BASELINE_COMMAND,
) -> list[int]:
"""Return one full G20 command with exactly one replaced motor slot."""
if len(baseline) != 20:
raise ValueError("baseline must contain exactly 20 values")
values = [int(value) for value in baseline]
if any(value < 0 or value > 255 for value in values):
raise ValueError("baseline values must be in [0, 255]")
if motor_index not in (0, 5, 15):
raise ValueError(
"front thumb calibration only permits motor 0, 5 or 15"
)
command_u8 = int(command_u8)
if command_u8 < 0 or command_u8 > 255:
raise ValueError("command_u8 must be in [0, 255]")
values[motor_index] = command_u8
return values
def scan_targets(
repetitions: int = 3,
command_step: int = 1,
) -> list[tuple[int, str, int]]:
"""Build repeated 255->0->255 scan targets on a bounded command grid."""
if repetitions < 1:
raise ValueError("repetitions must be positive")
if command_step < 1 or command_step > 255:
raise ValueError("command_step must be in [1, 255]")
increasing = list(range(0, 256, command_step))
if increasing[-1] != 255:
increasing.append(255)
decreasing = list(reversed(increasing))
targets: list[tuple[int, str, int]] = []
for cycle in range(repetitions):
targets.extend(
(cycle, DIRECTION_DECREASING, command)
for command in decreasing
)
targets.extend(
(cycle, DIRECTION_INCREASING, command)
for command in increasing
)
return targets
def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
quaternion = np.asarray(values, dtype=float)
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
raise ValueError("quaternion must contain four finite xyzw values")
norm = float(np.linalg.norm(quaternion))
if norm < 1e-12:
raise ValueError("quaternion norm is zero")
return quaternion / norm
def relative_quaternion_xyzw(
parent_camera_quaternion: Sequence[float],
child_camera_quaternion: Sequence[float],
) -> tuple[float, float, float, float]:
"""Compute parent->child orientation from two camera->tag rotations."""
parent = Rotation.from_quat(normalize_quaternion_xyzw(parent_camera_quaternion))
child = Rotation.from_quat(normalize_quaternion_xyzw(child_camera_quaternion))
quaternion = (parent.inv() * child).as_quat()
return tuple(float(value) for value in quaternion)
def image_plane_tag_quaternion_xyzw(
corners_xy: Sequence[Sequence[float]],
) -> tuple[float, float, float, float]:
"""Estimate tag orientation about the optical axis from ordered corners."""
corners = np.asarray(corners_xy, dtype=float)
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
raise ValueError("corners_xy must contain four finite xy points")
# AprilTag corners 0->1 and 3->2 both follow the tag-local x axis.
# Average the two edges to reduce sub-pixel corner noise and perspective
# asymmetry. Image y points down, hence the minus sign for a right-handed
# camera-frame z rotation.
x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
if float(np.linalg.norm(x_axis)) < 1e-9:
raise ValueError("tag x-axis is degenerate")
angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
return tuple(float(value) for value in quaternion)
def robust_rotation_summary(
quaternions_xyzw: Sequence[Sequence[float]],
) -> tuple[tuple[float, float, float, float], float]:
"""Return a robust orientation and maximum angular residual in radians."""
if not quaternions_xyzw:
raise ValueError("at least one quaternion is required")
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
reference = rotations[0]
delta_vectors = (reference.inv() * rotations).as_rotvec()
median_delta = np.median(delta_vectors, axis=0)
robust = reference * Rotation.from_rotvec(median_delta)
residuals = (robust.inv() * rotations).magnitude()
maximum = float(np.max(residuals)) if residuals.size else 0.0
return (
tuple(float(value) for value in robust.as_quat()),
maximum,
)
def rotation_spread_rad(
quaternions_xyzw: Sequence[Sequence[float]],
) -> float:
"""Return the maximum geodesic residual around a robust orientation."""
_, spread = robust_rotation_summary(quaternions_xyzw)
return spread
def rotation_rms_rad(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float | None = None,
) -> float:
"""Return RMS geodesic noise around a robust orientation."""
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
if outlier_threshold_rad is not None:
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
residuals = residuals[residuals <= threshold]
if residuals.size == 0:
return float("inf")
return float(np.sqrt(np.mean(np.square(residuals))))
def rotation_inlier_fraction(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float,
) -> float:
"""Return the fraction close to the robust orientation."""
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
return float(np.mean(residuals <= threshold))
def delta_rotation_vector(
reference_xyzw: Sequence[float],
observed_xyzw: Sequence[float],
) -> np.ndarray:
reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
return (reference.inv() * observed).as_rotvec()
def fit_rotation_axis(
vectors: Sequence[Sequence[float]],
commands: Sequence[int],
) -> np.ndarray:
"""Fit and orient the single rotational axis used by one motor sweep."""
matrix = np.asarray(vectors, dtype=float)
command_values = np.asarray(commands, dtype=int)
if matrix.ndim != 2 or matrix.shape[1] != 3:
raise ValueError("vectors must have shape (N, 3)")
if command_values.shape != (matrix.shape[0],):
raise ValueError("commands must match vectors")
useful = np.linalg.norm(matrix, axis=1) > 1e-6
if int(np.count_nonzero(useful)) < 3:
raise ValueError("insufficient non-zero rotations to fit an axis")
_, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
axis = vh[0]
projections = matrix @ axis
low = projections[command_values <= 16]
high = projections[command_values >= 239]
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
axis = -axis
return axis / np.linalg.norm(axis)
def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
"""Unweighted PAVA projection onto non-increasing values."""
original = np.asarray(values, dtype=float)
if original.ndim != 1 or not np.all(np.isfinite(original)):
raise ValueError("values must be a finite vector")
negated = -original
levels: list[float] = []
weights: list[int] = []
starts: list[int] = []
for index, value in enumerate(negated):
levels.append(float(value))
weights.append(1)
starts.append(index)
while len(levels) >= 2 and levels[-2] > levels[-1]:
total_weight = weights[-2] + weights[-1]
merged = (
levels[-2] * weights[-2] + levels[-1] * weights[-1]
) / total_weight
levels[-2:] = [merged]
weights[-2:] = [total_weight]
starts.pop()
projected = np.empty_like(original)
for block_index, (level, start) in enumerate(zip(levels, starts)):
end = starts[block_index + 1] if block_index + 1 < len(starts) else len(original)
projected[start:end] = -level
return projected
def _record_rotation(record: Mapping[str, Any], pair: str) -> tuple[float, ...]:
rotations = record.get("relative_quaternion_xyzw", {})
value = rotations.get(pair)
if value is None:
raise ValueError(f"sample record is missing {pair}")
return tuple(float(component) for component in value)
@dataclass(frozen=True)
class FitResult:
joints: dict[str, dict[str, Any]]
axes: dict[str, tuple[float, float, float]]
references: dict[str, tuple[float, float, float, float]]
ip_coupling: dict[str, float]
max_monotonic_correction_rad: float
max_hysteresis_rad: float
measurement_mode: str = "rotation"
trajectory_models: dict[str, Any] = field(default_factory=dict)
trajectory_quality: dict[str, Any] = field(default_factory=dict)
def measure_from_reference(
self,
joint_name: str,
observed_quaternion_xyzw: Sequence[float],
reference_quaternion_xyzw: Sequence[float] | None = None,
) -> float:
reference = (
reference_quaternion_xyzw
if reference_quaternion_xyzw is not None
else self.references[joint_name]
)
vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
axis = np.asarray(self.axes[joint_name], dtype=float)
return float(vector @ axis)
def fit_calibration_curves(records: Iterable[Mapping[str, Any]]) -> FitResult:
"""Fit six complete 256-entry curves from dense or sparse scan records."""
samples = [
dict(record)
for record in records
if record.get("kind", "sample") == "sample"
]
if not samples:
raise ValueError("no scan records were provided")
joint_results: dict[str, dict[str, Any]] = {}
axes: dict[str, tuple[float, float, float]] = {}
references: dict[str, tuple[float, float, float, float]] = {}
maximum_correction = 0.0
maximum_hysteresis = 0.0
for joint_name, (phase, pair, motor_index) in JOINT_SPECS.items():
phase_records = [record for record in samples if record.get("phase") == phase]
if not phase_records:
raise ValueError(f"no records for phase {phase}")
cycle_references: dict[int, tuple[float, ...]] = {}
for record in phase_records:
if (
record.get("direction") == DIRECTION_DECREASING
and int(record.get("command_u8", -1)) == 255
):
cycle_references.setdefault(
int(record["cycle"]),
_record_rotation(record, pair),
)
cycles = sorted({int(record["cycle"]) for record in phase_records})
if any(cycle not in cycle_references for cycle in cycles):
raise ValueError(f"{joint_name} is missing a command-255 cycle reference")
vectors: list[np.ndarray] = []
commands: list[int] = []
indexed: list[tuple[Mapping[str, Any], np.ndarray]] = []
for record in phase_records:
cycle = int(record["cycle"])
vector = delta_rotation_vector(
cycle_references[cycle],
_record_rotation(record, pair),
)
vectors.append(vector)
commands.append(int(record["command_u8"]))
indexed.append((record, vector))
axis = fit_rotation_axis(vectors, commands)
axes[joint_name] = tuple(float(value) for value in axis)
references[joint_name] = robust_rotation_summary(
list(cycle_references.values())
)[0]
branch_values: dict[str, list[list[float]]] = {
direction: [[] for _ in range(256)] for direction in DIRECTIONS
}
for record, vector in indexed:
direction = str(record["direction"])
command = int(record["command_u8"])
branch_values[direction][command].append(float(vector @ axis))
fitted_branches: dict[str, list[float]] = {}
for direction in DIRECTIONS:
sample_commands = np.asarray(
[
command
for command, values in enumerate(branch_values[direction])
if values
],
dtype=int,
)
if (
sample_commands.size < 3
or int(sample_commands[0]) != 0
or int(sample_commands[-1]) != 255
):
raise ValueError(
f"{joint_name}.{direction} requires at least three samples "
"including commands 0 and 255"
)
raw = np.asarray(
[
float(np.median(branch_values[direction][command]))
for command in sample_commands
],
dtype=float,
)
raw -= raw[-1]
projected_samples = isotonic_nonincreasing(raw)
projected_samples -= projected_samples[-1]
correction = float(np.max(np.abs(projected_samples - raw)))
maximum_correction = max(maximum_correction, correction)
projected = np.interp(
np.arange(256, dtype=float),
sample_commands.astype(float),
projected_samples,
)
projected -= projected[255]
fitted_branches[direction] = [
round(float(value), 8) for value in projected
]
hysteresis = float(
np.max(
np.abs(
np.asarray(fitted_branches[DIRECTION_DECREASING])
- np.asarray(fitted_branches[DIRECTION_INCREASING])
)
)
)
maximum_hysteresis = max(maximum_hysteresis, hysteresis)
combined_curve = 0.5 * (
np.asarray(
fitted_branches[DIRECTION_DECREASING], dtype=float
)
+ np.asarray(
fitted_branches[DIRECTION_INCREASING], dtype=float
)
)
combined_curve -= combined_curve[255]
joint_result: dict[str, Any] = {
"motor_index": motor_index,
"angle_rad": [
round(float(value), 8) for value in combined_curve
],
"decreasing_rad": fitted_branches[DIRECTION_DECREASING],
"increasing_rad": fitted_branches[DIRECTION_INCREASING],
}
if joint_name == "thumb_ip":
joint_result["passive"] = True
joint_results[joint_name] = joint_result
mcp = joint_results["thumb_mcp"]
ip = joint_results["thumb_ip"]
x = np.asarray(mcp["angle_rad"], dtype=float)
y = np.asarray(ip["angle_rad"], dtype=float)
design = np.column_stack((x, np.ones_like(x)))
multiplier, offset = np.linalg.lstsq(design, y, rcond=None)[0]
predicted = multiplier * x + offset
residual_sum = float(np.sum((y - predicted) ** 2))
total_sum = float(np.sum((y - np.mean(y)) ** 2))
r_squared = 1.0 if total_sum < 1e-12 else 1.0 - residual_sum / total_sum
return FitResult(
joints=joint_results,
axes=axes,
references=references,
ip_coupling={
"multiplier": round(float(multiplier), 8),
"offset_rad": round(float(offset), 8),
"r_squared": round(float(r_squared), 8),
},
max_monotonic_correction_rad=maximum_correction,
max_hysteresis_rad=maximum_hysteresis,
)
def create_final_payload(
*,
serial_number: str,
fit: FitResult,
validation_errors_rad: Sequence[float],
passed: bool,
baseline: Sequence[int] = BASELINE_COMMAND,
) -> dict[str, Any]:
errors = np.abs(np.asarray(validation_errors_rad, dtype=float))
mae = float(np.mean(errors)) if errors.size else float("nan")
p95 = float(np.percentile(errors, 95)) if errors.size else float("nan")
runtime_joints: dict[str, dict[str, Any]] = {}
for joint_name, joint in fit.joints.items():
runtime_joint: dict[str, Any] = {
"motor_index": int(joint["motor_index"]),
"angle_rad": [
round(float(value), 8) for value in joint["angle_rad"]
],
}
if joint_name == "thumb_ip":
runtime_joint["passive"] = True
runtime_joints[joint_name] = runtime_joint
payload = {
"schema_version": 2,
"model": "G20",
"side": "left",
"serial_number": str(serial_number),
"angle_unit": "rad",
"command_range": [0, 255],
"zero_command_u8": 255,
"baseline_command_u8": [int(value) for value in baseline],
"joints": runtime_joints,
"ip_coupling": {
"multiplier": fit.ip_coupling["multiplier"],
"offset_rad": fit.ip_coupling["offset_rad"],
},
"quality": {
"passed": bool(passed),
"validation_mae_rad": None if not np.isfinite(mae) else round(mae, 8),
"validation_p95_rad": None if not np.isfinite(p95) else round(p95, 8),
},
}
validate_final_payload(payload)
return payload
def maximum_non_target_drift_rad(
records: Iterable[Mapping[str, Any]],
fit: FitResult,
) -> float:
"""Measure unintended active-joint motion during the two isolated scans."""
samples = [
dict(record)
for record in records
if record.get("kind", "sample") == "sample"
]
maximum = 0.0
checks = (
(PHASE_ROOT, "thumb_mcp", PAIR_MCP),
(PHASE_ROOT, "thumb_ip", PAIR_IP),
(PHASE_TIP, "thumb_cmc_pitch", PAIR_ROOT),
)
for phase, joint_name, pair in checks:
phase_records = [record for record in samples if record.get("phase") == phase]
for cycle in sorted({int(record["cycle"]) for record in phase_records}):
cycle_records = [
record for record in phase_records if int(record["cycle"]) == cycle
]
reference_record = next(
(
record
for record in cycle_records
if record.get("direction") == DIRECTION_DECREASING
and int(record.get("command_u8", -1)) == 255
),
None,
)
if reference_record is None:
continue
reference = _record_rotation(reference_record, pair)
axis = np.asarray(fit.axes[joint_name], dtype=float)
for record in cycle_records:
drift = abs(
float(
delta_rotation_vector(
reference,
_record_rotation(record, pair),
)
@ axis
)
)
maximum = max(maximum, drift)
return maximum
def validate_final_payload(payload: Mapping[str, Any]) -> None:
"""Validate the deliberately small runtime JSON schema."""
if payload.get("schema_version") != 2:
raise ValueError("schema_version must be 2")
if payload.get("model") != "G20" or payload.get("side") != "left":
raise ValueError("payload must describe a left G20")
if payload.get("angle_unit") != "rad":
raise ValueError("angle_unit must be rad")
baseline = payload.get("baseline_command_u8")
if not isinstance(baseline, list) or len(baseline) != 20:
raise ValueError("baseline_command_u8 must contain 20 values")
joints = payload.get("joints")
if not isinstance(joints, Mapping) or set(joints) != set(JOINT_SPECS):
raise ValueError("payload must contain exactly the three thumb joints")
for joint_name, joint in joints.items():
expected_motor = JOINT_SPECS[joint_name][2]
if int(joint.get("motor_index", -1)) != expected_motor:
raise ValueError(f"{joint_name} has the wrong motor index")
curve = joint.get("angle_rad")
if not isinstance(curve, list) or len(curve) != 256:
raise ValueError(
f"{joint_name}.angle_rad must contain 256 values"
)
values = np.asarray(curve, dtype=float)
if not np.all(np.isfinite(values)):
raise ValueError(
f"{joint_name}.angle_rad contains non-finite values"
)
if np.any(np.diff(values) > 1e-7):
raise ValueError(
f"{joint_name}.angle_rad must be non-increasing"
)
if abs(float(values[255])) > 1e-6:
raise ValueError(
f"{joint_name}.angle_rad[255] must be zero"
)
if joints["thumb_ip"].get("passive") is not True:
raise ValueError("thumb_ip must be marked passive")
@@ -0,0 +1,14 @@
# 历史兼容区
此目录保留旧格式读写、旧布局、历史数据诊断及必要的离线工具,不参与正式在线调度。
四型号的 runner/node/pipeline 已由 `runtime/runner.py`、`runtime/session.py` 和
`runtime/artifacts/finalization.py` 替代。只保留仍被历史工具调用的入口;
L6/O6 无调用的 pipeline 包装已删除。
旧独立断点实现、低速预检和在线节点已移除。旧发布函数已拒绝更新正式发布指针。
这里生成的离线诊断文件不能作为标准 URDF 已验收的证据;正式回放使用
`calibrate_hand --config <product.yaml> --offline-raw <raw_samples.jsonl>`。
不要在此目录增加新型号。新型号提供 Profile、产品 YAML、原始 CAD/mesh;仅新 SDK 协议增加 Adapter。
需要恢复已删除的历史实现时,使用工作区
`calibration_output/refactor_backup.4WRWNn/` 中的归档,不要重新接入生产入口。
@@ -0,0 +1 @@
"""Read-only compatibility for archived sessions and diagnostic tools."""
@@ -0,0 +1,17 @@
"""Model- and side-specific calibration policies."""
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import (
EngineBindings,
ProfileRegistry,
RegisteredProfile,
get_default_registry,
)
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.runtime_schema import validate_schema_v6_runtime_payload
__all__ = [
"EngineBindings",
"ProfileRegistry",
"RegisteredProfile",
"get_default_registry",
"validate_schema_v6_runtime_payload",
]
@@ -0,0 +1,3 @@
"""Compatibility imports for the G20 model package."""
from linkerhand_calibration.models.g20 import register_profiles
__all__ = ["register_profiles"]
@@ -0,0 +1,4 @@
"""Compatibility import for g20._adapter."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20._adapter")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.artifacts."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.artifacts")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.command_layout."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.command_layout")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.golden_regression."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.golden_regression")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.legacy_11."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.legacy_11")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.motion."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.motion")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.offline_replay."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.offline_replay")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.profile."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.profile")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.publication."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.publication")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.reporting_zh."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.reporting_zh")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.right_19."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.right_19")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.runner."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.runner")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.urdf_input."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.urdf_input")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.zero_policy."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.zero_policy")
@@ -0,0 +1,4 @@
"""Compatibility import for g20.zero_solver."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.g20.zero_solver")
@@ -0,0 +1,3 @@
"""Compatibility imports for the L6 model package."""
from linkerhand_calibration.models.l6 import register_profiles
__all__ = ["register_profiles"]
@@ -0,0 +1,4 @@
"""Compatibility import for l6.artifacts."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.l6.artifacts")
@@ -0,0 +1,4 @@
"""Compatibility import for l6.fitting."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.l6.fitting")
@@ -0,0 +1,4 @@
"""Compatibility import for l6.left_transfer."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.l6.left_transfer")
@@ -0,0 +1,4 @@
"""Compatibility import for l6.motion."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.l6.motion")
@@ -0,0 +1,4 @@
"""Compatibility import for l6.profile."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.l6.profile")
@@ -0,0 +1,4 @@
"""Compatibility import for l6.runner."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.l6.runner")
@@ -0,0 +1,4 @@
"""Compatibility import for l6.urdf."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.l6.urdf")
@@ -0,0 +1,3 @@
"""Compatibility imports for the O12 model package."""
from linkerhand_calibration.models.o12 import register_profiles
__all__ = ["register_profiles"]
@@ -0,0 +1,4 @@
"""Compatibility import for o12.artifacts."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.artifacts")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.fitting."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.fitting")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.health."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.health")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.kinematics."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.kinematics")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.motion."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.motion")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.observations."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.observations")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.pipeline."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.pipeline")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.pnp."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.pnp")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.profile."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.profile")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.quality."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.quality")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.runner."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.runner")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.urdf."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.urdf")
@@ -0,0 +1,4 @@
"""Compatibility import for o12.zero."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o12.zero")
@@ -0,0 +1,3 @@
"""Compatibility imports for the O6 model package."""
from linkerhand_calibration.models.o6 import register_profiles
__all__ = ["register_profiles"]
@@ -0,0 +1,4 @@
"""Compatibility import for o6.artifacts."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o6.artifacts")
@@ -0,0 +1,4 @@
"""Compatibility import for o6.fitting."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o6.fitting")
@@ -0,0 +1,4 @@
"""Compatibility import for o6.left_transfer."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o6.left_transfer")
@@ -0,0 +1,4 @@
"""Compatibility import for o6.profile."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o6.profile")
@@ -0,0 +1,4 @@
"""Compatibility import for o6.runner."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o6.runner")
@@ -0,0 +1,4 @@
"""Compatibility import for o6.urdf."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.o6.urdf")
@@ -0,0 +1,4 @@
"""Compatibility import for the shared model registry."""
from importlib import import_module
import sys
sys.modules[__name__] = import_module("linkerhand_calibration.models.registry")
@@ -0,0 +1,25 @@
"""Dispatch model-specific runtime artifact validation by profile identity."""
from __future__ import annotations
from typing import Any, Mapping
from linkerhand_calibration.core import ProfileKey
def validate_schema_v6_runtime_payload(payload: Mapping[str, Any]) -> None:
key = ProfileKey.parse(str(payload.get("profile_id", "")))
if key.model == "L6":
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.artifacts import validate_l6_runtime_payload
validate_l6_runtime_payload(payload)
return
if key.model == "O6":
from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.artifacts import validate_o6_runtime_payload
validate_o6_runtime_payload(payload)
return
raise ValueError(f"schema v6 is unsupported for {key.profile_id}")
__all__ = ["validate_schema_v6_runtime_payload"]
@@ -0,0 +1,251 @@
"""Read-only renderers for old recorded status payloads, not live runners."""
from __future__ import annotations
from typing import Any, Mapping
from ...operator_report import ProgressEstimator
from ...runtime.reporting import reason_zh, render_progress_zh
common_reason_zh = reason_zh
render_six_channel_progress_zh = render_progress_zh
_L6_TASK_LABELS = {
"thumb_roll_top": "拇指 CMC roll(上方机位,ID6→ID7)",
"thumb_pitch_dip_front": "拇指 CMC pitch / DIP(正面机位,ID0→ID1→ID2)",
"pinky_pitch_dip_side": "小指 MCP pitch / DIP(侧面机位,ID3→ID4→ID5)",
}
def _l6_reason_zh(
status: Mapping[str, Any], *, model_name: str = "L6"
) -> tuple[str, str, str]:
return common_reason_zh(status, model_name=model_name)
def render_l6_progress_zh(
status: Mapping[str, Any],
estimator: ProgressEstimator | None = None,
) -> str:
"""Render L6 progress in the same operator-oriented layout as G20."""
return render_six_channel_progress_zh(
status,
task_labels=_L6_TASK_LABELS,
reason_renderer=_l6_reason_zh,
estimator=estimator,
)
_O6_TASK_LABELS = {
"thumb_yaw_top": "拇指 CMC yaw(上面机位,ID6→ID7)",
"thumb_pitch_ip_front": "拇指 CMC pitch / IP(正面机位,ID0→ID1→ID2)",
"pinky_pitch_dip_side": "小指 MCP pitch / DIP(侧面机位,ID3→ID4→ID5)",
}
def render_o6_progress_zh(
status: dict[str, Any],
estimator: ProgressEstimator | None = None,
) -> str:
return render_progress_zh(
status,
task_labels=_O6_TASK_LABELS,
reason_renderer=lambda value: reason_zh(
value, model_name="O6"
),
estimator=estimator,
)
_O12_TASK_LABELS = {
"thumb_pitch_front": "拇指 CMC pitch(正面 ID0→ID1)",
"thumb_roll_front": "拇指 CMC roll(正面 ID0→ID1)",
"thumb_mcp_dip_front": "拇指 MCP / 被动 DIP(正面 ID1→ID2→ID3)",
"thumb_yaw_top": "拇指 CMC yaw(顶部 ID14→ID15)",
"pinky_chain_side": "小指 MCP / 被动 PIP/DIP(侧面 ID4→ID5→ID6→ID7)",
"middle_roll_front": "中指 MCP roll(正面 ID0→ID12 + 侧面 ID4→ID8)",
"middle_mcp_side": "中指 MCP pitch(侧面 ID4→ID8)",
"middle_pip_dip_side": "中指 PIP / 被动 DIP(侧面 ID8→ID9)",
"index_roll_front": "食指 MCP roll(正面 ID0→ID13 + 侧面 ID4→ID10)",
"index_mcp_side": "食指 MCP pitch(侧面 ID4→ID10)",
"index_pip_dip_side": "食指 PIP / 被动 DIP(侧面 ID10→ID11)",
}
def _o12_reason_zh(status):
reason = str(status.get("reason", ""))
if reason.startswith("sweep_quality_failed:"):
details = reason.split(":", 2)[-1]
if any(
item in details
for item in ("side_frames=", "side_bins=", "side_maximum_gap=")
):
return (
"O12-CROSS-VIEW-SAMPLING-104",
"侧摆任务的主视角数据已采到,但辅助侧面视角在本段轨迹内"
f"保留的时序覆盖不足:{details}。这不表示 Tag 被遮挡。",
"程序会保留已通过断点;重新启动后只重做该扫描单元。"
"若 Tag 持续可见,优先检查侧面 AprilTag 节点的发布频率和 CPU 调度。",
)
if reason.startswith("mapping_preflight_no_target_motion:"):
task = reason.split(":", 1)[1]
return (
"O12-MAPPING-301",
f"固定 SDK 映射点动时没有观察到目标关节运动:{task}。",
"检查对应 Tag、通道映射和机械连接;不要交换通道后强行继续。",
)
if reason.startswith("mapping_preflight_wrong_feedback_direction:"):
task = reason.split(":", 1)[1]
return (
"O12-MAPPING-302",
f"固定 SDK 映射点动的目标反馈方向不正确:{task}。",
"检查对应 SDK 通道和机械连接;不要交换通道后强行继续。",
)
if reason.startswith("o12_active_motor_fault"):
return (
"O12-HEALTH-201",
f"O12 返回活动电机故障:{status.get('error_faults', [])}。",
"检查对应电机的堵转、过流、过热或电机异常;排除后从断点启动。",
)
if reason.startswith("o12_new_communication_fault"):
return (
"O12-HEALTH-202",
f"运行中出现了新的 O12 通信异常:{status.get('error_faults', [])}。",
"检查 HCAN 线缆、供电和对应电机通信;程序已保持当前位置。",
)
if reason.startswith("o12_feedback_stream_timeout"):
return (
"O12-HEALTH-203",
"O12 命令触发反馈流中断超过 1 秒。",
"检查 HCAN 连接、SDK 节点和供电;恢复后从断点启动。",
)
if reason.startswith("feedback_outside_registered_feedback_domain:"):
return (
"O12-FEEDBACK-202",
"O12 反馈超出了已登记的物理反馈范围。",
"保持当前位置,检查诊断中的通道、反馈值和机械端点。",
)
if reason == "calibration_node_status_timeout":
return (
"O12-NODE-500",
"O12 标定节点已停止发布状态,运行栈将自动退出。",
"查看本次 calibration.log 中的 Python traceback,并从断点重启。",
)
return reason_zh(status, model_name="O12")
def render_o12_progress_zh(status, estimator=None) -> str:
"""Render O12 in the same operator-oriented layout as G20 and L6."""
text = render_progress_zh(
status,
task_labels=_O12_TASK_LABELS,
reason_renderer=_o12_reason_zh,
estimator=estimator,
)
temperature = (
"直接温度回读"
if status.get("temperature_report_verified")
else "错误码 bit1 过热保护"
if status.get("temperature_fallback_active")
else "等待温度能力确认"
)
resume = status.get("resume", {})
resume_text = ""
if isinstance(resume, dict) and resume.get("used"):
resume_text = (
"\n断点恢复:已复用 "
f"{int(resume.get('completed_unit_count', 0))} 个扫描单元;"
f"来源 {resume.get('source_session', '-')}"
)
cross_view = status.get("roll_cross_view", {})
cross_view_text = ""
if isinstance(cross_view, dict) and cross_view.get("active"):
recognized = "/".join(
f"ID{int(value)}"
for value in cross_view.get("recognized_tag_ids", ())
) or "无"
missing = "/".join(
f"ID{int(value)}"
for value in cross_view.get("unrecognized_tag_ids", ())
) or "无"
cross_view_text = (
"\n侧摆双机位:侧面已识别 "
f"{recognized};未识别/不合格 {missing};联合 "
f"{int(cross_view.get('valid_frames', 0))}/"
f"{int(cross_view.get('total_frames', 0))} 帧("
f"{float(cross_view.get('joint_frame_rate', 0.0)):.1%})"
)
coupling = status.get("feedback_coupling", {})
coupling_text = ""
if isinstance(coupling, dict) and coupling.get("active"):
stage = {
"training_observation": "训练采集",
"holdout_pending_full_sweep_validation": "第四轮整段验证",
"vendor_solver_motion_prior": "运动准备",
}.get(str(coupling.get("reference_kind", "")), "耦合观测")
coupling_text = (
"\nO12耦合:"
f"{coupling.get('coupled_channel', '-')} 位移 "
f"{float(coupling.get('displacement_rad', 0.0)):.3f}/"
f"{float(coupling.get('hard_displacement_limit_rad', 0.0)):.3f} rad;"
f"{stage};vendor偏差 "
f"{float(coupling.get('residual_rad', 0.0)):.3f} rad(仅诊断)"
)
auxiliary = status.get("auxiliary_tracking", {})
auxiliary_text = ""
if isinstance(auxiliary, dict) and auxiliary.get("active"):
channels = list(auxiliary.get("channels", ()))
worst = max(
channels,
key=lambda item: float(item.get("error_rad", 0.0)),
default={},
)
stage = (
"平滑过渡中"
if auxiliary.get("mode") == "transitioning"
else "已稳定保持"
if auxiliary.get("ready")
else "等待稳定"
)
auxiliary_text = (
"\nO12避让轴:"
f"{stage};最大偏差 {worst.get('channel', '-')}="
f"{float(worst.get('error_rad', 0.0)):.3f} rad"
)
locked_ids = list(status.get("locked_reference_tag_ids", ()))
locked_reference_text = ""
if locked_ids:
locked_reference_text = (
"\n固定基准:"
+ "/".join(f"ID{int(value)}" for value in locked_ids)
+ " 已锁定,避让遮挡期间复用"
)
error_health = str(
status.get("error_health_classification", "awaiting_error_report")
)
if error_health == "historical_communication_latch":
channels = "/".join(
str(value) for value in status.get(
"confirmed_historical_communication_channels", ()
)
) or "未知通道"
error_health_text = f"历史通信位已核验({channels})"
elif error_health == "confirming_historical_communication":
error_health_text = (
"确认历史通信位 "
f"{int(status.get('error_report_matching_count', 0))}/3"
)
elif error_health == "clear":
error_health_text = "正常"
else:
error_health_text = error_health
return (
text + cross_view_text + coupling_text + auxiliary_text
+ locked_reference_text + resume_text + (
"\nO12安全:POSITION="
f"{bool(status.get('position_mode_verified'))};"
f"错误码通道={bool(status.get('error_report_verified'))}"
f"({error_health_text});"
f"温度策略={temperature};速度倍率="
f"{float(status.get('motion_speed_scale', 1.0)):.1f}x;"
f"控制发布={float(status.get('command_publish_hz', 0.0)):.1f} Hz"
)
)
@@ -0,0 +1,283 @@
"""Read-only helpers retained for archived runner diagnostics; no live loop."""
from __future__ import annotations
import json
import os
from pathlib import Path
from typing import Any, Mapping
from ...product import ProductConfig, sha256_file
from ...runtime.engine import ACQUISITION_POLICY_VERSION
STATUS_TIMEOUT_SECONDS = 90.0
FITTING_STATUS_TIMEOUT_SECONDS = 600.0
def _status_timeout_seconds(status: Mapping[str, Any]) -> float:
"""Return the watchdog deadline for the node's current phase.
Motion and acquisition are expected to publish twice a second and retain
the strict transport watchdog. The final 3-D fit is intentionally a
synchronous, CPU-bound operation, so its executor cannot service the
status timer until the fit returns. The node publishes an explicit
FITTING status immediately before entering that operation; allow that
known phase enough time without weakening motion safety checks.
"""
if str(status.get("state", "")) == "FITTING":
return FITTING_STATUS_TIMEOUT_SECONDS
return STATUS_TIMEOUT_SECONDS
def _calibration_node_exited_before_status(log_path: Path) -> bool:
"""Detect a launch child crash while the parent launch is still alive."""
try:
with log_path.open("rb") as stream:
stream.seek(0, os.SEEK_END)
size = stream.tell()
stream.seek(max(0, size - 65536), os.SEEK_SET)
tail = stream.read().decode("utf-8", errors="replace")
except OSError:
return False
return (
"[three_camera_calibration_node-" in tail
and "]: process has died" in tail
)
def _launch_command(
config: ProductConfig,
session: Path,
*,
resume_from: Path | None = None,
recalibration_scope: str = "full",
) -> list[str]:
values = {
"model": config.model,
"hand_type": config.side,
"tag_layout": config.tag_layout,
"serial_number": config.serial_number,
"can_interface": config.can_interface,
"session_dir": str(session),
"output_root": str(config.output_root),
"camera_extrinsics_file": str(config.camera_extrinsics),
"source_urdf_path": str(config.source_urdf),
"source_urdf_expected_sha256": config.source_urdf_sha256,
"camera_extrinsics_expected_sha256": config.camera_extrinsics_sha256,
"calibration_config_expected_sha256": config.calibration_config_sha256,
"tag_config_expected_sha256": config.tag_config_sha256,
"profile_config_expected_sha256": getattr(
config, "profile_config_sha256", ""
),
"corrected_urdf_output_dir": str(session),
"calibration_config": str(config.calibration_config),
"tag_config": str(config.tag_config),
"commands_enabled": "true",
"start_cameras": "true",
"start_sdk": "true",
"record_bag": "false",
"validation_enabled": "false",
"recalibration_scope": recalibration_scope,
}
if resume_from is not None:
values["resume_raw_samples_path"] = str(
resume_from / "raw_samples.jsonl"
)
for view, camera in config.cameras.items():
values[f"{view}_camera_serial"] = camera["serial_number"]
values[f"{view}_camera_name"] = camera["camera_name"]
values[f"{view}_camera_info_url"] = camera["camera_info"]
return [
"ros2",
"launch",
"linkerhand_calibration",
"unified_calibration.launch.py",
*(f"{name}:={value}" for name, value in values.items()),
]
def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
"""Return the newest compatible failed attempt, never a passed session.
Do not trust only ``latest_attempt``. A process interrupted during the
device-only startup gate may have already moved that pointer while still
containing no ``session_start`` checkpoint. In that case walk backwards
to the preceding usable failed session instead of throwing away hours of
completed tasks.
"""
root = config.session_root
try:
resolved_root = root.resolve(strict=True)
except OSError:
return None
candidates: list[Path] = []
pointer = config.session_root / "latest_attempt"
if pointer.exists():
try:
candidates.append(pointer.resolve(strict=True))
except OSError:
pass
try:
candidates.extend(
sorted(
(
path
for path in root.iterdir()
if path.is_dir() and not path.name.startswith("latest_")
),
key=lambda path: path.name,
reverse=True,
)
)
except OSError:
return None
passed_pointer = config.session_root / "latest_passed"
passed: Path | None = None
if passed_pointer.exists():
try:
passed = passed_pointer.resolve(strict=True)
except OSError:
pass
seen: set[Path] = set()
for unresolved in candidates:
try:
candidate = unresolved.resolve(strict=True)
except OSError:
continue
if candidate in seen:
continue
seen.add(candidate)
if candidate.parent != resolved_root or not candidate.is_dir():
continue
# A failed attempt older than the current formal release is stale and
# must not seed a new independent calibration.
if passed is not None and candidate.name <= passed.name:
continue
raw_path = candidate / "raw_samples.jsonl"
if not raw_path.is_file():
continue
summary_path = candidate / "calibration_summary_zh.json"
summary: dict[str, Any] | None = None
if summary_path.is_file():
try:
loaded = json.loads(summary_path.read_text(encoding="utf-8"))
except (OSError, json.JSONDecodeError):
continue
if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
continue
summary = loaded
hashes = summary.get("hashes", {})
if not isinstance(hashes, Mapping):
continue
if (
str(hashes.get("source_urdf_sha256", ""))
!= config.source_urdf_sha256
or str(hashes.get("camera_extrinsics_sha256", ""))
!= config.camera_extrinsics_sha256
):
continue
start: dict[str, Any] | None = None
try:
with raw_path.open("r", encoding="utf-8") as stream:
for line in stream:
if not line.strip():
continue
value = json.loads(line)
if (
isinstance(value, dict)
and value.get("kind") == "session_start"
):
start = value
break
except (OSError, json.JSONDecodeError):
continue
if (
start is None
or start.get("acquisition_policy_version")
!= ACQUISITION_POLICY_VERSION
or start.get("hand_type") != config.side
or start.get("tag_layout") != config.tag_layout
or start.get("source_urdf_sha256")
!= config.source_urdf_sha256
or (
bool(start.get("protected_hashes"))
and dict(start.get("protected_hashes", {}))
!= {
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"tag_config_sha256": config.tag_config_sha256,
"profile_config_sha256": config.profile_config_sha256,
}
)
):
continue
if summary is None:
# Ctrl+C can terminate the ROS launch tree before the wrapper gets
# a chance to create calibration_summary_zh.json. The immutable
# checkpoint itself is enough to resume only after independently
# proving that its external geometry still matches the product.
try:
checkpoint_extrinsics = Path(
str(start["camera_extrinsics_file"])
).expanduser().resolve(strict=True)
if sha256_file(checkpoint_extrinsics) != (
config.camera_extrinsics_sha256
):
continue
except (KeyError, OSError, ValueError):
continue
return candidate
return None
def _resolve_partial_base_session(
config: ProductConfig, base_session: str | Path | None
) -> Path:
"""Validate the complete passed session that donates non-target tasks."""
if base_session is None or not str(base_session).strip():
raise ValueError(
"partial scope requires --base-session pointing to a passed "
"complete G20 right session"
)
candidate = Path(base_session).expanduser().resolve(strict=True)
root = config.session_root.resolve()
if candidate.parent != root or not candidate.is_dir():
raise ValueError(
"base session must resolve to a direct session directory under "
f"{root}"
)
raw_path = candidate / "raw_samples.jsonl"
summary_path = candidate / "calibration_summary_zh.json"
payload_path = (
candidate
/ f"g20_right_{config.serial_number}_calibration.json"
)
for required in (raw_path, summary_path, payload_path):
if not required.is_file():
raise ValueError(f"base session is missing required artifact: {required}")
try:
summary = json.loads(summary_path.read_text(encoding="utf-8"))
except json.JSONDecodeError as error:
raise ValueError("base session summary is invalid JSON") from error
if (
not isinstance(summary, dict)
or summary.get("result") != "PASS"
or not bool(summary.get("quality", {}).get("passed"))
):
raise ValueError("base session is not a formally passed session")
hashes = summary.get("hashes", {})
expected_hashes = {
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
}
if not isinstance(hashes, Mapping) or any(
str(hashes.get(name, "")) != expected
for name, expected in expected_hashes.items()
):
raise ValueError(
"base session source URDF, camera extrinsics or calibration "
"configuration differs from the current product"
)
return candidate
@@ -0,0 +1,52 @@
"""Path compatibility for immutable v1 product configurations."""
from __future__ import annotations
from pathlib import Path
_LEGACY_SOURCE_PREFIX = Path("src/g20_thumb_apriltag_calibration")
_CURRENT_SOURCE_PREFIX = Path("src/linkerhand_calibration")
def _resolve_package_uri(value: str, workspace: Path) -> Path | None:
prefix = "package://"
if not value.startswith(prefix):
return None
package_name, separator, relative = value[len(prefix) :].partition("/")
if not separator or not package_name or not relative:
raise ValueError(f"invalid ROS package resource path: {value}")
workspace_candidate = (workspace / "src" / package_name / relative).resolve()
if workspace_candidate.exists():
return workspace_candidate
try:
from ament_index_python.packages import get_package_share_directory
package_share = Path(get_package_share_directory(package_name))
except Exception:
return workspace_candidate
return (package_share / relative).resolve()
def resolve_renamed_package_path(value: str | Path, workspace: Path) -> Path:
"""Resolve workspace paths, ROS package resources and the former prefix.
Deployed v1 product YAML files are kept byte-for-byte stable because the
artifact paths participate in operational review. Existing paths always
win; package URIs prefer a source-workspace copy, and the rename mapping
is used only when the literal legacy path no longer exists.
"""
text = str(value).strip()
package_resource = _resolve_package_uri(text, workspace)
if package_resource is not None:
return package_resource
raw = Path(text).expanduser()
candidate = raw if raw.is_absolute() else workspace / raw
candidate = candidate.resolve()
if candidate.exists() or raw.is_absolute():
return candidate
try:
suffix = raw.relative_to(_LEGACY_SOURCE_PREFIX)
except ValueError:
return candidate
return (workspace / _CURRENT_SOURCE_PREFIX / suffix).resolve()
@@ -0,0 +1,92 @@
"""Hardware- and model-independent calibration kernel."""
from .domain import (
AcquisitionPolicy,
ArtifactPolicy,
AcquisitionStatus,
CalibrationProfile,
CalibrationResult,
JointMapping,
JointLimitPolicy,
CalibrationStatus,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
MotionStatus,
PauseStatus,
ProfileKey,
ProfileValidationError,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskStatus,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
validate_profile,
)
from .domain.task import (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
DIRECTIONS,
PHASE_ROOT,
PHASE_TIP,
)
from .fitting import FitResult, isotonic_nonincreasing
from .geometry import (
delta_rotation_vector,
fit_rotation_axis,
image_plane_tag_quaternion_xyzw,
normalize_quaternion_xyzw,
relative_quaternion_xyzw,
robust_rotation_summary,
rotation_inlier_fraction,
rotation_rms_rad,
rotation_spread_rad,
)
__all__ = [
"AcquisitionPolicy",
"ArtifactPolicy",
"AcquisitionStatus",
"CalibrationProfile",
"CalibrationResult",
"JointMapping",
"JointLimitPolicy",
"CalibrationStatus",
"CommandLayout",
"DIRECTION_DECREASING",
"DIRECTION_INCREASING",
"DIRECTIONS",
"FitResult",
"MeasurementPolicy",
"MeasurementSpec",
"MotionPolicy",
"MotionStatus",
"PauseStatus",
"PHASE_ROOT",
"PHASE_TIP",
"ProfileKey",
"ProfileValidationError",
"QualityPolicy",
"ScopePolicy",
"TagSpec",
"TaskStatus",
"TaskSpec",
"ViewSpec",
"VisionRigSpec",
"ZeroSolvePolicy",
"delta_rotation_vector",
"fit_rotation_axis",
"image_plane_tag_quaternion_xyzw",
"isotonic_nonincreasing",
"normalize_quaternion_xyzw",
"relative_quaternion_xyzw",
"robust_rotation_summary",
"rotation_inlier_fraction",
"rotation_rms_rad",
"rotation_spread_rad",
"validate_profile",
]
@@ -0,0 +1 @@
"""Storage helpers shared by calibration artifact writers."""
@@ -0,0 +1,525 @@
"""Indexed compressed storage for large calibration evidence records.
The JSONL file remains the ordered, crash-safe journal. Small control records
stay inline. Large records are stored in independently compressed blocks and
the JSONL line contains the scalar resume index plus a content-bound reference.
Readers can either inspect that index or reconstruct the original record.
"""
from __future__ import annotations
from collections.abc import Iterable, Mapping
from dataclasses import dataclass
from functools import lru_cache
import hashlib
import json
import os
from pathlib import Path
import shutil
import zlib
FORMAT = "linkerhand_indexed_evidence_v1"
REFERENCE_KEY = "$evidence_record"
FIELDS_KEY = "$record_fields"
SCHEMA_KEY = "$record_schema"
CANONICAL_SHA256_KEY = "$record_canonical_sha256"
DEFAULT_THRESHOLD_BYTES = 16 * 1024
MAXIMUM_BLOCK_BYTES = 4 * 1024 * 1024
INDEXED_SCALAR_FIELDS = {
"kind", "profile_id", "serial_number", "task_name", "joint",
"observation_joint", "view", "image_stamp_ns", "cycle", "direction",
"segment_key", "attempt", "sample_phase", "passed", "status",
"motion_version", "session_epoch", "reference_path_index",
"observer_preparation_id", "preparation_witness_joint", "tag_role",
"motor_index", "steady_index",
"steady_target", "command_u8", "command_rad", "feedback_u8",
"feedback_rad", "command_index", "branch_revision",
}
DETAILED_RECORD_KINDS = {
"motion_branch_observation", "image_observation_frame",
"pnp_candidate_frame", "pnp_branch_event", "joint_sample",
"secondary_joint_sample", "joint_zero_sample",
"visual_motion_observation", "diagnostic_observation_frame",
}
CANONICAL_INDEX_KINDS = {
"motion_branch_observation", "motion_branch_initialization",
"image_observation_frame", "pnp_candidate_frame", "joint_zero_sample",
"joint_zero_motion_reference_verified", "parent_reference_verified",
"observer_preparation_decision", "observer_preparation_segment_complete",
"observer_preparation_excursion_started", "observer_preparation_cancelled_before_excursion",
"capture_scope_extension", "capture_observation_revision",
"passed_tasks_reused", "passed_pose_models_restored", "staged_pose_models_restored",
"resume_verification", "joint_zero_recovery", "visual_motion_observation",
"visual_motion_settled", "task_observation_state",
}
@dataclass(frozen=True, slots=True)
class EvidenceRecordReference:
offset: int
compressed_length: int
block_sha256: str
record_offset: int
record_length: int
record_sha256: str
archive: str = ""
@classmethod
def from_mapping(cls, value):
common = {"version", "offset", "compressed_length", "block_sha256",
"record_offset", "record_length", "record_sha256"}
version = value.get("version")
if ((version == 1 and set(value) != common)
or (version == 2 and set(value) != common | {"archive"})
or version not in {1, 2}):
raise ValueError("invalid_compressed_evidence_reference")
integers = [value[key] for key in ("offset", "compressed_length", "record_offset", "record_length")]
if any(type(item) is not int or item < 0 for item in integers) or integers[1] < 1 or integers[3] < 1:
raise ValueError("invalid_compressed_evidence_reference")
archive = value.get("archive", "")
if archive and (Path(archive).name != archive or archive in {".", ".."}):
raise ValueError("invalid_compressed_evidence_reference")
return cls(*(value[key] for key in (
"offset", "compressed_length", "block_sha256", "record_offset",
"record_length", "record_sha256")), archive)
def as_mapping(self):
value = {"version": 2 if self.archive else 1, "offset": self.offset,
"compressed_length": self.compressed_length, "block_sha256": self.block_sha256,
"record_offset": self.record_offset, "record_length": self.record_length,
"record_sha256": self.record_sha256}
if self.archive:
value["archive"] = self.archive
return value
def manifest_path(journal_path: str | Path) -> Path:
path = Path(journal_path)
return path.with_name(path.stem + ".storage.json")
def archive_path(journal_path: str | Path) -> Path:
path = Path(journal_path)
return path.with_name(path.stem + ".evidence.zlib")
def initialize_evidence_journal(journal_path: str | Path, *,
threshold_bytes=DEFAULT_THRESHOLD_BYTES, compression_level=1) -> None:
"""Enable indexed storage for a new journal without changing old captures."""
journal = Path(journal_path)
marker = manifest_path(journal)
if marker.exists():
payload = json.loads(marker.read_text(encoding="utf-8"))
if payload.get("format") != FORMAT:
raise ValueError("unsupported_evidence_journal_format")
return
if journal.exists() and journal.stat().st_size:
raise ValueError("indexed_evidence_requires_new_or_migrated_journal")
journal.parent.mkdir(parents=True, exist_ok=True)
if type(compression_level) is not int or not 1 <= compression_level <= 9:
raise ValueError("invalid_evidence_compression_level")
from .storage import atomic_write_json
atomic_write_json(marker, {
"format": FORMAT,
"archive": archive_path(journal).name,
"codec": f"zlib_level_{compression_level}_independent_blocks",
"compression_level": compression_level,
"envelope_fields": "archive_bound_v2",
"field_schemas": {},
"large_record_threshold_bytes": int(threshold_bytes),
"maximum_uncompressed_block_bytes": MAXIMUM_BLOCK_BYTES,
})
_journal_configuration.cache_clear()
@lru_cache(maxsize=128)
def _journal_configuration(journal_path: str) -> dict | None:
marker = manifest_path(journal_path)
if not marker.is_file():
return None
try:
value = json.loads(marker.read_text(encoding="utf-8"))
except (OSError, ValueError, TypeError):
return None
return value if value.get("format") == FORMAT else None
def is_indexed_journal(journal_path: str | Path) -> bool:
return _journal_configuration(str(Path(journal_path).resolve())) is not None
def _resume_index(record: Mapping) -> dict:
return {key: record[key] for key in INDEXED_SCALAR_FIELDS if key in record
and (record[key] is None or isinstance(record[key], (str, int, float, bool)))}
def _schema_identity(fields) -> str:
return hashlib.sha256(json.dumps(list(fields), separators=(",", ":")).encode()).hexdigest()[:16]
@lru_cache(maxsize=512)
def _schema_fields(journal_path: str | Path, identity: str):
marker = _journal_configuration(str(Path(journal_path).resolve())) or {}
fields = marker.get("field_schemas", {}).get(identity)
if not isinstance(fields, list) or not all(isinstance(field, str) for field in fields):
raise ValueError("compressed_evidence_schema_missing")
if _schema_identity(fields) != identity:
raise ValueError("compressed_evidence_schema_changed")
return tuple(fields)
def _envelope(record: Mapping, reference, schema_identity) -> dict:
indexed = _resume_index(record)
if REFERENCE_KEY in indexed or FIELDS_KEY in indexed or SCHEMA_KEY in indexed:
raise ValueError("calibration_record_uses_reserved_storage_field")
# record_sha256 binds the complete archived object. Repeating its field
# names in every one-line resume index added tens of MB without adding an
# independent integrity check.
result = {REFERENCE_KEY: reference, SCHEMA_KEY: schema_identity, **indexed}
if record.get("kind") in CANONICAL_INDEX_KINDS:
result[CANONICAL_SHA256_KEY] = hashlib.sha256(json.dumps(record, sort_keys=True,
separators=(",", ":"), allow_nan=False).encode()).hexdigest()
return result
def _write_all(stream, payload: bytes) -> None:
written = stream.write(payload)
if written != len(payload):
raise OSError("incomplete_evidence_archive_write")
def append_indexed_records(journal_path: str | Path, records: Iterable[Mapping], *, durable: bool) -> None:
"""Append records in order; evidence is durable before its references."""
journal = Path(journal_path)
marker = _journal_configuration(str(journal.resolve()))
if marker is None:
raise ValueError("unsupported_evidence_journal_format")
threshold = int(marker["large_record_threshold_bytes"])
compression_level = int(marker.get("compression_level", 1))
if not 1 <= compression_level <= 9:
raise ValueError("invalid_evidence_compression_level")
prepared = []
imported = {}
for record in records:
if REFERENCE_KEY in record or FIELDS_KEY in record or SCHEMA_KEY in record:
raise ValueError("calibration_record_uses_reserved_storage_field")
reference = getattr(record, "reference", None)
field_names = getattr(record, "field_names", None)
fields = tuple(record) if field_names is None else tuple(field_names)
schema_identity = _schema_identity(fields)
if isinstance(reference, EvidenceRecordReference):
imported[len(prepared)] = (record, schema_identity)
line = None
else:
line = json.dumps(record, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
prepared.append((record, line, schema_identity, fields))
if not prepared:
return
schemas = dict(marker.get("field_schemas", {}))
changed = False
for _, _, identity, fields in prepared:
existing = schemas.get(identity)
if existing is not None and tuple(existing) != fields:
raise ValueError("compressed_evidence_schema_collision")
if existing is None:
schemas[identity] = list(fields)
changed = True
if changed:
marker = {**marker, "field_schemas": schemas}
from .storage import atomic_write_json
atomic_write_json(manifest_path(journal), marker)
_journal_configuration.cache_clear()
evidence = archive_path(journal)
evidence.parent.mkdir(parents=True, exist_ok=True)
output: list[bytes | None] = [None] * len(prepared)
for index, (record, schema_identity) in imported.items():
envelope = _linked_envelope(journal, record, schema_identity)
output[index] = json.dumps(envelope, ensure_ascii=False,
separators=(",", ":")).encode("utf-8")
with evidence.open("ab") as archive:
large: list[tuple[int, Mapping, bytes, str]] = []
size = 0
def flush_block():
nonlocal large, size
if not large:
return
block = b"".join(line for _, _, line, _ in large)
compressed = zlib.compress(block, compression_level)
offset = archive.tell()
_write_all(archive, compressed)
block_digest = hashlib.sha256(compressed).hexdigest()
local = 0
for index, record, line, schema_identity in large:
reference = {
"version": 1, "offset": offset,
"compressed_length": len(compressed),
"block_sha256": block_digest,
"record_offset": local, "record_length": len(line),
"record_sha256": hashlib.sha256(line).hexdigest(),
}
output[index] = json.dumps(_envelope(record, reference, schema_identity),
ensure_ascii=False, separators=(",", ":")).encode("utf-8")
local += len(line)
large, size = [], 0
for index, (record, line, schema_identity, _) in enumerate(prepared):
if line is None:
continue
if len(line) < threshold and record.get("kind") not in DETAILED_RECORD_KINDS:
output[index] = line
continue
if large and size + len(line) > MAXIMUM_BLOCK_BYTES:
flush_block()
large.append((index, record, line, schema_identity))
size += len(line)
flush_block()
archive.flush()
if durable:
os.fsync(archive.fileno())
with journal.open("ab") as stream:
_write_all(stream, b"\n".join(output) + b"\n")
stream.flush()
if durable:
os.fsync(stream.fileno())
def _linked_envelope(destination: Path, record, schema_identity) -> dict:
"""Hard-link an immutable source archive and retain its indexed row."""
reference = record.reference
source_archive = (record.path.parent / reference.archive
if reference.archive else archive_path(record.path))
stat = source_archive.stat()
identity = hashlib.sha256(
f"{stat.st_dev}:{stat.st_ino}:{stat.st_size}".encode()).hexdigest()[:20]
target = destination.parent / f"imported_evidence_{identity}.zlib"
if target.exists():
if not os.path.samefile(source_archive, target):
raise ValueError("imported_evidence_identity_collision")
else:
try:
os.link(source_archive, target)
except OSError as error:
if error.errno != getattr(os, "EXDEV", 18):
raise
shutil.copy2(source_archive, target)
directory = os.open(destination.parent, os.O_RDONLY)
try:
os.fsync(directory)
finally:
os.close(directory)
linked = EvidenceRecordReference(reference.offset, reference.compressed_length,
reference.block_sha256, reference.record_offset, reference.record_length,
reference.record_sha256, target.name)
result = {REFERENCE_KEY: linked.as_mapping(), SCHEMA_KEY: schema_identity, **record.metadata}
if record.canonical_sha256:
result[CANONICAL_SHA256_KEY] = record.canonical_sha256
return result
def sync_indexed_journal(journal_path: str | Path) -> None:
"""Order the persistence barrier so a durable reference is never dangling."""
evidence = archive_path(journal_path)
if evidence.exists():
with evidence.open("rb") as stream:
os.fsync(stream.fileno())
with Path(journal_path).open("rb") as stream:
os.fsync(stream.fileno())
@lru_cache(maxsize=8)
def _read_block(path: str, offset: int, length: int, digest: str) -> bytes:
with Path(path).open("rb") as stream:
stream.seek(offset)
compressed = stream.read(length)
if len(compressed) != length or hashlib.sha256(compressed).hexdigest() != digest:
raise ValueError("compressed_evidence_block_changed")
try:
return zlib.decompress(compressed)
except zlib.error as error:
raise ValueError("compressed_evidence_block_invalid") from error
def evidence_record_bytes(journal_path: str | Path, reference: Mapping) -> bytes:
reference = (reference if isinstance(reference, EvidenceRecordReference)
else EvidenceRecordReference.from_mapping(reference))
journal_path = Path(journal_path)
evidence = (journal_path.parent / reference.archive
if reference.archive else archive_path(journal_path))
block = _read_block(str(evidence.resolve()), reference.offset,
reference.compressed_length, reference.block_sha256)
start, length = reference.record_offset, reference.record_length
raw = block[start:start + length]
if len(raw) != length or hashlib.sha256(raw).hexdigest() != reference.record_sha256:
raise ValueError("compressed_evidence_record_changed")
return raw
def decode_envelope(journal_path: str | Path, envelope: Mapping, decoder):
if REFERENCE_KEY not in envelope:
return envelope
value = decoder.loads(evidence_record_bytes(journal_path, envelope[REFERENCE_KEY]))
expected_fields = (envelope.get(FIELDS_KEY) if FIELDS_KEY in envelope
else _schema_fields(journal_path, envelope[SCHEMA_KEY]) if SCHEMA_KEY in envelope else None)
if (not isinstance(value, dict) or expected_fields is not None
and tuple(value) != tuple(expected_fields)):
raise ValueError("compressed_evidence_record_shape_changed")
return value
def compact_existing_journal(source_path: str | Path, *, replace=False) -> dict:
"""Stream a legacy journal into indexed storage and verify every record.
With ``replace=True`` the original path is swapped only after the compact
copy has passed record-content verification. The archive contains every
externalized record in full; no observation or diagnostic field is lost.
"""
source = Path(source_path).resolve()
if not source.is_file():
raise FileNotFoundError(source)
source_is_indexed = is_indexed_journal(source)
source_marker = (json.loads(manifest_path(source).read_text(encoding="utf-8"))
if source_is_indexed else {})
compact = source.with_name(source.name + ".compacting")
compact_archive = archive_path(compact)
for path in (compact, compact_archive, manifest_path(compact)):
if path.exists():
path.unlink()
# Closed checkpoints favor disk footprint. Live capture remains level 1 so
# evidence persistence cannot stall the control loop.
initialize_evidence_journal(compact, compression_level=6)
source_digest = hashlib.sha256()
record_count = 0
batch = []
try:
from ...core.geometry.frozen_evidence import EvidenceDecoder
decoder = EvidenceDecoder(verify_hashes=False)
with source.open("rb") as stream:
for number, line in enumerate(stream, 1):
source_digest.update(line)
if not line.strip():
continue
try:
row = decoder.loads(line)
if REFERENCE_KEY in row:
row = decode_envelope(source, row, decoder)
except json.JSONDecodeError as error:
raise ValueError(f"invalid calibration JSONL at line {number}") from error
if not isinstance(row, dict):
raise ValueError(f"calibration JSONL record is not an object at line {number}")
batch.append(row)
record_count += 1
if len(batch) == 256:
append_indexed_records(compact, batch, durable=False)
batch.clear()
append_indexed_records(compact, batch, durable=False)
sync_indexed_journal(compact)
# Stream verification keeps the migration bounded even for multi-GB captures.
left_decoder, right_decoder = (EvidenceDecoder(verify_hashes=False) for _ in range(2))
with source.open("rb") as left, compact.open("rb") as right:
right_lines = (line for line in right if line.strip())
verified = 0
for left_line in left:
if not left_line.strip():
continue
try:
right_line = next(right_lines)
except StopIteration as error:
raise ValueError("compacted_evidence_record_count_changed") from error
left_row = left_decoder.loads(left_line)
if REFERENCE_KEY in left_row:
left_row = decode_envelope(source, left_row, left_decoder)
envelope = right_decoder.loads(right_line)
right_row = decode_envelope(compact, envelope, right_decoder)
if left_row != right_row:
raise ValueError("compacted_evidence_differs_from_source")
verified += 1
try:
next(right_lines)
except StopIteration:
pass
else:
raise ValueError("compacted_evidence_record_count_changed")
if verified != record_count:
raise ValueError("compacted_evidence_record_count_changed")
def file_sha256(path):
with Path(path).open("rb") as stream:
return hashlib.file_digest(stream, "sha256").hexdigest()
compact_digest = file_sha256(compact)
archive_digest = file_sha256(compact_archive)
result = {
"format": FORMAT, "source": str(source),
"source_sha256": source_digest.hexdigest(),
"source_bytes": source.stat().st_size,
"record_count": record_count,
"compact_journal_bytes": compact.stat().st_size,
"compressed_evidence_bytes": compact_archive.stat().st_size,
"compact_journal_sha256": compact_digest,
"compressed_evidence_sha256": archive_digest,
"semantic_verification": "passed",
}
if replace:
final_archive = archive_path(source)
final_manifest = manifest_path(source)
backup = source.with_name(source.name + ".uncompressed-backup")
backup_archive = final_archive.with_name(final_archive.name + ".backup")
backup_manifest = final_manifest.with_name(final_manifest.name + ".backup")
if backup.exists() or backup_archive.exists() or backup_manifest.exists():
raise FileExistsError("compact journal destination already exists")
if not source_is_indexed and (final_archive.exists() or final_manifest.exists()):
raise FileExistsError("compact journal destination already exists")
marker = json.loads(manifest_path(compact).read_text(encoding="utf-8"))
marker["archive"] = final_archive.name
marker.update({key: result[key] for key in ("source_sha256", "source_bytes", "record_count")})
if source_marker:
marker["lineage_source_sha256"] = source_marker.get(
"lineage_source_sha256", source_marker.get("source_sha256"))
marker["lineage_source_bytes"] = source_marker.get(
"lineage_source_bytes", source_marker.get("source_bytes"))
from .storage import atomic_write_json
try:
if source_is_indexed:
final_archive.replace(backup_archive)
final_manifest.replace(backup_manifest)
compact_archive.replace(final_archive)
source.replace(backup)
compact.replace(source)
if (file_sha256(source) != compact_digest
or file_sha256(final_archive) != archive_digest):
raise ValueError("installed_compact_journal_verification_failed")
atomic_write_json(final_manifest, marker)
_journal_configuration.cache_clear()
except Exception:
final_manifest.unlink(missing_ok=True)
if backup.exists():
if source.exists():
source.replace(compact)
backup.replace(source)
if final_archive.exists():
final_archive.replace(compact_archive)
if source_is_indexed:
if backup_archive.exists():
backup_archive.replace(final_archive)
if backup_manifest.exists():
backup_manifest.replace(final_manifest)
raise
backup.unlink()
backup_archive.unlink(missing_ok=True)
backup_manifest.unlink(missing_ok=True)
manifest_path(compact).unlink(missing_ok=True)
_journal_configuration.cache_clear()
result["installed"] = True
return result
except Exception:
_journal_configuration.cache_clear()
compact.unlink(missing_ok=True)
compact_archive.unlink(missing_ok=True)
manifest_path(compact).unlink(missing_ok=True)
raise
@@ -0,0 +1,135 @@
"""Crash-safe session storage for hardware calibration."""
from __future__ import annotations
import json
import os
from pathlib import Path
from typing import Any, Iterable, Mapping
def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> None:
destination = Path(path)
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_suffix(destination.suffix + ".tmp")
with temporary.open("w", encoding="utf-8") as stream:
json.dump(payload, stream, ensure_ascii=False, indent=2)
stream.write("\n")
stream.flush()
os.fsync(stream.fileno())
os.replace(temporary, destination)
def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
destination = Path(path)
destination.parent.mkdir(parents=True, exist_ok=True)
from .evidence_journal import is_indexed_journal, append_indexed_records
if is_indexed_journal(destination):
append_indexed_records(destination, (payload,), durable=True)
return
line = json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
with destination.open("a", encoding="utf-8") as stream:
stream.write(line + "\n")
stream.flush()
os.fsync(stream.fileno())
def append_jsonl_many(
path: str | Path, payloads: Iterable[Mapping[str, Any]], *, durable: bool = True,
) -> None:
"""Append one ordered batch, durable by default.
Continuous observations may defer fsync until a reference/checkpoint,
pause, or explicit sync_jsonl barrier. They are still flushed before
returning; only complete, durable scan units can authorize resume.
"""
destination = Path(path)
destination.parent.mkdir(parents=True, exist_ok=True)
payloads = tuple(payloads)
from .evidence_journal import is_indexed_journal, append_indexed_records
if is_indexed_journal(destination):
append_indexed_records(destination, payloads, durable=durable)
return
lines = [
json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
for payload in payloads
]
if not lines:
return
with destination.open("a", encoding="utf-8") as stream:
stream.write("\n".join(lines) + "\n")
stream.flush()
if durable:
os.fsync(stream.fileno())
def sync_jsonl(path: str | Path) -> None:
"""Persist all previously flushed records before hashing or publishing."""
from .evidence_journal import is_indexed_journal, sync_indexed_journal
if is_indexed_journal(path):
sync_indexed_journal(path)
return
with Path(path).open("rb") as stream:
os.fsync(stream.fileno())
def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
source = Path(path)
if not source.exists():
return []
records: list[dict[str, Any]] = []
from .evidence_journal import REFERENCE_KEY, decode_envelope
from ..geometry.frozen_evidence import EvidenceDecoder
decoder = EvidenceDecoder()
with source.open("r", encoding="utf-8") as stream:
lines = stream.readlines()
nonempty_lines = [
index for index, line in enumerate(lines, 1) if line.strip()
]
last_nonempty_line = nonempty_lines[-1] if nonempty_lines else 0
for line_number, line in enumerate(lines, 1):
if not line.strip():
continue
try:
value = json.loads(line)
except json.JSONDecodeError as error:
if line_number == last_nonempty_line:
break
raise ValueError(
f"{source}:{line_number}: invalid JSONL record"
) from error
if not isinstance(value, dict):
raise ValueError(f"{source}:{line_number}: record must be an object")
if REFERENCE_KEY in value:
value = decode_envelope(source, value, decoder)
records.append(value)
return records
def load_json(path: str | Path) -> dict[str, Any] | None:
source = Path(path)
if not source.exists():
return None
with source.open("r", encoding="utf-8") as stream:
value = json.load(stream)
if not isinstance(value, dict):
raise ValueError(f"{source} must contain a JSON object")
return value
def completed_scan_keys(
records: Iterable[Mapping[str, Any]],
) -> set[tuple[str, int, str, int]]:
keys: set[tuple[str, int, str, int]] = set()
for record in records:
if record.get("kind", "sample") != "sample":
continue
keys.add(
(
str(record["phase"]),
int(record["cycle"]),
str(record["direction"]),
int(record["command_u8"]),
)
)
return keys
@@ -0,0 +1,65 @@
"""Calibration domain types."""
from .profile import (
AcquisitionPolicy,
ArtifactPolicy,
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
ProfileValidationError,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
JointZeroSpec,
KnownBaselineGeometry,
JointLimitPolicy,
validate_profile,
)
from .result import CalibrationResult, JointMapping
from .reference import JointZeroReference, JointZeroSample
from .status import (
AcquisitionStatus,
CalibrationStatus,
MotionStatus,
PauseStatus,
TaskStatus,
)
__all__ = [
"AcquisitionPolicy",
"ArtifactPolicy",
"AcquisitionStatus",
"CalibrationProfile",
"CalibrationResult",
"JointMapping",
"JointZeroReference",
"JointZeroSample",
"JointZeroSpec",
"KnownBaselineGeometry",
"JointLimitPolicy",
"CalibrationStatus",
"CommandLayout",
"MeasurementPolicy",
"MeasurementSpec",
"MotionPolicy",
"MotionStatus",
"ProfileKey",
"ProfileValidationError",
"PauseStatus",
"QualityPolicy",
"ScopePolicy",
"TagSpec",
"TaskStatus",
"TaskSpec",
"ViewSpec",
"VisionRigSpec",
"ZeroSolvePolicy",
"validate_profile",
]
@@ -0,0 +1,214 @@
"""One immutable partition contract for acquisition, fitting and release.
Cycle IDs are evidence identities, not display ordinals. Optional training
never renumbers the independent validation cycle or changes its command grid.
"""
from dataclasses import asdict, dataclass, replace
import hashlib
import json
PLAN_VERSION = "capture_plan_v1"
FIXED = "fixed"
ADAPTIVE = "adaptive_2_to_3"
STAGED = "staged_2_to_3"
TASKWISE_STAGED = "taskwise_staged_2_to_3"
TASKWISE_TWO_ROUND = "taskwise_2_plus_1"
RAW_JOINT = "raw_joint_2_plus_1"
RAW_FEEDBACK = "raw_feedback_3_plus_1"
FEEDBACK_RAW_PROTOCOL = "raw_feedback_images_v1"
RAW_PROTOCOL = "raw_joint_images_v2"
LEGACY_RAW_PROTOCOL = "raw_joint_images_v1"
STAGED_POLICIES = frozenset((STAGED, TASKWISE_STAGED))
PREPARATION_POLICIES = STAGED_POLICIES | {TASKWISE_TWO_ROUND}
LEGACY_TRAINING = (0, 1, 2)
LEGACY_HOLDOUT = 3
def raw_joint_capture(profile):
return profile.quality.training_policy == RAW_JOINT
def raw_feedback_capture(profile):
return profile.quality.training_policy == RAW_FEEDBACK
def evidence_digest(value):
return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(",", ":"),
allow_nan=False).encode()).hexdigest()
@dataclass(frozen=True)
class TaskPartition:
task_key: str
training: tuple[int, ...]
holdout: int
command_training: tuple[int, ...]
command_holdout: int
@property
def motion_cycles(self):
return (*self.training, self.holdout)
def cycles(self, phase):
return ((*self.command_training, self.command_holdout)
if phase == "steady" else self.motion_cycles)
@dataclass(frozen=True)
class CapturePlan:
policy: str
command_capture_mode: str
tasks: tuple[TaskPartition, ...]
motion_contract_json: str
@classmethod
def from_profile(cls, profile):
tasks = tuple(task_partition(profile, task) for task in profile.motion.tasks)
# JSON normalization keeps equality stable across checkpoint round trips.
from .sampling import command_nodes
motion = {"unit": profile.command.unit, "motion": asdict(profile.motion),
"nodes": {task.key: {"training": command_nodes(profile, task),
"holdout": command_nodes(profile, task, holdout=True)} for task in profile.motion.tasks}}
# Auxiliary observer excursions produce no scan/partition identities.
# Their full paths are protected by preparation_contract instead; old
# completed units and training partitions keep their original hashes.
motion['motion'].pop('observer_preparations')
if not profile.motion.preparation_witnesses:
motion["motion"].pop("preparation_witnesses") # Historical plan hashes stay unchanged.
return cls(profile.quality.training_policy, profile.acquisition.command_capture_mode, tasks,
json.dumps(motion, sort_keys=True, separators=(",", ":"), allow_nan=False))
def task(self, key):
return next(task for task in self.tasks if task.task_key == key)
def as_dict(self, *, legacy_raw=False):
version = (("capture_plan_v6_raw_joint" if legacy_raw else "capture_plan_v7_raw_joint") if self.policy == RAW_JOINT else
"capture_plan_v5" if self.policy == TASKWISE_TWO_ROUND else
"capture_plan_v3" if self.policy == TASKWISE_STAGED else
"capture_plan_v2" if self.policy == STAGED else
"capture_plan_v8_raw_feedback" if self.policy == RAW_FEEDBACK else PLAN_VERSION)
result = {"version": version, "policy": self.policy,
"command_capture_mode": self.command_capture_mode,
"motion_contract": json.loads(self.motion_contract_json),
"tasks": {t.task_key: {"training_cycles": list(t.training),
"holdout_cycle": t.holdout,
"command_training_cycles": list(t.command_training),
"command_holdout_cycle": t.command_holdout} for t in self.tasks}}
if self.policy in STAGED_POLICIES:
result["stages"] = [
{"name": name, "cycle": cycle, "tasks": [t.task_key for t in self.tasks
if cycle in t.motion_cycles]}
for name, cycle in (("first_training", 0), ("second_training", 1),
("supplement", 2), ("validation", 3))]
if self.policy == TASKWISE_STAGED:
result["schedule"] = (
"first_whole_hand_pass_then_taskwise_second_pass_with_immediate_supplement_then_validation"
)
if self.policy == TASKWISE_TWO_ROUND:
result["schedule"] = "each_task_two_training_rounds_then_independent_validation"
result["preparation_recovery"] = {
"policy": "dependency_ordered_task_deferral_v2",
"maximum_revisits": 1,
"preserve_task_round_order": True,
"incomplete_capture_blocks_publication": True,
}
if self.policy == RAW_FEEDBACK:
result["observation_protocol"] = {
"version": FEEDBACK_RAW_PROTOCOL,
"online_geometry_authorization": False,
"motion_training_cycles": [0, 1, 2], "motion_holdout_cycle": 3,
"command_training_cycle": 4, "command_holdout_cycle": 5,
"maximum_unit_recaptures": 1,
"incomplete_capture_blocks_publication": True,
}
if self.policy == RAW_JOINT:
result["schedule"] = "each_task_two_training_rounds_then_independent_validation"
result["observation_protocol"] = {
"version": LEGACY_RAW_PROTOCOL if legacy_raw else RAW_PROTOCOL,
"online_geometry_authorization": False,
"maximum_unit_recaptures": 1,
"recapture_order": "queue_tail" if legacy_raw else "immediate",
"incomplete_capture_blocks_publication": True,
}
if not legacy_raw:
result["observation_protocol"].update(hardware_stall_retries=1,
stall_retry_delay_seconds=3.0, motion_progress_scope="direction_active_time")
return result
@property
def sha256(self):
return evidence_digest(self.as_dict())
def task_for_joint(profile, joint):
donor = profile.measurement.transferred_motion_sources.get(joint, joint)
secondary = {source: primary for primary, source in profile.measurement.cross_view_sources.items()}
donor = secondary.get(donor, donor)
for task in profile.motion.tasks:
if donor in task.joints:
return task
raise ValueError(f"capture_plan_joint_has_no_task:{joint}")
def task_partition(profile, task=None, *, joint=None):
if joint is not None:
task = task_for_joint(profile, joint)
key = getattr(task, "key", task)
default = (LEGACY_TRAINING[:2] if profile.quality.training_policy in PREPARATION_POLICIES | {RAW_JOINT}
else profile.quality.training_cycles)
training = tuple(profile.quality.task_training_cycles.get(key, default))
holdout = profile.quality.holdout_cycle
separate = profile.acquisition.command_capture_mode == "separate"
return TaskPartition(key, training, holdout, (holdout + 1,) if separate else training,
holdout + 2 if separate else holdout)
def training_cycles(profile, task=None, *, joint=None):
return task_partition(profile, task, joint=joint).training
def select_task_training(profile, task_key, cycles):
cycles = tuple(cycles)
if task_key not in {t.key for t in profile.motion.tasks}:
raise ValueError("capture_plan_unknown_task")
if cycles not in (LEGACY_TRAINING, LEGACY_TRAINING[:2]):
raise ValueError("capture_plan_invalid_training_partition")
if profile.quality.training_policy in {TASKWISE_TWO_ROUND, RAW_JOINT}:
if cycles != LEGACY_TRAINING[:2]:
raise ValueError("taskwise_two_round_cannot_add_training")
return profile
if (cycles != tuple(profile.quality.training_cycles)
and profile.quality.training_policy not in {ADAPTIVE, *STAGED_POLICIES}):
raise ValueError("capture_plan_fixed_training_cannot_change")
selections = {**profile.quality.task_training_cycles, task_key: cycles}
return replace(profile, quality=replace(profile.quality, task_training_cycles=selections))
def configure_training(profile, policy):
if policy not in {FIXED, ADAPTIVE, *STAGED_POLICIES, TASKWISE_TWO_ROUND, RAW_JOINT, RAW_FEEDBACK}:
raise ValueError("unknown_training_policy")
if policy == RAW_FEEDBACK and (profile.command_based_release
or profile.acquisition.command_capture_mode != "separate"
or profile.artifacts.output_schema_version < 3
or tuple(profile.quality.training_cycles) != LEGACY_TRAINING
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
raise ValueError("raw_feedback_requires_separate_feedback_v3_profile")
if policy == RAW_JOINT and profile.key.profile_id != 'O30/right/o30_right_18/v1':
raise ValueError("raw_joint_capture_requires_o30_profile")
if profile.artifacts.output_schema_version in {2, 3} and (
tuple(profile.quality.training_cycles) != LEGACY_TRAINING
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
raise ValueError("production_partition_requires_versioned_training_policy")
if policy in {ADAPTIVE, *STAGED_POLICIES, TASKWISE_TWO_ROUND, RAW_JOINT} and (profile.artifacts.output_schema_version < 3
or not profile.command_based_release
or profile.acquisition.command_capture_mode != "interleaved"
or tuple(profile.quality.training_cycles) != LEGACY_TRAINING
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
raise ValueError("adaptive_training_requires_measured_interleaved_command_release")
# Fixed two-round collection uses the ordinary task-contiguous scheduler.
# Its partition is derived by task_partition, not an executed journal
# decision. Keep the reusable profile serializable for the launched node.
return replace(profile, quality=replace(profile.quality,
training_policy=policy, task_training_cycles={}))
@@ -0,0 +1,72 @@
"""Shared measurement and curve contracts; no hardware or model defaults."""
from __future__ import annotations
from dataclasses import dataclass
from typing import Any, Mapping
@dataclass(frozen=True)
class JointSpec:
name: str
motor_index: int
active: bool
view: str | None
parent_role: str | None
child_role: str | None
source_joint: str | None = None
zero_kind: str | None = None
# Keep the physical axis-line gate only when that line contributes to a
# released URDF zero/axis decision. Curve-only passive measurements may
# retain the monocular line residual as a diagnostic while their image
# trajectory, relative rotation, synchronisation and holdout gates remain
# release-critical.
pose_axis_line_required: bool = True
@property
def measured(self) -> bool:
return self.source_joint is None
@dataclass(frozen=True)
class SweepSpec:
view: str
motor_index: int
joints: tuple[str, ...]
task_name: str = ""
auxiliary_commands: tuple[tuple[int, int], ...] = ()
validation_only: bool = False
@property
def key(self) -> str:
if self.task_name:
return self.task_name
return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}"
@dataclass(frozen=True)
class PalmAxisObserver:
"""Non-blocking direction observation attached to an existing task.
This is deliberately not a ``JointSpec``: it has no curve, endpoint or
retry semantics. The camera callback records it only while both Tags are
visible during the named sweep.
"""
source_name: str
task_name: str
view: str
parent_role: str
child_role: str
model_joint: str
motor_index: int
@dataclass(frozen=True)
class JointCurveFit:
angle_rad: tuple[float, ...]
decreasing_rad: tuple[float, ...]
increasing_rad: tuple[float, ...]
circle: Mapping[str, Any]
maximum_monotonic_correction_rad: float
maximum_hysteresis_rad: float
quality: Mapping[str, float]
zero_offset_rad: float = 0.0
@@ -0,0 +1,78 @@
"""Declared multi-channel paths and durable scan identities.
Only paths own interpolation. Every observation retains its own motor channel;
the scalar path coordinate is never a joint measurement or a new zero.
"""
from dataclasses import dataclass
import math
@dataclass(frozen=True)
class MotionSegment:
key: str
start_commands: tuple[tuple[int, float], ...]
end_commands: tuple[tuple[int, float], ...]
joints: tuple[str, ...]
@property
def moving_channels(self):
end = dict(self.end_commands)
return tuple(i for i, value in self.start_commands if end[i] != value)
@property
def command_index(self):
return self.moving_channels[0]
@property
def start(self):
return dict(self.start_commands)[self.command_index]
@property
def end(self):
return dict(self.end_commands)[self.command_index]
@property
def direction(self):
return "increasing" if self.end > self.start else "decreasing"
def commands_at(self, value, *, quantize=False):
fraction = (value-self.start)/(self.end-self.start)
if not math.isfinite(fraction) or not -1e-9 <= fraction <= 1+1e-9:
raise ValueError("motion segment coordinate outside declared path")
end = dict(self.end_commands)
values = {i: start+(end[i]-start)*fraction for i, start in self.start_commands}
return {i: float(round(v)) for i, v in values.items()} if quantize else values
def task_segments(task):
if task.segments:
return task.segments
return tuple(MotionSegment("", ((task.command_index, start),),
((task.command_index, end),), task.joints)
for start, end in ((task.start_value, task.end_value), (task.end_value, task.start_value)))
def task_segment(task, key):
for segment in task.segments:
if segment.key == key:
return segment
raise ValueError(f"unknown motion segment:{task.key}:{key}")
def task_channels(task):
return tuple(sorted({i for segment in task_segments(task) for i in segment.moving_channels}))
def scan_identity(task, cycle, direction, segment_key=""):
base = (task, cycle, direction)
return (*base, segment_key) if segment_key else base
def record_scan_identity(row):
return scan_identity(row.get("task_name"), row.get("cycle"), row.get("direction"),
row.get("segment_key", ""))
def segment_metadata(key):
return {"segment_key": key} if key else {}
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,249 @@
"""Immutable visual baselines, acquired once before each joint's training."""
from __future__ import annotations
from dataclasses import asdict, dataclass, replace
import math
import re
from typing import Mapping, Sequence
import numpy as np
from scipy.spatial.transform import Rotation
@dataclass(frozen=True)
class JointZeroPose:
quaternion_xyzw: tuple[float, ...]
translation_xyz_m: tuple[float, ...]
@classmethod
def from_mapping(cls, row):
q, t = tuple(row["quaternion_xyzw"]), tuple(row["translation_xyz_m"])
if (len(q) != 4 or len(t) != 3 or not np.all(np.isfinite(q+t))
or not np.isclose(np.linalg.norm(q), 1, atol=1e-5)):
raise ValueError("joint_zero_pose_invalid")
return cls(q, t)
@dataclass(frozen=True)
class JointZeroSample:
sample_id: str
view: str
image_stamp_ns: int
relative_quaternion_xyzw: tuple[float, ...]
relative_translation_xyz_m: tuple[float, ...]
command: tuple[float, ...]
feedback: tuple[float, ...]
arrival_directions: tuple[str, ...]
parent_pose_common: JointZeroPose
child_pose_common: JointZeroPose
@classmethod
def from_row(cls, row, unit):
return cls(f"{row['view']}:{int(row['image_stamp_ns'])}", row["view"],
int(row["image_stamp_ns"]), tuple(row["relative_quaternion_xyzw"]),
tuple(row["relative_translation_xyz_m"]), tuple(row[f"command_vector_{unit}"]),
tuple(row.get(f"state_{unit}") or ()), tuple(row.get("command_direction_by_index", ())),
JointZeroPose.from_mapping(row["parent_pose_common"]), JointZeroPose.from_mapping(row["child_pose_common"]))
@dataclass(frozen=True)
class JointZeroBranchReference:
"""A moving physical Tag's immutable pre-zero motion authorization."""
view: str
tag_role: str
branch_revision: int
motion_evidence_id: str
def __post_init__(self):
if (not self.view or not self.tag_role or type(self.branch_revision) is not int
or self.branch_revision < 1 or not isinstance(self.motion_evidence_id, str)
or re.fullmatch(r"[0-9a-f]{64}", self.motion_evidence_id) is None):
raise ValueError("joint_zero_motion_evidence_invalid")
@dataclass(frozen=True)
class JointZeroReference:
joint: str
task_key: str
baseline_command: float
channel: int
session_epoch: int
motion_version: int
samples: tuple[JointZeroSample, ...]
branch_references: tuple[JointZeroBranchReference, ...] = ()
def quaternion(self, view):
rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in self.samples if s.view == view])
return rotations.mean().as_quat()
def input_reference(self, view, *, feedback=False):
values = [(s.feedback if feedback else s.command)[self.channel]
for s in self.samples if s.view == view]
if not values:
raise ValueError(f"joint_zero_missing_view:{self.joint}:{view}")
return float(np.median(values))
def as_record(self):
return {"kind": "joint_zero_reference", **asdict(self)}
@classmethod
def from_record(cls, row):
for sample in row["samples"]:
if sample["sample_id"] != f"{sample['view']}:{int(sample['image_stamp_ns'])}":
raise ValueError(f"joint_zero_image_identity_invalid:{row['joint']}")
samples = tuple(JointZeroSample(**{**s,
"parent_pose_common": JointZeroPose.from_mapping(s["parent_pose_common"]),
"child_pose_common": JointZeroPose.from_mapping(s["child_pose_common"]),
**{key: tuple(s[key]) for key in ("relative_quaternion_xyzw", "relative_translation_xyz_m",
"command", "feedback", "arrival_directions")}}) for s in row["samples"])
return cls(str(row["joint"]), str(row["task_key"]), float(row["baseline_command"]),
int(row["channel"]), int(row["session_epoch"]), int(row["motion_version"]), samples,
tuple(JointZeroBranchReference(**item) for item in row.get("branch_references", ())))
def joint_zero_moving_roles(profile, joint):
"""Physical roles that require motion evidence for this joint's zero."""
names = (joint, profile.measurement.cross_view_sources.get(joint))
roles = set()
for name in names:
if name is None:
continue
measurement = profile.measurement.measurements[name]
for view in profile.vision.views:
if view.name == measurement.view:
roles.update((view.name, tag.role) for tag in view.tags
if tag.role in (measurement.parent_role, measurement.child_role) and not tag.fixed_reference)
return roles
def _branch_references(profile, joint, rows):
allowed = joint_zero_moving_roles(profile, joint)
result, observed = {}, {}
for row in rows:
for item in row.get("pnp_observation_evidence", {}).values():
key = (row["view"], item.get("tag_role"))
diagnostics = item.get("candidate_diagnostics", {})
verified, identity = diagnostics.get("motion_evidence_verified"), diagnostics.get("motion_evidence_id")
observed.setdefault(key, []).append((verified, identity))
if not identity and verified is not True:
continue # Legacy image confirmation remains readable as legacy.
if (key not in allowed or verified is not True or item.get("pose_source") not in {
"current_image", "image_constrained_current_image"}
or diagnostics.get("observation_stamp_ns") != row["image_stamp_ns"]
or diagnostics.get("branch_status") != "tracking"):
raise ValueError(f"joint_zero_motion_evidence_unconfirmed:{joint}")
reference = JointZeroBranchReference(*key, diagnostics.get("branch_revision"), identity)
if key in result and result[key] != reference:
raise ValueError(f"joint_zero_motion_evidence_changed:{joint}")
result[key] = reference
for key, reference in result.items():
required = sum(row["view"] == key[0] for row in rows)
if len(observed[key]) != required or any(
verified is not True or identity != reference.motion_evidence_id for verified, identity in observed[key]):
raise ValueError(f"joint_zero_motion_evidence_incomplete:{joint}")
return tuple(result[key] for key in sorted(result))
def _validate_branch_bindings(profile, reference):
keys = [(item.view, item.tag_role) for item in reference.branch_references]
if len(set(keys)) != len(keys) or not set(keys) <= joint_zero_moving_roles(profile, reference.joint):
raise ValueError(f"joint_zero_motion_evidence_binding_invalid:{reference.joint}")
def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, session_epoch, motion_version):
spec = profile.motion.joint_zero_references[joint]
task = next(t for t in profile.motion.tasks if t.key == spec.task_key)
from ..fitting.motion_fit import channel_for_joint
channel = channel_for_joint(profile, joint)
views = {profile.measurement.measurements[joint].view}
secondary = profile.measurement.cross_view_sources.get(joint)
if secondary:
views.add(profile.measurement.measurements[secondary].view)
if session_epoch < 1 or motion_version < 1:
raise ValueError(f"joint_zero_session_version_invalid:{joint}")
# Incomplete windows are checked by the control timer repeatedly. Check
# their image coverage before constructing and validating every pose.
minimum = profile.acquisition.fixed_reference_minimum_frames
for view in views:
count = len({int(row["image_stamp_ns"]) for row in rows if row["view"] == view})
if count < minimum:
measurements = [profile.measurement.measurements[n] for n in (joint, secondary) if n is not None]
roles = {role for m in measurements if m.view == view for role in (m.parent_role, m.child_role)}
ids = [t.tag_id for v in profile.vision.views if v.name == view for t in v.tags if t.role in roles]
raise ValueError(f"joint_zero_samples_missing:{joint}:{view}:required_tags={ids}:{count}/{minimum}")
samples = tuple(JointZeroSample.from_row(row, profile.command.unit) for row in rows)
for sample in samples:
if sample.image_stamp_ns < 1 or sample.sample_id != f"{sample.view}:{sample.image_stamp_ns}":
raise ValueError(f"joint_zero_image_identity_invalid:{joint}")
if len(sample.relative_quaternion_xyzw) != 4 or not np.isclose(np.linalg.norm(sample.relative_quaternion_xyzw), 1, atol=1e-5):
raise ValueError(f"joint_zero_rotation_invalid:{joint}")
parent, child = sample.parent_pose_common, sample.child_pose_common
rp, rc = Rotation.from_quat(parent.quaternion_xyzw), Rotation.from_quat(child.quaternion_xyzw)
if ((Rotation.from_quat(sample.relative_quaternion_xyzw).inv()*rp.inv()*rc).magnitude() > 1e-7
or not np.allclose(rp.inv().apply(np.asarray(child.translation_xyz_m)-parent.translation_xyz_m),
sample.relative_translation_xyz_m, atol=1e-8, rtol=0)):
raise ValueError(f"joint_zero_relative_pose_inconsistent:{joint}")
for view in views:
selected = [s for s in samples if s.view == view]
rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in selected])
if np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1):
raise ValueError(f"joint_zero_not_stable:{joint}:{view}")
identities = [(s.view, s.sample_id) for s in samples]
if len(set(identities)) != len(identities):
raise ValueError(f"duplicate_joint_zero_image:{joint}")
for sample in samples:
if sample.view not in views or sample.command != spec.command:
raise ValueError(f"joint_zero_pose_differs:{joint}")
if not profile.vision_motion and (len(sample.feedback) != profile.command.command_count or not np.all(np.isfinite(sample.feedback))):
raise ValueError(f"joint_zero_feedback_invalid:{joint}")
if len(sample.arrival_directions) != profile.command.command_count:
raise ValueError(f"joint_zero_arrival_missing:{joint}")
if spec.approach_commands:
before = spec.approach_commands[-1][channel]
expected = "increasing" if spec.command[channel] > before else "decreasing"
if sample.arrival_directions[channel] != expected:
raise ValueError(f"joint_zero_arrival_changed:{joint}")
return JointZeroReference(joint, spec.task_key, profile.command.baseline_values[channel],
channel, session_epoch, motion_version, samples, _branch_references(profile, joint, rows))
def read_joint_zero_references(profile, records):
"""Reject invented, repeated or late baselines in online and offline inputs."""
result = {}
first_training_stamp = {}
for row in records:
if row.get("kind") in {"joint_sample", "secondary_joint_sample"}:
name = row.get("joint", row.get("observation_joint"))
stamp = int(row["image_stamp_ns"])
first_training_stamp[name] = min(first_training_stamp.get(name, stamp), stamp)
if row.get("kind") != "joint_zero_reference":
continue
reference = JointZeroReference.from_record(row)
name = reference.joint
if name in result or name not in profile.motion.joint_zero_references:
raise ValueError(f"duplicate_or_unknown_joint_zero:{name}")
spec = profile.motion.joint_zero_references[name]
if (not 0 <= reference.channel < profile.command.command_count or reference.task_key != spec.task_key
or reference.baseline_command != profile.command.baseline_values[reference.channel]):
raise ValueError(f"joint_zero_contract_changed:{name}")
# Revalidate the complete stored evidence, not just an averaged quaternion.
rows = [{"view": s.view, "image_stamp_ns": s.image_stamp_ns,
"relative_quaternion_xyzw": s.relative_quaternion_xyzw,
"relative_translation_xyz_m": s.relative_translation_xyz_m,
f"command_vector_{profile.command.unit}": s.command, f"state_{profile.command.unit}": s.feedback,
"command_direction_by_index": s.arrival_directions,
"parent_pose_common": asdict(s.parent_pose_common), "child_pose_common": asdict(s.child_pose_common)}
for s in reference.samples]
checked = build_joint_zero_reference(profile, name, rows,
session_epoch=reference.session_epoch, motion_version=reference.motion_version)
_validate_branch_bindings(profile, reference)
checked = replace(checked, branch_references=reference.branch_references)
if checked.channel != reference.channel:
raise ValueError(f"joint_zero_channel_changed:{name}")
result[name] = checked
for name, reference in result.items():
if name in first_training_stamp and max(s.image_stamp_ns for s in reference.samples) >= first_training_stamp[name]:
raise ValueError(f"joint_zero_not_before_training:{name}")
return result
@@ -0,0 +1,105 @@
"""Frozen numerical results, independent of a serializer or SDK transport."""
from __future__ import annotations
from dataclasses import dataclass, field
import math
from typing import TYPE_CHECKING, Mapping, Sequence
import numpy as np
if TYPE_CHECKING:
from .measurement import JointCurveFit
from ..fitting.coupling import RelativeMimicEvidence
from ..fitting.spatial import ZeroSolveResult
@dataclass(frozen=True)
class CalibrationResult:
"""Training-frozen measurements and parameters consumed by every writer.
output_mappings is the primary geometry/replay domain, explicitly labeled
by each JointMapping. Optional feedback diagnostics never supply geometry.
This is not a publication certificate: the final serialized URDF must
still pass independent spatial replay and structural authorization.
"""
curves: Mapping[str, JointCurveFit]
output_mappings: Mapping[str, JointMapping]
zero_offsets_rad: Mapping[str, float]
zero_method_by_joint: Mapping[str, str]
standard_mimic_evidence: Mapping[str, RelativeMimicEvidence]
holdout_errors_rad: Mapping[str, tuple[float, ...]]
spatial_zero: ZeroSolveResult
cross_view_metrics: Mapping = field(default_factory=dict)
reference_inputs: Mapping[str, float] = field(default_factory=dict)
command_mappings: Mapping[str, JointMapping] = field(default_factory=dict)
command_holdout_metrics: Mapping = field(default_factory=dict)
command_applicability: Mapping = field(default_factory=dict)
joint_zero_references: Mapping = field(default_factory=dict)
mimic_approximations: Mapping = field(default_factory=dict)
feedback_diagnostics: Mapping = field(default_factory=dict)
@dataclass(frozen=True)
class JointMapping:
"""SDK input -> output URDF coordinate; branches use raw SDK direction.
Coordinate/sign choices belong to fitting. Writers may round numbers but
must not rebase, reverse, clip or refit this result.
"""
joint: str
motor_index: int
input_domain: str
knots: tuple[float, ...]
angle_rad: tuple[float, ...]
increasing_rad: tuple[float, ...]
decreasing_rad: tuple[float, ...]
transferred_from: str | None = None
def __post_init__(self) -> None:
if not self.joint or self.motor_index < 0:
raise ValueError("mapping requires a joint and a valid SDK channel")
if self.input_domain not in {"feedback_rad", "command_rad", "feedback_u8", "command_u8"}:
raise ValueError("mapping requires an explicit SDK input domain")
x = np.asarray(self.knots, dtype=float)
if x.ndim != 1 or x.size < 2 or not np.all(np.isfinite(x)) or np.any(np.diff(x) <= 0):
raise ValueError("mapping knots must be finite and strictly increasing")
for branch in (self.angle_rad, self.increasing_rad, self.decreasing_rad):
y = np.asarray(branch, dtype=float)
if y.shape != x.shape or not np.all(np.isfinite(y)):
raise ValueError("mapping curve must align with its native knots")
d = np.diff(y)
if not (np.all(d >= -1e-9) or np.all(d <= 1e-9)):
raise ValueError("mapping curve must be monotonic")
@property
def bounds_rad(self) -> tuple[float, float]:
values = self.angle_rad + self.increasing_rad + self.decreasing_rad
return min(values), max(values)
def evaluate(self, value: float, direction: str = "") -> float:
if direction not in {"", "increasing", "decreasing"}:
raise ValueError("invalid native SDK direction")
if not math.isfinite(value) or not self.knots[0] <= value <= self.knots[-1]:
raise ValueError(f"SDK input outside fitted support:{self.joint}:{value}")
branch = getattr(self, direction + "_rad") if direction else self.angle_rad
# Commands become actual wire bytes. Feedback can be interpolated
# between CAN receipts at an image's exposure time and stays continuous.
if self.input_domain == "command_u8":
value = math.floor(value + 0.5)
return float(np.interp(value, self.knots, branch))
def evaluate_mappings(
mappings: Mapping[str, JointMapping], sdk_values: Sequence[float],
directions: Mapping[str, str],
) -> dict[str, float]:
result = {}
for joint, mapping in mappings.items():
if joint != mapping.joint or mapping.motor_index >= len(sdk_values):
raise ValueError("mapping/SDK vector contract differs")
result[joint] = mapping.evaluate(float(sdk_values[mapping.motor_index]), directions.get(joint, ""))
return result
@@ -0,0 +1,192 @@
"""Canonical command/feedback schema for calibration observations.
The hand command and its measured motor feedback are different physical
domains. Durable samples always retain both. Fitting code may still use the
historical ``command_u8`` key, but it is created only as an explicit projection
of a canonical record at the fitting boundary.
"""
from __future__ import annotations
import math
from typing import Any, Iterable, Literal, Mapping
import numpy as np
SAMPLE_KINDS = frozenset(
{
'sample',
'baseline_hold_sample',
'steady_command_sample',
'palm_axis_sample',
}
)
FitDomain = Literal['default', 'requested', 'feedback']
class SampleDataContractError(ValueError):
"""A calibration observation mixes or omits command domains."""
def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float:
try:
number = float(value)
except (TypeError, ValueError) as error:
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be numeric'
) from error
if not math.isfinite(number) or not 0.0 <= number <= 255.0:
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]'
)
if integral:
rounded = int(round(number))
if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9):
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be an integer command'
)
return rounded
return number
def explicit_domain_value(
source: Mapping[str, Any], domain: Literal['requested', 'feedback']
) -> int | float:
"""Read and validate one explicitly named domain from any observation."""
field = (
'requested_command_u8' if domain == 'requested' else 'feedback_u8'
)
if field not in source or source[field] is None:
raise SampleDataContractError(
f'DATA-CONTRACT-701:observation is missing explicit {field}'
)
return _finite_u8(source[field], field, integral=domain == 'requested')
def canonical_sample_record(
source: Mapping[str, Any],
*,
allow_legacy_command: bool = False,
) -> dict[str, Any]:
"""Return one durable, unambiguous calibration observation.
``allow_legacy_command`` is restricted to importing historical sessions
and unit fixtures. New online observations must provide both explicit
fields and therefore cannot silently reinterpret ``command_u8``.
"""
record = dict(source)
kind = str(record.get('kind', ''))
if not kind and allow_legacy_command:
# Old in-memory steady-curve fixtures predate durable sample kinds.
# This adapter is never enabled by the new online/import contract.
kind = 'steady_command_sample'
record['kind'] = kind
if kind not in SAMPLE_KINDS:
raise SampleDataContractError(
f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}'
)
requested = record.get('requested_command_u8')
feedback = record.get('feedback_u8')
legacy = record.get('command_u8')
if requested is None or feedback is None:
if not allow_legacy_command or legacy is None:
missing = [
name
for name, value in (
('requested_command_u8', requested),
('feedback_u8', feedback),
)
if value is None
]
raise SampleDataContractError(
'DATA-CONTRACT-701:'
f'{kind} is missing explicit {",".join(missing)}'
)
# Historical in-memory records used requested commands for settled
# checkpoints and feedback bins for dense/baseline/palm observations.
if requested is None:
requested = legacy
if feedback is None:
feedback = legacy
record.pop('command_u8', None)
record['requested_command_u8'] = explicit_domain_value(
{'requested_command_u8': requested}, 'requested'
)
record['feedback_u8'] = explicit_domain_value(
{'feedback_u8': feedback}, 'feedback'
)
return record
def fitting_sample_record(
source: Mapping[str, Any],
*,
domain: FitDomain = 'default',
allow_legacy_command: bool = False,
snap_requested_endpoints: bool = False,
) -> dict[str, Any]:
"""Project a canonical sample into the legacy curve-fitter interface."""
record = canonical_sample_record(
source, allow_legacy_command=allow_legacy_command
)
kind = str(record['kind'])
selected = domain
if selected == 'default':
selected = (
'requested' if kind == 'steady_command_sample' else 'feedback'
)
if selected not in {'requested', 'feedback'}:
raise SampleDataContractError(
f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}'
)
requested = int(record['requested_command_u8'])
if selected == 'requested' or (
snap_requested_endpoints and requested in {0, 255}
):
index = requested
else:
index = int(
np.clip(np.rint(float(record['feedback_u8'])), 0, 255)
)
record['command_u8'] = index
return record
def fitting_sample_records(
records: Iterable[Mapping[str, Any]],
*,
domain: FitDomain = 'default',
allow_legacy_command: bool = False,
snap_requested_endpoints: bool = False,
) -> list[dict[str, Any]]:
"""Project several canonical samples into one explicit fitting domain."""
return [
fitting_sample_record(
record,
domain=domain,
allow_legacy_command=allow_legacy_command,
snap_requested_endpoints=snap_requested_endpoints,
)
for record in records
]
def validate_sample_records(
records: Iterable[Mapping[str, Any]],
*,
allow_legacy_command: bool = False,
) -> None:
"""Validate a collection without changing its representation."""
for index, record in enumerate(records):
try:
canonical_sample_record(
record, allow_legacy_command=allow_legacy_command
)
except SampleDataContractError as error:
raise SampleDataContractError(
f'{error};record_index={index}'
) from error
@@ -0,0 +1,32 @@
"""The declared command nodes used by acquisition and fitting alike."""
import numpy as np
def command_training_cycles(profile, task=None, *, joint=None):
from .capture_plan import task_partition
return task_partition(profile, task, joint=joint).command_training
def command_holdout_cycle(profile):
from .capture_plan import task_partition
return task_partition(profile).command_holdout
def command_nodes(profile, task, *, holdout=False):
lo, hi = sorted((task.start_value, task.end_value))
train = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
train = np.unique(np.r_[train, profile.acquisition.steady_extra_training_nodes.get(task.key, ())])
if profile.artifacts.output_schema_version >= 3:
from .motion_path import task_channels
baselines = [profile.command.baseline_values[i] for i in task_channels(task)]
if any(not lo <= baseline <= hi for baseline in baselines):
raise ValueError(f"baseline_outside_calibrated_support:{task.key}")
train = np.unique(np.r_[train, baselines])
if task.segments:
boundaries = [v for s in task.segments for i, v in (*s.start_commands, *s.end_commands)]
train = np.unique(np.r_[train, boundaries])
values = np.r_[lo, (train[:-1]+train[1:])/2, hi] if holdout else train
if profile.command.unit == "u8":
values = np.floor(values+.5)
return tuple(sorted(set(float(v) for v in values)))
@@ -0,0 +1,85 @@
"""Model-independent progress and failure status contracts."""
from __future__ import annotations
from dataclasses import asdict, dataclass, field
from typing import Any, Mapping
@dataclass(frozen=True)
class TaskStatus:
key: str = ""
label: str = ""
cycle: int | None = None
cycle_count: int = 4
direction: str = ""
capture_stage: str = "motion"
required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field(
default_factory=dict
)
segment_key: str = ""
segment_commands: Mapping[str, tuple[float, float]] = field(default_factory=dict)
@dataclass(frozen=True)
class MotionStatus:
channel: str = ""
command: float | None = None
feedback: float | None = None
unit: str = ""
speed: float | None = None
@dataclass(frozen=True)
class AcquisitionStatus:
valid_samples: int = 0
minimum_samples: int = 40
coverage_01: float = 0.0
bins: int = 0
maximum_gap: int = 0
retry_count: int = 0
warnings: tuple[str, ...] = ()
@dataclass(frozen=True)
class PauseStatus:
code: str = ""
reason: str = ""
suggestion: str = ""
details: Mapping[str, Any] = field(default_factory=dict)
@dataclass(frozen=True)
class CalibrationStatus:
"""Stable payload rendered by the one common product runner."""
schema_version: int = 1
state: str = "WAIT_DEVICE"
phase: str = ""
overall_progress_01: float = 0.0
estimated_remaining_seconds: float | None = None
reference_locked: bool = False
reference_message: str = ""
task: TaskStatus = field(default_factory=TaskStatus)
recognized_tag_ids: tuple[int, ...] = ()
missing_tag_ids: tuple[int, ...] = ()
rejected_tag_ids: tuple[int, ...] = ()
motion: MotionStatus = field(default_factory=MotionStatus)
acquisition: AcquisitionStatus = field(default_factory=AcquisitionStatus)
resume: Mapping[str, Any] = field(default_factory=dict)
hardware: Mapping[str, Any] = field(default_factory=dict)
pause: PauseStatus = field(default_factory=PauseStatus)
log_path: str = ""
outputs: Mapping[str, str] = field(default_factory=dict)
def as_dict(self) -> dict[str, Any]:
return asdict(self)
__all__ = [
"AcquisitionStatus",
"CalibrationStatus",
"MotionStatus",
"PauseStatus",
"TaskStatus",
]
@@ -0,0 +1,19 @@
"""Shared task direction vocabulary."""
DIRECTION_DECREASING = "decreasing"
DIRECTION_INCREASING = "increasing"
DIRECTIONS: tuple[str, ...] = (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
)
PHASE_ROOT = "root"
PHASE_TIP = "tip"
__all__ = [
"DIRECTION_DECREASING",
"DIRECTION_INCREASING",
"DIRECTIONS",
"PHASE_ROOT",
"PHASE_TIP",
]
@@ -0,0 +1,19 @@
"""Pure curve and axis fitting."""
from .curve import FitResult, isotonic_nonincreasing
from .coupling import (
CouplingFit, LinearMimicFit, PairedJointObservation, RelativeMimicEvidence,
curve_travel_rad, fit_coupling_model, fit_standard_mimic, fit_relative_standard_mimic,
)
from .rotation_curve import RotationCurve, RotationObservation, fit_rotation_curve
__all__ = [
"CouplingFit",
"FitResult",
"curve_travel_rad",
"fit_coupling_model",
"isotonic_nonincreasing",
"LinearMimicFit", "PairedJointObservation", "RelativeMimicEvidence",
"fit_standard_mimic", "fit_relative_standard_mimic",
"RotationCurve", "RotationObservation", "fit_rotation_curve",
]
@@ -0,0 +1,22 @@
"""Training-only monotonic projection with an exact, already observed zero."""
import math
import numpy as np
from .curve import isotonic_nonincreasing
def anchored_mean(knots, increasing, decreasing, baseline, sign):
x = np.asarray(knots, dtype=float)
values = (np.asarray(increasing) + decreasing) / 2
indices = np.flatnonzero(np.isclose(x, baseline, atol=1e-9, rtol=0))
if len(indices) != 1:
raise ValueError("baseline_requires_an_observed_training_knot")
i = int(indices[0])
oriented = values * sign
left = np.minimum(-isotonic_nonincreasing(-oriented[:i]), 0) if i else []
right = np.maximum(-isotonic_nonincreasing(-oriented[i+1:]), 0) if i+1 < len(x) else []
fitted = np.r_[left, 0.0, right] * sign
if np.max(np.abs(fitted-values)) > math.radians(1):
raise ValueError("baseline_constraint_inconsistent_with_training")
return tuple(float(v) for v in fitted)
@@ -0,0 +1,144 @@
"""Generic circular statistics and robust image-plane circle fitting."""
from __future__ import annotations
import math
from typing import Sequence
import numpy as np
from scipy.optimize import least_squares
def wrap_angle_rad(angle_rad: float) -> float:
"""Wrap one finite angle to [-pi, pi)."""
angle = float(angle_rad)
if not math.isfinite(angle):
raise ValueError("angle must be finite")
return (angle + math.pi) % (2.0 * math.pi) - math.pi
def signed_angle_difference_rad(angle_rad: float, reference_rad: float) -> float:
"""Return the wrapped signed rotation from reference to angle."""
return wrap_angle_rad(float(angle_rad) - float(reference_rad))
def circular_mean_rad(angles_rad: Sequence[float]) -> float:
"""Return the circular mean of at least one finite angle."""
angles = np.asarray(angles_rad, dtype=float)
if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)):
raise ValueError("angles_rad must contain finite angles")
vector = complex(
float(np.mean(np.cos(angles))),
float(np.mean(np.sin(angles))),
)
if abs(vector) < 1.0e-12:
raise ValueError("angles do not have a unique circular mean")
return wrap_angle_rad(math.atan2(vector.imag, vector.real))
def circular_median_rad(angles_rad: Sequence[float]) -> float:
"""Return a robust circular median around the circular mean."""
angles = np.asarray(angles_rad, dtype=float)
reference = circular_mean_rad(angles)
deltas = np.asarray(
[signed_angle_difference_rad(value, reference) for value in angles],
dtype=float,
)
return wrap_angle_rad(reference + float(np.median(deltas)))
def circular_std_rad(angles_rad: Sequence[float]) -> float:
"""Return circular standard deviation in radians."""
angles = np.asarray(angles_rad, dtype=float)
if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)):
raise ValueError("angles_rad must contain finite angles")
resultant = float(
math.hypot(
float(np.mean(np.cos(angles))),
float(np.mean(np.sin(angles))),
)
)
resultant = min(1.0, max(1.0e-15, resultant))
return math.sqrt(max(0.0, -2.0 * math.log(resultant)))
def maximum_pairwise_angle_difference_rad(
angles_rad: Sequence[float],
) -> float:
"""Return the largest wrapped distance between any two angles."""
angles = [float(value) for value in angles_rad]
if not angles or not all(math.isfinite(value) for value in angles):
raise ValueError("angles_rad must contain finite angles")
return max(
(
abs(signed_angle_difference_rad(left, right))
for index, left in enumerate(angles)
for right in angles[index + 1:]
),
default=0.0,
)
def _fit_circle(points_xy: np.ndarray) -> tuple[np.ndarray, float]:
"""Fit a geometric circle with robust nonlinear least squares."""
points = np.asarray(points_xy, dtype=float)
if points.ndim != 2 or points.shape[1] != 2 or len(points) < 3:
raise ValueError("circle fit requires at least three 2D points")
origin = np.mean(points, axis=0)
centered = points - origin
matrix = np.column_stack(
(2.0 * centered[:, 0], 2.0 * centered[:, 1], np.ones(len(points)))
)
rhs = np.sum(np.square(centered), axis=1)
solution, _, rank, _ = np.linalg.lstsq(matrix, rhs, rcond=None)
if rank < 3:
raise ValueError("trajectory points are degenerate")
centre0 = solution[:2] + origin
radius0_squared = (
float(solution[2]) + float(np.dot(solution[:2], solution[:2]))
)
if radius0_squared <= 0.0:
raise ValueError("trajectory circle radius is invalid")
radius0 = math.sqrt(radius0_squared)
def residual(parameters: np.ndarray) -> np.ndarray:
centre = parameters[:2]
radius = parameters[2]
return np.linalg.norm(points - centre, axis=1) - radius
initial_error = residual(np.asarray([*centre0, radius0], dtype=float))
robust_scale = max(
0.25,
1.4826
* float(
np.median(
np.abs(initial_error - float(np.median(initial_error)))
)
),
)
result = least_squares(
residual,
np.asarray([*centre0, radius0], dtype=float),
loss="soft_l1",
f_scale=robust_scale,
max_nfev=1000,
)
centre = np.asarray(result.x[:2], dtype=float)
radius = float(abs(result.x[2]))
if not result.success or not np.all(np.isfinite(centre)):
raise ValueError("trajectory circle optimization failed")
if not math.isfinite(radius) or radius <= 0.0:
raise ValueError("trajectory circle radius is invalid")
return centre, radius
def _trajectory_arc_rad(points_xy: np.ndarray, centre_xy: np.ndarray) -> float:
angles = np.mod(
np.arctan2(
points_xy[:, 1] - centre_xy[1],
points_xy[:, 0] - centre_xy[0],
),
2.0 * math.pi,
)
if len(angles) < 2:
return 0.0
angles = np.sort(angles)
gaps = np.diff(np.r_[angles, angles[0] + 2.0 * math.pi])
return float(2.0 * math.pi - np.max(gaps))
@@ -0,0 +1,168 @@
"""Steady command calibration against independently observed Tag angles.
Feedback is NOT a command label and is never the target of this regression.
The visual reference/axis was frozen by the continuous training acquisition.
"""
from dataclasses import asdict, replace
import math
import numpy as np
from ..domain.result import JointMapping
from ..urdf.acceptance import angular_metrics
from .curve import isotonic_nonincreasing
from .spatial import measure_joint_curve_observation
from ..domain.sampling import command_nodes, command_training_cycles, command_holdout_cycle
from ..domain.motion_path import task_channels
def fit_command_mappings(profile, fit, records_by_joint):
mappings, metrics, applicability = fit_command_observations(profile, fit.curves, records_by_joint,
channels={name: mapping.motor_index for name, mapping in fit.output_mappings.items()},
reference_inputs=fit.reference_inputs, zero_references=fit.joint_zero_references)
return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics,
command_applicability=applicability)
def fit_command_training(profile, curves, records_by_joint, *, channels, reference_inputs, zero_references):
"""Fit stable command labels to angles measured in frozen visual frames."""
mappings, metrics, applicability = {}, {}, {}
holdout_cycle = command_holdout_cycle(profile)
for joint in sorted(channels):
donor = joint if profile.artifacts.output_schema_version >= 3 else profile.zero.transferred_zero_sources.get(joint, joint)
rows = records_by_joint.get(donor, ())
if not rows or any(r.get("sample_phase") != "steady" for r in rows):
raise ValueError(f"steady_command_observations_missing:{joint}")
training_cycles = command_training_cycles(profile, joint=donor)
if any(r["cycle"] not in training_cycles for r in rows):
raise ValueError("command_training_received_nontraining_data")
curve = curves[donor]
gauge = 0.0
# Byte curves already carry their shared coordinate reference. Native
# curves retain the arbitrary observed Tag reference in fit.curves.
if profile.command.unit != "u8" and profile.artifacts.output_schema_version < 3:
gauge = float(np.interp(reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad))
observed = {r["sample_id"]: measure_joint_curve_observation(curve,
quaternion_xyzw=r.get("relative_quaternion_xyzw"),
image_relative_xy_px=r.get("image_relative_xy_px"))-gauge for r in rows}
if len(observed) != len(rows):
raise ValueError("duplicate steady visual image")
channel = channels[joint]
source_channel = channels[donor]
branches = {}
maximum_correction = 0.0
directional = joint in profile.artifacts.directional_command_joints
task = next(t for t in profile.motion.tasks if donor in t.joints)
from .command_sampling import declared_training_nodes
knots = declared_training_nodes(profile, task, rows)
sign = profile.command.joint_directions[source_channel]
if profile.artifacts.output_schema_version >= 3 and not profile.command_based_release:
sign = 1 if curve.angle_rad[-1] > curve.angle_rad[0] else -1
for direction in ("increasing", "decreasing"):
values = []
for knot in knots:
medians = []
for cycle in training_cycles:
selected = [r for r in rows if r["cycle"] == cycle and r["direction"] == direction
and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9)]
if len(selected) < profile.acquisition.steady_minimum_samples:
raise ValueError(f"steady_training_incomplete:{joint}:{cycle}:{direction}:{knot}")
for r in selected:
if not math.isclose(float(r[f"command_{profile.command.unit}"]), knot, abs_tol=1e-9):
raise ValueError("steady observation captured during command motion")
medians.append(float(np.median([observed[r["sample_id"]] for r in selected])))
values.append(float(np.median(medians)))
values = np.asarray(values)
projected = -sign*isotonic_nonincreasing(-sign*values)
maximum_correction = max(maximum_correction, float(np.max(np.abs(projected-values))))
if maximum_correction > math.radians(1):
raise ValueError(f"steady_command_not_monotonic:{joint}:{direction}")
branches[direction] = tuple(float(v) for v in projected)
if directional:
from .anchored_mapping import anchored_mean
branches = {direction: anchored_mean(knots, values, values,
zero_references[joint].baseline_command, sign)
for direction, values in branches.items()}
mean = tuple((a+b)/2 for a, b in zip(branches["increasing"], branches["decreasing"]))
if profile.artifacts.output_schema_version >= 3:
from .anchored_mapping import anchored_mean
mean = anchored_mean(knots, branches["increasing"], branches["decreasing"],
zero_references[joint].baseline_command, sign)
mapping = JointMapping(joint, channel, f"command_{profile.command.unit}", knots,
mean, branches["increasing"], branches["decreasing"], donor if donor != joint else None)
mappings[joint] = mapping
metrics[joint] = {"maximum_monotonic_correction_rad": maximum_correction,
"independently_measured": donor == joint, "source_joint": donor,
"training_cycles": list(training_cycles), "holdout_cycle": holdout_cycle,
"training_sample_ids": sorted(r["sample_id"] for r in rows)}
training = [r for r in rows if r["cycle"] in training_cycles]
vectors = np.asarray([r[f"command_vector_{profile.command.unit}"] for r in training])
held = {str(i): float(np.median(vectors[:, i])) for i in range(profile.command.command_count)
if i not in task_channels(task)}
for i, value in held.items():
if np.max(np.abs(vectors[:, int(i)]-value)) > 1e-8:
raise ValueError(f"scan_changed_multiple_command_channels:{donor}:{i}")
applicability[joint] = {"task": task.key, "held_command_values": held,
"scope": "declared_multi_channel_path" if task.segments else "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False,
"branch_initialization": "recorded_baseline_then_endpoint_resets" if task.segments else "full_range_endpoint_reset"}
if task.segments:
applicability[joint]["motion_segments"] = [asdict(s) for s in task.segments]
if directional:
applicability[joint].update(command_mapping="directional", direction_source="raw_sdk_input",
interior_reversals_validated=False)
return mappings, metrics, applicability
def validate_command_training(profile, curves, mappings, metrics, records_by_joint, *, reference_inputs):
"""Evaluate independent images against already frozen frames and mappings."""
holdout_cycle = command_holdout_cycle(profile)
accepted = {}
for joint, mapping in mappings.items():
donor = metrics[joint]["source_joint"]
curve = curves[donor]
rows = records_by_joint.get(donor, ())
if not rows or any(r["cycle"] != holdout_cycle or r.get("sample_phase") != "steady" for r in rows):
raise ValueError(f"steady_holdout_incomplete:{joint}")
ids = [r["sample_id"] for r in rows]
trained = set(metrics[joint]["training_sample_ids"]) | set(curve.circle.get("training_sample_ids", ()))
if len(ids) != len(set(ids)) or trained.intersection(ids):
raise ValueError("command_holdout_image_identity_overlaps_training")
task = next(t for t in profile.motion.tasks if donor in t.joints)
for direction in ("increasing", "decreasing"):
for knot in command_nodes(profile, task, holdout=True):
if sum(r["direction"] == direction and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9)
for r in rows) < profile.acquisition.steady_minimum_samples:
raise ValueError(f"steady_holdout_incomplete:{joint}:{direction}:{knot}")
gauge = (float(np.interp(reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad))
if profile.command.unit != "u8" and profile.artifacts.output_schema_version < 3 else 0.)
directional = joint in profile.artifacts.directional_command_joints
errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]),
r["direction"] if directional or profile.artifacts.output_schema_version < 3 else "")
- measure_joint_curve_observation(curve, quaternion_xyzw=r.get("relative_quaternion_xyzw"),
image_relative_xy_px=r.get("image_relative_xy_px")) + gauge for r in rows]
quality = angular_metrics(errors)
if not quality.passed:
raise ValueError(f"steady_command_holdout_failed:{joint}:{quality}")
accepted[joint] = {**metrics[joint], **asdict(quality)}
if profile.artifacts.output_schema_version >= 3:
branches = {direction: angular_metrics([error for error, row in zip(errors, rows)
if row["direction"] == direction]) for direction in ("increasing", "decreasing")}
if not all(value.passed for value in branches.values()):
raise ValueError(f"command_curve_direction_holdout_failed:{joint}")
accepted[joint]["by_direction"] = {key: asdict(value) for key, value in branches.items()}
return accepted
def fit_command_observations(profile, curves, records_by_joint, *, channels, reference_inputs, zero_references):
training, validation = {}, {}
for name, rows in records_by_joint.items():
cycles = command_training_cycles(profile, joint=name)
holdout = command_holdout_cycle(profile)
if any(row["cycle"] not in (*cycles, holdout) for row in rows):
raise ValueError("command_observation_outside_capture_plan")
training[name] = [row for row in rows if row["cycle"] in cycles]
validation[name] = [row for row in rows if row["cycle"] == holdout]
mappings, metrics, applicability = fit_command_training(profile, curves, training,
channels=channels, reference_inputs=reference_inputs, zero_references=zero_references)
return mappings, validate_command_training(profile, curves, mappings, metrics, validation,
reference_inputs=reference_inputs), applicability
@@ -0,0 +1,114 @@
"""Command-certified motion with an independent, non-blocking feedback diagnostic.
Moving images determine only a hinge frame. Stable commanded poses train the
control curves. A fourth cycle never contributes to either training stage.
"""
from dataclasses import asdict, dataclass, replace
import numpy as np
from ..domain.measurement import JointCurveFit
from ..urdf.acceptance import angular_metrics
from .command_mapping import fit_command_training as train_commands, validate_command_training
from ..domain.capture_plan import training_cycles, LEGACY_TRAINING, LEGACY_HOLDOUT
from ..domain.sampling import command_training_cycles, command_holdout_cycle
from .motion_fit import channel_for_joint, joint_input_direction
from .observed_motion import image_identity
from .rotation_curve import RotationObservation, fit_rotation_frame, fit_rotation_curve
def rotation_observations(rows, domain):
return [RotationObservation(image_identity(row), float(row[domain]),
tuple(row['relative_quaternion_xyzw']), int(row['cycle']), str(row['direction'])) for row in rows]
def feedback_diagnostic(rows, *, domain, sign, reference, cycles=LEGACY_TRAINING, holdout_cycle=LEGACY_HOLDOUT):
result = {'required_for_release': False, 'input_domain': domain, 'training_cycles': list(cycles),
'holdout_cycle': holdout_cycle, 'extrapolation': 'reject'}
try:
if any(row.get(domain) is None or not np.isfinite(row[domain]) for row in rows):
raise ValueError('position_feedback_missing_or_invalid')
observations = rotation_observations(rows, domain)
curve = fit_rotation_curve([r for r in observations if r.cycle in cycles],
input_domain=domain, sdk_to_joint_direction=sign, reference_xyzw=reference,
training_cycles=cycles, holdout_cycle=holdout_cycle)
held = [r for r in observations if r.cycle == holdout_cycle]
supported = [r for r in held if curve.knots[0] <= r.input_value <= curve.knots[-1]]
outside = [r for r in held if not curve.knots[0] <= r.input_value <= curve.knots[-1]]
quality = angular_metrics(curve.holdout_errors(supported)) if supported else None
metrics = {**asdict(quality), 'passed': quality.passed} if quality else None
result.update(status='passed' if held and not outside and metrics and metrics['passed'] else 'incomplete_or_inaccurate',
training_support=[curve.knots[0], curve.knots[-1]], holdout_samples=len(held),
outside_training_support=len(outside), outside_image_ids=[r.sample_id for r in outside],
supported_holdout_metrics=metrics, mapping={**asdict(curve),
"training_sample_ids": sorted(curve.training_sample_ids)})
except (ValueError, TypeError) as error:
result.update(status='unavailable', reason=str(error))
return result
@dataclass(frozen=True)
class CommandMotionFit:
observed_curves: dict
coordinate_curves: dict
mappings: dict
command_metrics: dict
applicability: dict
reference_inputs: dict
feedback_diagnostics: dict
@property
def holdout_errors_rad(self):
# Command errors are retained with their per-direction metrics.
return {}
@property
def cross_view_metrics(self):
return {}
def fit_command_training(profile, source_model, sweep, steady, references):
"""Training-only motion API. Passing a holdout image is a contract error."""
if set(sweep) != set(steady) or set(sweep) != set(references):
raise ValueError('command_motion_observation_coverage_differs')
frames = {}
channels = {name: channel_for_joint(profile, name) for name in sweep}
for name, rows in sweep.items():
cycles = training_cycles(profile, joint=name)
if any(row['cycle'] not in cycles for row in rows):
raise ValueError('motion_training_received_nontraining_data')
view = profile.measurement.measurements[name].view
frames[name] = fit_rotation_frame(rotation_observations(rows, f'command_{profile.command.unit}'),
sdk_to_joint_direction=joint_input_direction(profile, source_model, name),
reference_xyzw=references[name].quaternion(view), training_cycles=cycles)
inputs = {name: references[name].baseline_command for name in frames}
mappings, metrics, applicability = train_commands(profile, frames, steady,
channels=channels, reference_inputs=inputs, zero_references=references)
curves = {}
for name, mapping in mappings.items():
curves[name] = JointCurveFit(mapping.angle_rad, mapping.decreasing_rad, mapping.increasing_rad,
{**frames[name].circle, 'input_domain': mapping.input_domain, 'input_knots': mapping.knots,
'mapping_training_sample_ids': metrics[name]['training_sample_ids']},
metrics[name]["maximum_monotonic_correction_rad"],
float(np.max(np.abs(np.array(mapping.increasing_rad)-mapping.decreasing_rad))), {})
return CommandMotionFit(curves, curves, mappings, metrics, applicability, inputs, {})
def fit_command_motion(profile, source_model, sweep, steady, references, *, frozen_motion=None):
train_sweep = {name: [r for r in rows if r['cycle'] in training_cycles(profile, joint=name)]
for name, rows in sweep.items()}
train_steady = {name: [r for r in rows if r['cycle'] in command_training_cycles(profile, joint=name)]
for name, rows in steady.items()}
motion = frozen_motion or fit_command_training(profile, source_model, train_sweep, train_steady, references)
held = {name: [r for r in rows if r['cycle'] == command_holdout_cycle(profile)]
for name, rows in steady.items()}
metrics = validate_command_training(profile, motion.coordinate_curves,
motion.mappings, motion.command_metrics, held, reference_inputs=motion.reference_inputs)
diagnostics = {}
for name, rows in sweep.items():
view = profile.measurement.measurements[name].view
diagnostics[name] = feedback_diagnostic(rows,
domain=f'feedback_{profile.command.unit}', sign=joint_input_direction(profile, source_model, name),
reference=references[name].quaternion(view), cycles=training_cycles(profile, joint=name),
holdout_cycle=profile.quality.holdout_cycle)
return replace(motion, command_metrics=metrics, feedback_diagnostics=diagnostics)
@@ -0,0 +1,149 @@
"""Training-only reduction of static command knots with immutable evidence.
The continuous curve chooses where to stop, never supplies a static command
angle. Every retained static knot is measured afresh in both directions. The
original independent validation grid and all publication gates remain intact.
"""
import hashlib
import json
import math
import numpy as np
from ..domain.sampling import command_nodes
from ..domain.profile import observation_streams
from .rotation_curve import RotationObservation, fit_rotation_curve
from ..domain.motion_path import record_scan_identity, segment_metadata
from ..domain.capture_plan import training_cycles
SAMPLING_POLICY = "training_shape_static_knots_v1"
MAXIMUM_REMOVAL_ERROR_RAD = math.radians(.25)
def digest(value):
return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest()
def training_sources(profile, task, records):
cycles = training_cycles(profile, task)
streams = {(field, name, spec.view) for field, name, spec in observation_streams(profile, task)}
latest = {}
for row in records:
if row.get("task_name") == task.key and row.get("cycle") in cycles and row.get("direction"):
key = record_scan_identity(row)
latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1)))
result = []
for row in records:
if (row.get("task_name") != task.key or row.get("cycle") not in cycles
or row.get("sample_phase", "sweep") != "sweep" or "relative_quaternion_xyzw" not in row):
continue
if int(row.get("attempt", 1)) != latest.get(record_scan_identity(row)):
continue
for field, name, view in sorted(streams):
if row.get(field) == name and row.get("view") == view:
result.append({"field": field, "joint": name, "view": view, "cycle": row["cycle"],
"direction": row["direction"], "image_stamp_ns": row["image_stamp_ns"],
**segment_metadata(row.get("segment_key", "")),
"input": float(row[f"{'command' if profile.command_based_release else 'feedback'}_{profile.command.unit}"]),
"quaternion": list(row["relative_quaternion_xyzw"])})
return sorted(result, key=lambda row: (row["field"], row["joint"], row["view"], row["image_stamp_ns"]))
def reduced_knots(grid, curves, mandatory):
"""Shortest path through the original grid; each omitted knot is bounded."""
grid, values = np.asarray(grid), np.asarray(curves)
paths = {0: (0,)}
for end in range(1, len(grid)):
candidates = []
for start, path in paths.items():
if any(start < index < end for index in mandatory):
continue
fraction = (grid[start:end+1]-grid[start])/(grid[end]-grid[start])
prediction = values[:, start, None] + (values[:, end]-values[:, start])[:, None]*fraction
if np.max(np.abs(prediction-values[:, start:end+1])) <= MAXIMUM_REMOVAL_ERROR_RAD:
candidates.append((*path, end))
paths[end] = min(candidates, key=lambda path: (len(path), path))
return tuple(float(grid[index]) for index in paths[len(grid)-1])
def build_sampling_plan(profile, task, records):
sources = training_sources(profile, task, records)
grid = command_nodes(profile, task)
knots, reason = grid, "full_grid_insufficient_training_shape"
curves = []
if task.segments:
reason = "segmented_motion_preserves_full_per_joint_grid"
else:
try:
for field, name, spec in observation_streams(profile, task):
rows = [row for row in sources if (row["field"], row["joint"], row["view"]) == (field, name, spec.view)]
observations = [RotationObservation(f"{row['view']}:{row['image_stamp_ns']}", row["input"],
tuple(row["quaternion"]), row["cycle"], row["direction"]) for row in rows]
fitted = fit_rotation_curve(observations, input_domain=f"{'command' if profile.command_based_release else 'feedback'}_{profile.command.unit}",
sdk_to_joint_direction=profile.command.joint_directions[task.command_index],
training_cycles=training_cycles(profile, task), holdout_cycle=profile.quality.holdout_cycle)
if fitted.maximum_monotonic_correction_rad > MAXIMUM_REMOVAL_ERROR_RAD:
raise ValueError("training_shape_noisy")
for branch in (fitted.increasing_rad, fitted.decreasing_rad):
curves.append(np.interp(grid, fitted.knots, branch))
mandatory_values = {grid[0], grid[-1], grid[len(grid)//2],
profile.command.baseline_values[task.command_index],
*profile.acquisition.steady_extra_training_nodes.get(task.key, ())}
mandatory = {i for i, value in enumerate(grid) if value in mandatory_values}
knots = reduced_knots(grid, curves, mandatory)
reason = "training_shape_bounded" if len(knots) < len(grid) else "full_grid_required_by_training_shape"
except (ValueError, KeyError, TypeError, np.linalg.LinAlgError) as error:
reason = "full_grid:" + str(error)
plan = {"kind": "command_sampling_plan", "policy": SAMPLING_POLICY, "task_name": task.key,
"training_nodes": list(knots), "validation_nodes": list(command_nodes(profile, task, holdout=True)),
"source_sha256": digest(sources), "source_count": len(sources), "training_cycles": list(training_cycles(profile, task)),
"maximum_removal_error_rad": MAXIMUM_REMOVAL_ERROR_RAD, "reason": reason}
return {**plan, "plan_sha256": digest(plan)}
def declared_training_nodes(profile, task, records):
"""Read a plan in complete journals or self-contained individual units."""
declarations = set()
for row in records:
if row.get("task_name") != task.key:
continue
if row.get("kind") == "command_sampling_plan":
if digest({k: v for k, v in row.items() if k != "plan_sha256"}) != row.get("plan_sha256"):
raise ValueError("command_sampling_plan_hash_changed")
declarations.add(tuple(row["training_nodes"]))
elif row.get("sample_phase") == "steady" and "steady_training_nodes" in row:
declarations.add(tuple(row["steady_training_nodes"]))
if len(declarations) > 1:
raise ValueError("command_sampling_plan_conflict")
if not declarations:
return command_nodes(profile, task)
nodes = next(iter(declarations))
allowed = set(command_nodes(profile, task))
if (len(nodes) < 3 or tuple(sorted(set(nodes))) != nodes or not set(nodes) <= allowed
or nodes[0] != min(allowed) or nodes[-1] != max(allowed)):
raise ValueError("command_sampling_nodes_invalid")
return nodes
def validate_sampling_plans(profile, records):
plans = {}
for row in records:
if row.get("kind") == "command_sampling_plan":
task = next(task for task in profile.motion.tasks if task.key == row["task_name"])
if row != build_sampling_plan(profile, task, records):
raise ValueError("command_sampling_training_evidence_changed")
if task.key in plans and plans[task.key] != row:
raise ValueError("command_sampling_plan_conflict")
plans[task.key] = row
required = any(row.get("command_sampling_policy") == SAMPLING_POLICY for row in records)
for row in records:
if row.get("sample_phase") != "steady" or "steady_target" not in row:
continue
plan = plans.get(row["task_name"])
if required or "sampling_plan_sha256" in row:
if (plan is None or row.get("sampling_plan_sha256") != plan["plan_sha256"]
or list(row.get("steady_training_nodes", ())) != plan["training_nodes"]):
raise ValueError("command_sampling_sample_unbound")
return plans
@@ -0,0 +1,77 @@
"""Training-only support for the physically shared turnaround endpoints.
The last incoming command owns the turnaround image. An unobserved outgoing
endpoint is a continuity constraint, never a second independent measurement.
Interior parameters and endpoints with actual directional evidence stay free.
"""
import numpy as np
TURNAROUND_POLICY = 'observed_turnaround_continuity_v1'
def compile_turnaround_constraints(bundle):
constraints = {}
for name, columns in bundle.curve_columns.items():
channel = bundle.channels[name]
indices = sorted({int(index) for role, data in bundle.observations.items()
if any(joint.name == bundle.cad_names[name] for joint in bundle.chains[role])
for index in data[0]})
if not indices:
continue
commands = bundle.commands[indices, channel]
directions = bundle.directions[indices, channel]
for node in (0, len(bundle.knots[name])-1):
for recipient in (0, 1):
column = int(columns[recipient, node])
# Unknown startup history observes the common endpoint; it
# does not independently identify the outgoing branch.
at_endpoint = commands == bundle.knots[name][node]
if column < 0 or np.any(at_endpoint & (directions == recipient)):
continue
donor = 1-recipient
sources = [bundle.rows[index] for index in indices
if bundle.commands[index, channel] == bundle.knots[name][node]
and bundle.directions[index, channel] == donor]
if not sources:
continue
constraints.setdefault(name, []).append(dict(node=node, recipient=recipient,
donor=donor, column=column, command=float(bundle.knots[name][node]),
source_sample_ids=sorted({f"{row['view']}:{row['image_stamp_ns']}" for row in sources}),
source_cycles=sorted({row['cycle'] for row in sources})))
return constraints
def validate_turnaround_mapping(mapping, support):
"""Read-back must preserve the saved constraint, including its sources."""
if support is None: # Historical artifacts retain their old contract.
return
if support.get('policy') != TURNAROUND_POLICY or not isinstance(support.get('constraints'), list):
raise ValueError('invalid_turnaround_support:' + mapping.joint)
endpoints = support.get('endpoints', ())
if (len(endpoints) != 2 or [row.get('command') for row in endpoints]
!= [mapping.knots[0], mapping.knots[-1]]):
raise ValueError('invalid_turnaround_endpoints:' + mapping.joint)
for endpoint in endpoints:
if not endpoint.get('samples') or any(
sample.get('cycle') not in (0, 1) or not isinstance(sample.get('id'), str)
or not sample['id'] or sample.get('direction') not in ('', 'increasing', 'decreasing')
for sample in endpoint['samples']):
raise ValueError('invalid_turnaround_samples:' + mapping.joint)
branches = (mapping.increasing_rad, mapping.decreasing_rad)
seen = set()
for row in support['constraints']:
node, recipient, donor = row['node'], row['recipient'], row['donor']
if (node not in (0, len(mapping.knots)-1) or recipient not in (0, 1)
or donor != 1-recipient or (node, recipient) in seen
or row['command'] != mapping.knots[node]
or not row['source_sample_ids'] or not row['source_cycles']
or not set(row['source_cycles']) <= {0, 1}
or abs(branches[recipient][node]-branches[donor][node]) > 1e-10):
raise ValueError('turnaround_mapping_differs_from_evidence:' + mapping.joint)
seen.add((node, recipient))
endpoint = endpoints[0 if node == 0 else 1]
observed = {sample['id'] for sample in endpoint['samples']
if sample['direction'] == ('increasing', 'decreasing')[donor]}
if not observed.intersection(row['source_sample_ids']):
raise ValueError('turnaround_source_not_observed:' + mapping.joint)
@@ -0,0 +1,388 @@
"""Model-independent active/passive joint coupling fitting."""
from __future__ import annotations
from dataclasses import dataclass
import math
from typing import Protocol, Sequence
import numpy as np
from scipy.optimize import least_squares
class DirectionalCurve(Protocol):
decreasing_rad: Sequence[float]
increasing_rad: Sequence[float]
@dataclass(frozen=True)
class LinearMimicFit:
"""The one relationship that a standard URDF can actually serialize."""
source_joint: str
target_joint: str
multiplier: float
offset_rad: float
offset_observed: bool
training_cycles: tuple[int, ...]
residuals_rad: tuple[float, ...]
@dataclass(frozen=True)
class PairedJointObservation:
"""Two independent visual angles from the same image and frozen mounts."""
sample_id: str
cycle: int
direction: str
source_rad: float
target_rad: float
@dataclass(frozen=True)
class RelativeMimicEvidence:
fit: LinearMimicFit
# Nuisance phase between the two frozen visual references, NOT a URDF zero.
installation_phase_rad: float
training_sample_ids: tuple[str, ...]
holdout_sample_ids: tuple[str, ...]
holdout_errors_rad: tuple[float, ...]
def fit_relative_standard_mimic(
source_joint: str, target_joint: str,
observations: Sequence[PairedJointObservation], *, cad_offset_rad: float,
) -> RelativeMimicEvidence:
"""Fit a standard linear mimic from paired visual motion, not SDK priors.
Unknown mounting angles make the absolute passive intercept unobservable.
A training-only nuisance phase aligns the *visual references*, while the
published CAD intercept is preserved. It cannot be re-estimated on holdout
or used as an origin/mimic correction. Both directions share one slope.
"""
rows = tuple(observations)
identities = [row.sample_id for row in rows]
if any(not value for value in identities) or len(set(identities)) != len(identities):
raise ValueError("paired mimic image identities are missing or duplicated")
if any(row.cycle not in (0, 1, 2, 3) or row.direction not in {"increasing", "decreasing"}
or not math.isfinite(row.source_rad) or not math.isfinite(row.target_rad)
for row in rows):
raise ValueError("invalid paired mimic observation")
for cycle in range(4):
for direction in ("increasing", "decreasing"):
if sum(row.cycle == cycle and row.direction == direction for row in rows) < 40:
raise ValueError(f"missing paired mimic observations: {target_joint}:{cycle}:{direction}")
training = tuple(row for row in rows if row.cycle != 3)
holdout = tuple(row for row in rows if row.cycle == 3)
relative_fit = fit_standard_mimic(source_joint, target_joint,
[row.source_rad for row in training], [row.target_rad for row in training],
cycles=[row.cycle for row in training], offset_rad=None, offset_observable=True)
errors = tuple(relative_fit.multiplier * row.source_rad + relative_fit.offset_rad - row.target_rad
for row in holdout)
# Final-file FK remains a separate mandatory gate; this is the prerequisite
# that a single standard mimic can even explain the independent motion.
for label, residuals in (("training", relative_fit.residuals_rad), ("holdout", errors)):
degrees = np.rad2deg(np.abs(residuals))
if np.mean(degrees) > 1 or np.percentile(degrees, 95) > 2 or np.max(degrees) > 3:
raise ValueError(
f"standard_urdf_mimic_not_expressive:joint={target_joint}:phase={label}:"
f"mae_deg={np.mean(degrees):.6f}:p95_deg={np.percentile(degrees, 95):.6f}:"
f"maximum_deg={np.max(degrees):.6f}; 标准 URDF 线性 mimic 表达能力不足或观测不一致"
)
if not math.isfinite(cad_offset_rad):
raise ValueError("passive CAD offset must be finite")
fit = LinearMimicFit(source_joint, target_joint, relative_fit.multiplier,
float(cad_offset_rad), False, (0, 1, 2), relative_fit.residuals_rad)
return RelativeMimicEvidence(fit, relative_fit.offset_rad,
tuple(row.sample_id for row in training), tuple(row.sample_id for row in holdout), errors)
def fit_standard_mimic(
source_joint: str,
target_joint: str,
active_rad: Sequence[float],
passive_rad: Sequence[float],
*,
cycles: Sequence[int],
offset_rad: float | None,
offset_observable: bool = False,
multiplier_bounds: tuple[float, float] | None = None,
) -> LinearMimicFit:
"""Fit actual synchronous observations, never a vendor-generated curve.
A static offset is fixed to the CAD/coordinate-conversion value unless an
independent datum makes it observable. Unknown tag mounting cannot justify
fitting an intercept. Numerical fitting is training-only; the serialized
URDF and independent fourth cycle decide acceptance afterwards.
"""
x, y = np.asarray(active_rad, dtype=float), np.asarray(passive_rad, dtype=float)
identifiers = np.asarray(cycles)
if x.ndim != 1 or x.shape != y.shape or x.shape != identifiers.shape or x.size < 40:
raise ValueError("mimic fitting requires at least 40 aligned observations")
if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)):
raise ValueError("mimic observations must be finite")
if not np.all(np.isin(identifiers, (0, 1, 2))) or set(identifiers.tolist()) != {0, 1, 2}:
raise ValueError("mimic fitting requires three training cycles; holdout is forbidden")
if not source_joint or not target_joint or source_joint == target_joint:
raise ValueError("mimic source and target must be distinct")
if float(np.ptp(x)) <= math.radians(1.0):
raise ValueError("mimic source travel is not observable")
if offset_rad is None:
if not offset_observable:
raise ValueError("unobservable passive static offset must retain CAD")
design, rhs = np.column_stack((x, np.ones_like(x))), y
else:
if not math.isfinite(float(offset_rad)):
raise ValueError("fixed mimic offset must be finite")
design, rhs = x[:, None], y - float(offset_rad)
initial, _, rank, _ = np.linalg.lstsq(design, rhs, rcond=None)
if rank != design.shape[1]:
raise ValueError("mimic parameters are not independently observable")
residual = rhs - design @ initial
scale = max(math.radians(0.1), 1.4826 * float(np.median(np.abs(residual - np.median(residual)))))
bounds = (-np.inf, np.inf)
if multiplier_bounds is not None:
if (offset_rad is None or len(multiplier_bounds) != 2
or not all(math.isfinite(v) for v in multiplier_bounds)
or multiplier_bounds[0] >= multiplier_bounds[1]):
raise ValueError("bounded mimic requires a fixed offset and a finite feasible interval")
bounds = multiplier_bounds
initial = np.clip(initial, *bounds)
solution = least_squares(lambda parameters: design @ parameters - rhs,
initial, loss="soft_l1", f_scale=scale, bounds=bounds)
if not solution.success or not np.all(np.isfinite(solution.x)):
raise ValueError("linear mimic fit failed")
multiplier = float(solution.x[0])
offset = float(solution.x[1]) if offset_rad is None else float(offset_rad)
return LinearMimicFit(source_joint, target_joint, multiplier, offset,
offset_rad is None, (0, 1, 2), tuple(float(v) for v in (multiplier*x + offset - y)))
def fit_range_bounded_mimic(source_joint, target_joint, active_rad, passive_rad, *,
cycles, parent_range, child_range):
"""Best robust affine approximation within the unchanged joint limits.
Optimize the two child angles at the parent's endpoints. Bounding those
two values guarantees that the entire affine motion stays in the child's
interval, including tiny negative baseline excursions. The intercept is
approximation error at baseline, NOT an observed physical zero correction.
"""
initial_fit = fit_standard_mimic(source_joint, target_joint, active_rad, passive_rad,
cycles=cycles, offset_rad=0.0)
for interval in (parent_range, child_range):
if (len(interval) != 2 or not all(math.isfinite(v) for v in interval)
or interval[0] >= interval[1]):
raise ValueError("invalid_mimic_motion_interval")
x, y = np.asarray(active_rad, dtype=float), np.asarray(passive_rad, dtype=float)
lower, upper = parent_range
fraction = (x - lower) / (upper - lower)
design = np.column_stack((1 - fraction, fraction))
residual = np.asarray(initial_fit.residuals_rad)
scale = max(math.radians(0.1), 1.4826 * float(np.median(np.abs(residual - np.median(residual)))))
initial = np.clip(np.asarray(parent_range) * initial_fit.multiplier, *child_range)
solution = least_squares(lambda endpoints: design @ endpoints - y, initial,
loss="soft_l1", f_scale=scale, bounds=child_range)
if not solution.success or not np.all(np.isfinite(solution.x)):
raise ValueError("bounded_affine_mimic_fit_failed")
multiplier = float((solution.x[1] - solution.x[0]) / (upper - lower))
offset = float(solution.x[0] - multiplier * lower)
return LinearMimicFit(source_joint, target_joint, multiplier, offset, False,
(0, 1, 2), tuple(float(v) for v in multiplier*x + offset - y))
@dataclass(frozen=True)
class CouplingFit:
source_joint: str
target_joint: str
model: str
coefficients: tuple[float, ...]
urdf_mimic_multiplier: float
urdf_mimic_policy: str
cycle_coefficients: tuple[tuple[float, ...], ...]
maximum_cycle_range: float
maximum_cycle_prediction_range_rad: float
residual_rms_rad: float
residual_p95_rad: float
residual_max_rad: float
@property
def multiplier(self) -> float:
return float(self.coefficients[0])
@property
def cycle_multipliers(self) -> tuple[float, ...]:
return tuple(float(values[0]) for values in self.cycle_coefficients)
@property
def mujoco_polycoef(self) -> tuple[float, ...]:
return (0.0, *self.coefficients, *(0.0,) * (5 - len(self.coefficients)))
def curve_travel_rad(fit: DirectionalCurve) -> float:
"""Return the mean travel of the decreasing and increasing curves."""
decreasing = np.asarray(fit.decreasing_rad, dtype=float)
increasing = np.asarray(fit.increasing_rad, dtype=float)
if decreasing.size < 2 or increasing.size < 2:
raise ValueError("directional curves require at least two knots")
travel = 0.5 * (
float(decreasing[0] - decreasing[-1])
+ float(increasing[0] - increasing[-1])
)
if not math.isfinite(travel) or travel <= 0.0:
raise ValueError("fitted travel must be finite and positive")
return travel
def _coupling_regression(
active: Sequence[float], passive: Sequence[float], *, degree: int
) -> tuple[tuple[float, ...], np.ndarray]:
x = np.asarray(active, dtype=float)
y = np.asarray(passive, dtype=float)
if x.shape != y.shape or x.ndim != 1 or x.size < 16:
raise ValueError("coupling curves must be aligned finite vectors")
if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)):
raise ValueError("coupling curves must be finite")
if degree not in {1, 2}:
raise ValueError("coupling degree must be one or two")
if float(x @ x) <= 1.0e-9:
raise ValueError("active coupling source has insufficient travel")
design = np.column_stack([x ** power for power in range(1, degree + 1)])
initial = np.linalg.lstsq(design, y, rcond=None)[0]
scale = max(
math.radians(0.25),
float(np.median(np.abs(y - design @ initial))),
)
fitted = least_squares(
lambda value: y - design @ value,
np.asarray(initial, dtype=float),
loss="soft_l1",
f_scale=scale,
)
if not fitted.success or not np.all(np.isfinite(fitted.x)):
raise ValueError("robust through-origin coupling regression failed")
coefficients = tuple(float(value) for value in fitted.x)
grid = np.linspace(0.0, float(np.max(x)), 256)
derivative = np.full_like(grid, coefficients[0])
if degree == 2:
derivative += 2.0 * coefficients[1] * grid
if float(np.min(derivative)) < -1.0e-7:
raise ValueError("coupling model is not monotonic")
return coefficients, y - design @ fitted.x
def fit_coupling_model(
source_joint: str,
target_joint: str,
active_fit: DirectionalCurve,
passive_fit: DirectionalCurve,
*,
model: str,
cycle_curve_pairs: Sequence[
tuple[Sequence[float], Sequence[float]]
] = (),
minimum_multiplier: float = 0.5,
maximum_multiplier: float = 1.5,
maximum_cycle_range: float = 0.03,
maximum_cycle_prediction_range_rad: float = math.radians(1.0),
maximum_residual_p95_rad: float = math.radians(2.0),
maximum_residual_rad: float = math.radians(3.0),
) -> CouplingFit:
"""Fit a profile-selected coupling primitive with release-grade metrics."""
supported = {"linear_mimic", "quadratic_runtime", "direction_aware_knots"}
if model not in supported:
raise ValueError(f"unsupported coupling model: {model}")
source_travel = curve_travel_rad(active_fit)
target_travel = curve_travel_rad(passive_fit)
endpoint_multiplier = float(target_travel / source_travel)
if model == "direction_aware_knots":
if not minimum_multiplier <= endpoint_multiplier <= maximum_multiplier:
raise ValueError(
f"{target_joint} coupling endpoint ratio is outside "
f"[{minimum_multiplier}, {maximum_multiplier}]"
)
return CouplingFit(
source_joint, target_joint, model, (endpoint_multiplier,),
endpoint_multiplier, "endpoint_linear_fallback", (),
0.0, 0.0, 0.0, 0.0, 0.0,
)
degree = 1 if model == "linear_mimic" else 2
active = np.concatenate((
np.asarray(active_fit.decreasing_rad, dtype=float),
np.asarray(active_fit.increasing_rad, dtype=float),
))
passive = np.concatenate((
np.asarray(passive_fit.decreasing_rad, dtype=float),
np.asarray(passive_fit.increasing_rad, dtype=float),
))
coefficients, residual = _coupling_regression(active, passive, degree=degree)
multiplier = coefficients[0]
if not minimum_multiplier <= multiplier <= maximum_multiplier:
raise ValueError(
f"{target_joint} coupling linear term is outside "
f"[{minimum_multiplier}, {maximum_multiplier}]"
)
cycle_coefficients = tuple(
_coupling_regression(source, target, degree=degree)[0]
for source, target in cycle_curve_pairs
)
cycle_multipliers = tuple(values[0] for values in cycle_coefficients)
cycle_range = (
0.0 if len(cycle_multipliers) < 2
else float(max(cycle_multipliers) - min(cycle_multipliers))
)
if cycle_range > maximum_cycle_range:
raise ValueError(
f"{target_joint} coupling linear-term cycle range exceeds "
f"{maximum_cycle_range}"
)
cycle_prediction_range = 0.0
if len(cycle_coefficients) >= 2:
grid = np.linspace(0.0, float(np.max(active)), 256)
predictions = np.asarray([
sum(value * grid ** (index + 1) for index, value in enumerate(values))
for values in cycle_coefficients
])
cycle_prediction_range = float(np.max(np.ptp(predictions, axis=0)))
if cycle_prediction_range > maximum_cycle_prediction_range_rad:
raise ValueError(
f"{target_joint} coupling cycle prediction range exceeds limit"
)
absolute = np.abs(residual)
rms = float(np.sqrt(np.mean(np.square(residual))))
p95 = float(np.percentile(absolute, 95.0))
maximum = float(np.max(absolute))
if p95 > maximum_residual_p95_rad or maximum > maximum_residual_rad:
raise ValueError(
"coupling_residual_exceeds:"
f"joint={target_joint}:model={model}:"
f"linear_term={multiplier:.6f}:"
f"p95_deg={math.degrees(p95):.3f}:"
f"maximum_deg={math.degrees(maximum):.3f}:"
f"p95_limit_deg={math.degrees(maximum_residual_p95_rad):.3f}:"
f"maximum_limit_deg={math.degrees(maximum_residual_rad):.3f}"
)
return CouplingFit(
source_joint=source_joint,
target_joint=target_joint,
model=model,
coefficients=coefficients,
urdf_mimic_multiplier=(
multiplier if model == "linear_mimic" else endpoint_multiplier
),
urdf_mimic_policy=(
"exact_linear" if model == "linear_mimic"
else "endpoint_linear_fallback"
),
cycle_coefficients=cycle_coefficients,
maximum_cycle_range=cycle_range,
maximum_cycle_prediction_range_rad=cycle_prediction_range,
residual_rms_rad=rms,
residual_p95_rad=p95,
residual_max_rad=maximum,
)
__all__ = ["CouplingFit", "DirectionalCurve", "curve_travel_rad", "fit_coupling_model",
"LinearMimicFit", "fit_standard_mimic", "PairedJointObservation",
"RelativeMimicEvidence", "fit_relative_standard_mimic"]
@@ -0,0 +1,70 @@
"""Model-independent curve fitting result and monotonic projection."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Sequence
import numpy as np
from ..geometry import delta_rotation_vector
def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
"""Unweighted PAVA projection onto non-increasing values."""
original = np.asarray(values, dtype=float)
if original.ndim != 1 or not np.all(np.isfinite(original)):
raise ValueError("values must be a finite vector")
negated = -original
levels: list[float] = []
weights: list[int] = []
starts: list[int] = []
for index, value in enumerate(negated):
levels.append(float(value))
weights.append(1)
starts.append(index)
while len(levels) >= 2 and levels[-2] > levels[-1]:
total_weight = weights[-2] + weights[-1]
merged = (
levels[-2] * weights[-2] + levels[-1] * weights[-1]
) / total_weight
levels[-2:] = [merged]
weights[-2:] = [total_weight]
starts.pop()
projected = np.empty_like(original)
for block_index, (level, start) in enumerate(zip(levels, starts)):
end = (
starts[block_index + 1]
if block_index + 1 < len(starts)
else len(original)
)
projected[start:end] = -level
return projected
@dataclass(frozen=True)
class FitResult:
joints: dict[str, dict[str, Any]]
axes: dict[str, tuple[float, float, float]]
references: dict[str, tuple[float, float, float, float]]
ip_coupling: dict[str, float]
max_monotonic_correction_rad: float
max_hysteresis_rad: float
measurement_mode: str = "rotation"
trajectory_models: dict[str, Any] = field(default_factory=dict)
trajectory_quality: dict[str, Any] = field(default_factory=dict)
def measure_from_reference(
self,
joint_name: str,
observed_quaternion_xyzw: Sequence[float],
reference_quaternion_xyzw: Sequence[float] | None = None,
) -> float:
reference = (
reference_quaternion_xyzw
if reference_quaternion_xyzw is not None
else self.references[joint_name]
)
vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
axis = np.asarray(self.axes[joint_name], dtype=float)
return float(vector @ axis)
@@ -0,0 +1,60 @@
"""Explicit JSON transport for immutable numerical training objects.
Only a fixed registry of calibration value types can be reconstructed. There
is no pickle, dynamic import or executable object in a persisted model.
"""
from dataclasses import fields, is_dataclass
import math
import numpy as np
from scipy.spatial.transform import Rotation
def encode(value):
if isinstance(value, Rotation):
return {"$type": "Rotation", "value": value.as_quat().tolist()}
if isinstance(value, np.ndarray):
return {"$type": "array", "value": value.tolist()}
if isinstance(value, np.generic):
return encode(value.item())
if is_dataclass(value):
return {"$type": type(value).__name__, "fields": {
field.name: encode(getattr(value, field.name)) for field in fields(value)}}
if isinstance(value, dict):
return {"$type": "mapping", "items": [[encode(k), encode(value[k])] for k in sorted(value, key=repr)]}
if isinstance(value, (tuple, list, frozenset, set)):
values = sorted(value) if isinstance(value, (set, frozenset)) else value
return {"$type": type(value).__name__, "items": [encode(item) for item in values]}
if isinstance(value, float) and not math.isfinite(value):
raise ValueError("frozen_training_nonfinite_value")
if value is None or isinstance(value, (str, int, float, bool)):
return value
raise TypeError(f"unsupported_frozen_training_type:{type(value).__name__}")
def decode(value):
if not isinstance(value, dict):
return value
kind = value.get("$type")
if kind == "Rotation":
return Rotation.from_quat(value["value"])
if kind == "array":
return np.asarray(value["value"], dtype=float)
if kind == "mapping":
return {decode(k): decode(v) for k, v in value["items"]}
sequences = {"tuple": tuple, "list": list, "set": set, "frozenset": frozenset}
if kind in sequences:
return sequences[kind](decode(item) for item in value["items"])
from . import command_motion, tag_installation
from .spatial_solver import stages, solve, types
from ..domain import measurement, result
registry = {name: cls for module in (command_motion, tag_installation, stages, solve,
types, measurement, result) for name, cls in vars(module).items()
if isinstance(cls, type) and is_dataclass(cls)}
if kind not in registry:
raise ValueError(f"unknown_frozen_training_type:{kind}")
cls = registry[kind]
if set(value["fields"]) != {field.name for field in fields(cls)}:
raise ValueError("frozen_training_fields_changed")
return cls(**{key: decode(item) for key, item in value["fields"].items()})
@@ -0,0 +1,74 @@
"""Training-frozen angular phase of an observed image-plane circle.
Only use this primitive where the Profile declares the projected circle
observable. Final standard-URDF SE(3) replay remains mandatory: a good image
circle alone cannot certify physical angles in an oblique projection.
"""
import math
import numpy as np
from ..domain.measurement import JointCurveFit
from .circle_geometry import _fit_circle, _trajectory_arc_rad
from .trajectory_geometry import _fit_joint_curve
def image_phase(point, circle):
point, centre, reference = (np.asarray(value, dtype=float) for value in
(point, circle["center_xy_px"], circle["reference_xy_px"]))
vector = point - centre
if point.shape != (2,) or not np.all(np.isfinite(point)) or np.linalg.norm(vector) < 1e-9:
raise ValueError("invalid image trajectory observation")
return float(circle["orientation_sign"]) * math.atan2(
float(reference[0]*vector[1] - reference[1]*vector[0]), float(reference @ vector))
def fit_joint_image_curve(records, *, zero_command_u8=255,
canonical_zero_direction="decreasing", maximum_radial_rms_px=2.,
maximum_radial_p95_px=3.5, minimum_radius_px=20., minimum_arc_rad=math.radians(15)):
samples = list(records)
if not samples or any(int(row["cycle"]) not in {0, 1, 2} for row in samples):
raise ValueError("image curve requires training observations only")
points = np.asarray([row["image_relative_xy_px"] for row in samples], dtype=float)
if points.shape != (len(samples), 2) or not np.all(np.isfinite(points)):
raise ValueError("image curve requires finite two-dimensional observations")
centre, radius = _fit_circle(points)
radial = np.linalg.norm(points-centre, axis=1)-radius
quality = {"radial_rms_px": float(np.sqrt(np.mean(radial**2))),
"radial_p95_px": float(np.percentile(np.abs(radial), 95)),
"radius_px": radius, "arc_rad": float(_trajectory_arc_rad(points, centre))}
if (quality["radial_rms_px"] > maximum_radial_rms_px
or quality["radial_p95_px"] > maximum_radial_p95_px
or radius < minimum_radius_px or quality["arc_rad"] < minimum_arc_rad):
raise ValueError("image_trajectory_not_observable")
# One observed reference for all training directions/cycles. Choose it
# near nominal baseline, then interpolate a single coordinate gauge.
indices = [index for index, row in enumerate(samples)
if canonical_zero_direction is None or row["direction"] == canonical_zero_direction]
if not indices:
raise ValueError("canonical image direction is missing")
index = min(indices, key=lambda i: abs(int(samples[i]["command_u8"])-zero_command_u8))
reference = points[index]-centre
reference /= np.linalg.norm(reference)
circle = {"space": "image_2d", "center_xy_px": centre.tolist(),
"reference_xy_px": reference.tolist(), "orientation_sign": 1., "radius_px": radius,
"training_cycles": sorted({int(row["cycle"]) for row in samples}),
"input_domain": "feedback_u8", "input_knots": list(range(256)),
"zero_command_u8": int(zero_command_u8)}
values = np.asarray([image_phase(point, circle) for point in points])
commands = np.asarray([int(row["command_u8"]) for row in samples])
trend = float((commands-np.mean(commands)) @ (values-np.mean(values)))
if abs(trend) < 1e-12:
raise ValueError("image motion direction is not observable")
if trend > 0:
values = -values
circle["orientation_sign"] = -1.
curves, correction, hysteresis = _fit_joint_curve(samples, values, preserve_direction_offset=True)
reference_angle = float(curves["angle_rad" if canonical_zero_direction is None else canonical_zero_direction+"_rad"][int(zero_command_u8)])
turn = reference_angle / circle["orientation_sign"]
rotation = np.array([[math.cos(turn), -math.sin(turn)], [math.sin(turn), math.cos(turn)]])
circle["reference_xy_px"] = (rotation @ reference).tolist()
curves = {name: tuple(float(value-reference_angle) for value in values) for name, values in curves.items()}
return JointCurveFit(curves["angle_rad"], curves["decreasing_rad"], curves["increasing_rad"],
circle, correction, hysteresis, quality)
@@ -0,0 +1,107 @@
"""Training-only URDF approximations, separate from exact JSON lookup tables."""
from dataclasses import dataclass, replace
import math
from .coupling import LinearMimicFit, fit_standard_mimic, fit_range_bounded_mimic
from .observed_motion import paired_visual_observations
@dataclass(frozen=True)
class MimicApproximation:
output_fit: LinearMimicFit
training_sample_ids: tuple[str, ...]
holdout_sample_ids: tuple[str, ...]
holdout_errors_rad: tuple[float, ...]
transferred_from_joint: str | None = None
multiplier_bounds: tuple[float, float] | None = None
parent_range_rad: tuple[float, float] | None = None
child_range_rad: tuple[float, float] | None = None
def in_cad_coordinates(self, offsets):
from dataclasses import replace
if self.parent_range_rad is not None:
raise ValueError("affine_approximation_intercept_is_not_a_physical_zero")
fit = self.output_fit
# The offset is supported by the TWO baseline geometry datums.
return replace(fit, offset_rad=offsets[fit.target_joint]-fit.multiplier*offsets[fit.source_joint],
offset_observed=True)
def shared_zero_multiplier_bounds(parent_range, child_range):
"""Intersect endpoint constraints for q_child = a*q_parent.
These ranges come from training support or mechanical bounds, never from
holdout observations. Neither measured curve nor joint range is modified.
"""
lower, upper = -math.inf, math.inf
if any(len(interval) != 2 or not all(math.isfinite(v) for v in interval)
or interval[0] >= interval[1] for interval in (parent_range, child_range)):
raise ValueError("invalid_mimic_motion_interval")
for endpoint in parent_range:
if endpoint == 0:
if not child_range[0] <= 0 <= child_range[1]:
raise ValueError("mimic_range_excludes_shared_zero")
continue
values = sorted(value / endpoint for value in child_range)
lower, upper = max(lower, values[0]), min(upper, values[1])
if not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper:
raise ValueError("no_feasible_shared_zero_mimic")
return lower, upper
def fit_mimic_approximations(model, curves, records, *, output_ranges=None, affine=False):
result = {}
for name, joint in model.joints.items():
parent = joint.mimic_joint
if parent is None or name not in curves or parent not in curves:
continue
rows = paired_visual_observations(records[parent], records[name], curves[parent], curves[name])
for cycle in range(4):
for direction in ("increasing", "decreasing"):
if sum(r.cycle == cycle and r.direction == direction for r in rows) < 40:
raise ValueError(f"paired_mimic_observations_missing:{name}:{cycle}:{direction}")
training = [r for r in rows if r.cycle != 3]
holdout = [r for r in rows if r.cycle == 3]
bounds = None
arguments = dict(source_joint=parent, target_joint=name,
active_rad=[r.source_rad for r in training], passive_rad=[r.target_rad for r in training],
cycles=[r.cycle for r in training])
if affine:
if output_ranges is None:
raise ValueError("affine_mimic_requires_training_motion_ranges")
fit = fit_range_bounded_mimic(**arguments,
parent_range=output_ranges[parent], child_range=output_ranges[name])
else:
bounds = None if output_ranges is None else shared_zero_multiplier_bounds(
output_ranges[parent], output_ranges[name])
fit = fit_standard_mimic(**arguments, offset_rad=0.0, multiplier_bounds=bounds)
if not math.isfinite(fit.multiplier) or fit.multiplier == 0:
raise ValueError(f"invalid_mimic_approximation:{name}")
result[name] = MimicApproximation(fit, tuple(r.sample_id for r in training),
tuple(r.sample_id for r in holdout),
tuple(fit.multiplier*r.source_rad + fit.offset_rad - r.target_rad for r in holdout),
multiplier_bounds=bounds,
parent_range_rad=tuple(output_ranges[parent]) if affine else None,
child_range_rad=tuple(output_ranges[name]) if affine else None)
return result
def fit_bounded_mimic_approximations(profile, model, fit, records):
"""Fit the URDF affine fallback after training establishes both ranges.
Only independently observed pairs are fitted; explicit donor expansion is
performed afterwards. JSON continues to use the independent nonlinear maps.
"""
ranges = {}
for name, mapping in fit.output_mappings.items():
joint = model.joints[name]
if name in profile.urdf_limit_policies or joint.mimic_joint is None:
command = fit.command_mappings[name]
ranges[name] = (min(0, mapping.bounds_rad[0], command.bounds_rad[0]),
max(0, mapping.bounds_rad[1], command.bounds_rad[1]))
else:
delta = fit.zero_offsets_rad[name]
ranges[name] = (joint.lower - delta, joint.upper - delta)
return replace(fit, mimic_approximations=fit_mimic_approximations(
model, fit.curves, records, output_ranges=ranges, affine=True))
@@ -0,0 +1,139 @@
"""Profile-selected native measurement primitives and coordinate gauges.
The curve reference is only a coordinate choice. CAD zero is solved later
from independently observed spatial axes; neither SDK zero nor an endpoint
is treated as a mechanical datum.
"""
from dataclasses import dataclass, replace
from typing import Mapping
import numpy as np
from ..domain.measurement import JointCurveFit
from ..urdf.acceptance import angular_metrics
from .coupling import fit_relative_standard_mimic
from .observed_motion import (
compare_native_views, fit_byte_observed_motion, fit_native_observed_rotation,
paired_visual_observations,
)
@dataclass(frozen=True)
class MotionFit:
observed_curves: Mapping[str, JointCurveFit]
coordinate_curves: Mapping[str, JointCurveFit]
holdout_errors_rad: Mapping[str, tuple[float, ...]]
mimic_evidence: Mapping
cross_view_metrics: Mapping
reference_inputs: Mapping[str, float]
def channel_for_joint(profile, joint):
seen = set()
while joint not in profile.command.command_index_by_joint:
if joint in seen or joint not in profile.zero.mimic_source_by_joint:
raise ValueError(f"measured joint has no SDK source:{joint}")
seen.add(joint)
joint = profile.zero.mimic_source_by_joint[joint]
return profile.command.command_index_by_joint[joint]
def joint_input_direction(profile, source_model, joint):
"""A passive measurement follows its declared CAD mimic chain's sign."""
sign, current, seen = 1, joint, set()
while current not in profile.command.command_index_by_joint:
if current in seen:
raise ValueError("cyclic mimic input binding")
seen.add(current)
cad = source_model.joints[current]
if cad.mimic_joint != profile.zero.mimic_source_by_joint.get(current) or cad.mimic_multiplier == 0:
raise ValueError(f"invalid measured mimic input binding:{current}")
sign *= 1 if cad.mimic_multiplier > 0 else -1
current = cad.mimic_joint
return sign * profile.command.joint_directions[profile.command.command_index_by_joint[current]]
def native_coordinate_curve(curve, baseline):
"""Choose one training-supported gauge, never extend a fitted domain.
Both directional branches subtract the SAME reference, retaining actual
backlash. If nominal baseline lies outside observed support, the nearest
measured input is the reference and is explicitly recorded as such.
"""
inputs = curve.circle["input_knots"]
reference_input = float(np.clip(baseline, inputs[0], inputs[-1]))
reference_angle = float(np.interp(reference_input, inputs, curve.angle_rad))
branches = {name: tuple(float(value - reference_angle) for value in getattr(curve, name))
for name in ("angle_rad", "increasing_rad", "decreasing_rad")}
return replace(curve, **branches, circle={**curve.circle,
"coordinate_reference_input": reference_input,
"coordinate_reference_angle_rad": reference_angle}), reference_input
def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_records=None, zero_references=None):
expected = {name for name, spec in profile.measurement.measurements.items()
if spec.view is not None and spec.kind != "axis_cross_view_validation"}
if set(records_by_joint) != expected:
raise ValueError("fitting observations differ from the Profile")
if any(profile.measurement.measurements[name].kind not in {"relative_rotation", "urdf_axis_chain", "curve"} for name in expected):
raise ValueError("measurement primitive is not implemented in this pipeline")
mimic_sources = {name: profile.zero.mimic_source_by_joint[name] for name in profile.zero.fitted_mimic_joints}
if profile.artifacts.output_schema_version >= 3:
if set(zero_references or {}) != expected:
raise ValueError("joint_zero_references_incomplete")
observed, holdout, evidence, references = {}, {}, {}, {}
for name, rows in sorted(records_by_joint.items()):
view = profile.measurement.measurements[name].view
reference = zero_references[name]
observed[name], holdout[name] = fit_native_observed_rotation(rows,
input_domain=profile.curve_input_domain,
input_to_joint_direction=joint_input_direction(profile, source_model, name),
reference_xyzw=reference.quaternion(view), joint=name, view=view)
references[name] = reference.input_reference(view, feedback=True)
coordinates = observed
elif profile.command.unit == "u8":
observed, holdout, evidence = fit_byte_observed_motion(records_by_joint,
source_model=source_model, mimic_sources=mimic_sources,
baseline_by_joint={name: profile.command.baseline_values[channel_for_joint(profile, name)] for name in expected},
direction_by_joint={name: joint_input_direction(profile, source_model, name) for name in expected},
image_curve_joints=profile.measurement.image_curve_joints, input_domain=profile.curve_input_domain)
coordinates = observed
references = {name: float(profile.command.baseline_values[channel_for_joint(profile, name)])
for name in observed}
else:
observed, coordinates, holdout, evidence, references = {}, {}, {}, {}, {}
for name, rows in sorted(records_by_joint.items()):
curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain,
input_to_joint_direction=joint_input_direction(profile, source_model, name),
joint=name, view=profile.measurement.measurements[name].view)
observed[name], holdout[name] = curve, errors
coordinates[name], references[name] = native_coordinate_curve(curve,
profile.command.baseline_values[channel_for_joint(profile, name)])
for target, source in mimic_sources.items():
cad = source_model.joints[target]
if cad.mimic_joint != source:
raise ValueError(f"mimic source differs from CAD:{target}")
evidence[target] = fit_relative_standard_mimic(source, target,
paired_visual_observations(records_by_joint[source], records_by_joint[target],
observed[source], observed[target]), cad_offset_rad=cad.mimic_offset)
secondary = cross_view_records or {}
if set(secondary) != set(profile.measurement.cross_view_sources):
raise ValueError("required cross-view observations are incomplete")
metrics = {}
for name, rows in secondary.items():
curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain,
input_to_joint_direction=joint_input_direction(profile, source_model, name),
reference_xyzw=(zero_references[name].quaternion(rows[0]["view"])
if profile.artifacts.output_schema_version >= 3 else None),
joint=name, view=profile.measurement.measurements[
profile.measurement.cross_view_sources[name]].view)
if "input_knots" not in observed[name].circle:
raise ValueError("cross-view comparison requires native input knots")
comparison = compare_native_views(observed[name], curve)
if comparison.get("direction_disagrees", False):
raise ValueError(f"cross-view motion direction disagrees:{name}")
quality = angular_metrics(errors)
metrics[name] = {**comparison, "holdout_mae_deg": quality.mae_deg,
"holdout_p95_deg": quality.p95_deg, "holdout_max_deg": quality.maximum_deg}
return MotionFit(observed, coordinates, holdout, evidence, metrics, references)
@@ -0,0 +1,111 @@
"""Apply explicit donor policies without creating recipient observations.
Motion curves, absolute zeros and URDF approximations have separate provenance.
Only the numerical mappings are expanded; training and holdout evidence remain
attached to the joints that were actually observed.
"""
from dataclasses import replace
from ..domain.profile import CalibrationProfile, validate_profile
from ..domain.result import CalibrationResult, JointMapping
from .motion_fit import channel_for_joint
def transferred_mapping(profile: CalibrationProfile, mapping: JointMapping,
target: str) -> JointMapping:
return replace(mapping, joint=target, motor_index=channel_for_joint(profile, target),
transferred_from=mapping.joint)
def expand_profile_transfers(profile: CalibrationProfile, fit: CalibrationResult) -> CalibrationResult:
"""Copy frozen fits once, after all measured command holdouts have passed."""
transfers = profile.measurement.transferred_motion_sources
if not transfers:
return fit
validate_profile(profile)
outputs, commands = dict(fit.output_mappings), dict(fit.command_mappings)
offsets, methods = dict(fit.zero_offsets_rad), dict(fit.zero_method_by_joint)
approximations = dict(fit.mimic_approximations)
for target, source in transfers.items():
if target in outputs or target in commands or target in fit.curves or target in fit.joint_zero_references:
raise ValueError(f"motion_transfer_cannot_replace_existing_result:{target}")
if source not in fit.curves or source not in outputs or source not in commands:
raise ValueError(f"motion_transfer_requires_fitted_donor:{target}:{source}")
for mappings in (outputs, commands):
donor = mappings[source]
if donor.joint != source or donor.transferred_from is not None \
or donor.motor_index != channel_for_joint(profile, source):
raise ValueError(f"motion_transfer_requires_direct_donor_binding:{source}")
mappings[target] = transferred_mapping(profile, donor, target)
# Dynamic similarity does not authorize copying an absolute CAD zero.
if profile.zero.transferred_zero_sources.get(target) == source and source in offsets:
offsets[target] = offsets[source]
methods[target] = "transferred_static_zero_on_own_cad"
# Historical fitted approximations remain transferable for replay.
# Current measured fitting emits no mimic fit; each recipient keeps its CAD coupling.
if target in profile.zero.passive_joints and approximations:
if source not in fit.mimic_approximations:
raise ValueError(f"motion_transfer_requires_paired_donor_mimic:{target}:{source}")
donor = fit.mimic_approximations[source]
approximations[target] = replace(donor,
output_fit=replace(donor.output_fit, target_joint=target,
source_joint=profile.zero.mimic_source_by_joint[target]),
transferred_from_joint=source)
return replace(fit, output_mappings=outputs, command_mappings=commands,
zero_offsets_rad=offsets, zero_method_by_joint=methods, mimic_approximations=approximations)
def validate_mapping_provenance(profile: CalibrationProfile, fit: CalibrationResult) -> None:
"""Require every published table to be measured or an exact authorized copy."""
validate_profile(profile)
transfers = profile.measurement.transferred_motion_sources
observed = set(fit.curves)
if observed & transfers.keys():
raise ValueError("independent_release_contains_transferred_or_unknown_joints")
expected = observed | transfers.keys()
for mappings in (fit.output_mappings, fit.command_mappings):
if set(mappings) != expected:
raise ValueError("independent_release_contains_transferred_or_unknown_joints")
for name, mapping in mappings.items():
if mapping.joint != name or mapping.motor_index != channel_for_joint(profile, name):
raise ValueError(f"mapping_sdk_binding_differs:{name}")
source = transfers.get(name)
if source is None:
if mapping.transferred_from is not None:
raise ValueError("independent_release_contains_transferred_or_unknown_joints")
elif source not in observed or mappings[source].transferred_from is not None \
or mapping != transferred_mapping(profile, mappings[source], name):
raise ValueError(f"mapping_differs_from_declared_donor:{name}")
for name, approximation in fit.mimic_approximations.items():
source = transfers.get(name)
if source is None:
if approximation.transferred_from_joint is not None:
raise ValueError(f"undeclared_mimic_transfer:{name}")
elif source not in fit.mimic_approximations:
raise ValueError(f"missing_donor_mimic_approximation:{name}")
else:
donor = fit.mimic_approximations[source]
expected_approximation = replace(donor, output_fit=replace(donor.output_fit,
target_joint=name, source_joint=profile.zero.mimic_source_by_joint[name]),
transferred_from_joint=source)
if approximation != expected_approximation:
raise ValueError(f"mimic_differs_from_declared_donor:{name}")
def validate_zero_provenance(profile: CalibrationProfile, fit: CalibrationResult) -> None:
"""Keep accepted premises and explicit zero copies true in the final plan."""
offsets, methods = fit.zero_offsets_rad, fit.zero_method_by_joint
for name in profile.zero.cad_zero_assumptions:
if offsets.get(name) != 0.0 or methods.get(name) != "assumed_source_cad_zero":
raise ValueError(f"assumed_cad_zero_differs_from_profile:{name}")
for name, geometry in profile.zero.known_baseline_geometry.items():
if offsets.get(name) != geometry.cad_angle_rad \
or methods.get(name) != "independent_known_baseline_geometry":
raise ValueError(f"known_baseline_geometry_differs_from_profile:{name}")
for target, source in profile.zero.transferred_zero_sources.items():
if profile.measurement.transferred_motion_sources.get(target) != source:
continue
if source not in offsets or offsets.get(target) != offsets[source] \
or methods.get(target) != "transferred_static_zero_on_own_cad":
raise ValueError(f"transferred_zero_differs_from_declared_donor:{target}")
@@ -0,0 +1,156 @@
"""Shared visual motion fitting and independently paired standard mimics."""
from __future__ import annotations
from typing import Any, Mapping
from .coupling import PairedJointObservation, fit_relative_standard_mimic
from .spatial import fit_rotation_joint_curve, measure_joint_curve_observation, joint_curve_holdout_errors
from .image_curve import fit_joint_image_curve
from ..urdf.acceptance import angular_metrics
from ..domain.measurement import JointCurveFit
from .rotation_curve import RotationObservation, fit_rotation_curve
from .rotation_diagnostics import RotationHoldoutFailure, rotation_holdout_diagnostic
def image_identity(row: Mapping[str, Any]) -> str:
identity = str(row.get("sample_id", ""))
if not identity and row.get("view") and row.get("image_stamp_ns") is not None:
identity = f"{row['view']}:{int(row['image_stamp_ns'])}"
if not identity:
raise ValueError("visual fitting requires recorded image identity")
return identity
def paired_visual_observations(source_rows, target_rows, source_fit, target_fit):
def indexed(rows):
output = {}
for row in rows:
identity = image_identity(row)
if identity in output:
raise ValueError("duplicated paired visual image identity")
output[identity] = row
return output
source, target = indexed(source_rows), indexed(target_rows)
output = []
for identity in sorted(source.keys() & target.keys()):
a, b = source[identity], target[identity]
if (a["cycle"], a["direction"]) != (b["cycle"], b["direction"]):
raise ValueError("paired visual image has different cycle/direction")
output.append(PairedJointObservation(identity, int(a["cycle"]), str(a["direction"]),
measure_joint_curve_observation(source_fit, quaternion_xyzw=a.get("relative_quaternion_xyzw"),
image_relative_xy_px=a.get("image_relative_xy_px")),
measure_joint_curve_observation(target_fit, quaternion_xyzw=b.get("relative_quaternion_xyzw"),
image_relative_xy_px=b.get("image_relative_xy_px"))))
return output
def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction,
reference_xyzw=None, joint=None, view=None):
"""Native-coordinate 3+1 hinge primitive with the shared geometric result.
The metadata carries real knots, not a synthetic byte command domain.
Scalar angle zero is only a fixed Tag reference, never a CAD datum.
"""
import numpy as np
observations = [RotationObservation(image_identity(row), float(row[input_domain]),
tuple(row["relative_quaternion_xyzw"]), int(row["cycle"]), str(row["direction"]))
for row in rows]
for direction in ("increasing", "decreasing"):
if sum(row.cycle == 3 and row.direction == direction for row in observations) < 40:
raise ValueError("holdout requires 40 images in each direction")
fitted = fit_rotation_curve([row for row in observations if row.cycle != 3],
input_domain=input_domain, sdk_to_joint_direction=input_to_joint_direction,
reference_xyzw=reference_xyzw)
holdout = [row for row in observations if row.cycle == 3]
errors = fitted.holdout_errors(holdout)
metrics = angular_metrics(errors)
if not metrics.passed:
raise RotationHoldoutFailure(rotation_holdout_diagnostic(
fitted, holdout, errors, metrics, joint=joint, view=view))
hysteresis = float(np.max(np.abs(np.asarray(fitted.increasing_rad) - fitted.decreasing_rad)))
curve = JointCurveFit(fitted.angle_rad, fitted.decreasing_rad, fitted.increasing_rad,
{"space": "relative_rotation_3d", "input_domain": input_domain, "input_knots": fitted.knots,
"reference_quaternion_xyzw": fitted.reference_xyzw, "axis_xyz": fitted.axis_xyz,
"training_cycles": [0, 1, 2], "training_sample_ids": sorted(fitted.training_sample_ids)},
fitted.maximum_monotonic_correction_rad, hysteresis, {"sample_count": float(len(observations)),
"holdout_mae_deg": metrics.mae_deg, "holdout_p95_deg": metrics.p95_deg,
"holdout_max_deg": metrics.maximum_deg})
return curve, errors
def compare_native_views(primary, secondary):
"""Diagnostic comparison in their common physical input interval.
Neither curve is replaced or recalibrated from the other view. Absolute
mount phase differs between Tags, so centre each frozen branch for this
diagnostic alone; final file FK validates the actual fixed mounts.
"""
import numpy as np
a, b = primary.circle["input_knots"], secondary.circle["input_knots"]
lower, upper = max(a[0], b[0]), min(a[-1], b[-1])
if upper <= lower:
raise ValueError("cross-view native input supports do not overlap")
inputs = np.linspace(lower, upper, 65)
left = np.interp(inputs, a, primary.angle_rad)
right = np.interp(inputs, b, secondary.angle_rad)
if (left[-1] - left[0]) * (right[-1] - right[0]) <= 0:
return {"direction_disagrees": 1.0}
result = {"travel_difference_rad": abs(float(np.ptp(left) - np.ptp(right)))}
for field in ("angle_rad", "increasing_rad", "decreasing_rad"):
x = np.interp(inputs, a, getattr(primary, field))
y = np.interp(inputs, b, getattr(secondary, field))
delta = (x - np.mean(x)) - (y - np.mean(y))
result[f"{field}_rms_difference_rad"] = float(np.sqrt(np.mean(delta ** 2)))
return result
def fit_byte_observed_motion(records_by_joint, *, source_model, mimic_sources,
baseline_by_joint=None, image_curve_joints=(), input_domain="feedback_u8", direction_by_joint=None):
"""3+1 frozen-reference byte-domain hinge fit; no per-cycle re-zeroing."""
curves, errors = {}, {}
for joint, rows in sorted(records_by_joint.items()):
# The retained byte primitive calls its abscissa command_u8. Adapt a
# PRIVATE numerical input, never overwrite the captured command.
rows = [dict(row, command_u8=int(round(float(row[input_domain])))) for row in rows]
ids = [image_identity(row) for row in rows]
if len(set(ids)) != len(ids):
raise ValueError(f"duplicate visual fitting image:{joint}")
for cycle in range(4):
for direction in ("increasing", "decreasing"):
if sum(int(row["cycle"]) == cycle and row["direction"] == direction for row in rows) < 40:
raise ValueError(f"missing bidirectional visual samples:{joint}:{cycle}:{direction}")
training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}]
holdout = [row for row in rows if int(row["cycle"]) == 3]
baseline = int((baseline_by_joint or {}).get(joint, 255))
primitive = fit_joint_image_curve if joint in image_curve_joints else fit_rotation_joint_curve
curve = primitive(training, zero_command_u8=baseline, canonical_zero_direction=None)
from dataclasses import replace
sign = int((direction_by_joint or {}).get(joint, -1))
if sign not in {-1, 1}:
raise ValueError("byte SDK direction must be explicitly signed")
if sign == 1:
circle = dict(curve.circle)
if circle["space"] == "image_2d":
circle["orientation_sign"] = -float(circle["orientation_sign"])
else:
circle["axis_xyz"] = [-float(v) for v in circle["axis_xyz"]]
# The legacy primitive chooses an arbitrary axis orientation with
# a negative input slope. Reorient BOTH the observation axis and
# every branch to the declared SDK direction, never just the table.
curve = replace(curve, circle=circle, **{field: tuple(-v for v in getattr(curve, field))
for field in ("angle_rad", "increasing_rad", "decreasing_rad")})
curve = replace(curve, circle={**curve.circle, "input_domain": input_domain, "input_knots": tuple(range(256))})
residuals = joint_curve_holdout_errors(curve, holdout, zero_command_u8=baseline)
if not angular_metrics(residuals).passed:
raise ValueError(f"independent_rotation_holdout_failed:{joint}")
curves[joint], errors[joint] = curve, residuals
evidence = {}
for target, source in mimic_sources.items():
cad = source_model.joints[target]
if cad.mimic_joint != source:
raise ValueError(f"mimic source differs from CAD:{target}")
evidence[target] = fit_relative_standard_mimic(source, target,
paired_visual_observations(records_by_joint[source], records_by_joint[target], curves[source], curves[target]),
cad_offset_rad=cad.mimic_offset)
return curves, errors, evidence
@@ -0,0 +1,322 @@
"""Approximate command curves using the existing CAD image objective.
Drafts estimate supported internal zeros. Unobserved root frames and declared
CAD leaf zeros remain explicit premises, never arbitrary measured offsets.
"""
from collections import defaultdict
from dataclasses import replace
from itertools import product
import numpy as np
from scipy.spatial.transform import Rotation
from ..domain.capture_plan import task_for_joint
from ..geometry.tag_pose.motion_evidence import _axis
from ..geometry.tag_pose.urdf_command_images import UrdfCommandImages, pose_vector
from .motion_fit import joint_input_direction
UPRIGHT_SPLAY_JOINTS = frozenset(finger+'_mcp_roll' for finger in ('index', 'middle', 'ring', 'pinky'))
UPRIGHT_SPLAY_REFERENCE = 'operator_defined_upright_splay_baseline'
def define_upright_splay_zero(profile, model):
"""Use the operator's upright baseline as a definition, not a measurement."""
if (profile.key.model != 'O30' or profile.key.side != 'right'
or not UPRIGHT_SPLAY_JOINTS <= profile.zero.active_joints):
raise ValueError('draft_upright_splay_requires_o30_right_four_fingers')
for name in UPRIGHT_SPLAY_JOINTS:
finger = name.split('_')[0]
positions = [model.link_transform(profile.command.urdf_joint_by_joint.get(joint, joint),
zero_offsets={}, joint_angles={}, independent_mimic_angles=True)[:3, 3]
for joint in (finger+'_mcp_pitch', finger+'_pip')]
direction = positions[1]-positions[0]
if not np.allclose(direction/np.linalg.norm(direction), [0., 0., 1.], atol=1e-6):
raise ValueError('draft_source_zero_is_not_upright:' + name)
assumptions = dict(profile.zero.cad_zero_assumptions)
assumptions.update({name: UPRIGHT_SPLAY_REFERENCE for name in UPRIGHT_SPLAY_JOINTS})
return replace(profile, zero=replace(profile.zero, cad_zero_assumptions=assumptions))
def draft_training_coverage(completed_units, units):
"""Audit accepted directions without borrowing from independent validation."""
completed = set(completed_units)
coverage = {}
for task in sorted({unit.task_key for unit in units}):
cycles = {}
for cycle in (0, 1):
expected = {unit.identity for unit in units if unit.task_key == task and unit.cycle == cycle}
accepted = expected & completed
cycles[cycle] = dict(expected_units=len(expected), accepted_units=len(accepted),
complete=bool(expected) and accepted == expected)
complete = [cycle for cycle, row in cycles.items() if row['complete']]
coverage[task] = dict(cycles=cycles, complete_cycles=complete,
initialization_cycle=complete[0] if complete else None)
return coverage
def draft_targets(profile, selected=None):
"""Report the requested whole-hand result independently of partial output."""
target_ranges = set(profile.zero.active_joints)
retained_leaves = set(profile.zero.cad_zero_assumptions)
target_zeros = target_ranges-retained_leaves
measured_ranges = set() if selected is None else set(selected.zero.active_joints)
measured_zeros = set() if selected is None else measured_ranges-set(selected.zero.cad_zero_assumptions)
defined_zeros = set() if selected is None else {name for name, reason in
selected.zero.cad_zero_assumptions.items() if reason == UPRIGHT_SPLAY_REFERENCE}
return dict(zero_target_joints=sorted(target_zeros), range_target_joints=sorted(target_ranges),
retained_leaf_zero_joints=sorted(retained_leaves), estimated_zero_joints=sorted(measured_zeros),
defined_zero_joints=sorted(defined_zeros),
estimated_range_joints=sorted(measured_ranges),
missing_zero_joints=sorted(target_zeros-measured_zeros-defined_zeros),
missing_range_joints=sorted(target_ranges-measured_ranges),
full_target_covered=(target_zeros <= measured_zeros | defined_zeros and target_ranges <= measured_ranges))
def draft_profile(profile, model, completed_units, units, training):
"""One complete training cycle supports a draft, with observed ancestors."""
coverage = draft_training_coverage(completed_units, units)
tasks = {task for task, row in coverage.items() if row['complete_cycles']}
names = {name for task in profile.motion.tasks if task.key in tasks for name in task.joints}
cad_names = {name: profile.command.urdf_joint_by_joint.get(name, name) for name in names}
observed = set(cad_names.values())
names = frozenset(name for name in names if all(j.kind == 'fixed' or j.name in observed
for j in model._chain(cad_names[name])))
if not names:
raise ValueError('draft_no_complete_ancestor_supported_training')
from ...profiles.observations import compile_tag_links
links = compile_tag_links(profile, model)
observed = {cad_names[name] for name in names}
visible = {role for row in training for role in row.get('tags', {})}
roles = {role for role, link in links.items() if role in visible and (link == model.root_link or all(
j.kind == 'fixed' or j.name in observed
for j in model._chain(model.parent_joint_by_child[link])))}
# Fixed tags belong to the root, which has no parent joint.
views = tuple(replace(view, tags=tuple(tag for tag in view.tags
if tag.fixed_reference or tag.role in roles)) for view in profile.vision.views)
assumptions = {name: reason for name, reason in profile.zero.cad_zero_assumptions.items() if name in names}
for name, observer in profile.zero.spatial.get('offset_observer_joint', {}).items():
if name in names and observer not in names:
assumptions[name] = 'partial_missing_zero_observer:' + observer
roots = [name for name in names if not any(j.kind != 'fixed'
for j in model._chain(cad_names[name])[:-1])]
# With only one root chain, rotating palm and root zero together leaves
# every image identical. Fix that gauge, while fitting the internal zeros.
if len(roots) == 1:
assumptions[roots[0]] = 'partial_single_root_zero_unobservable_without_palm_reference'
selected = replace(profile,
zero=replace(profile.zero, active_joints=names,
cad_zero_assumptions=assumptions),
motion=replace(profile.motion, tasks=tuple(task for task in profile.motion.tasks
if task.key in tasks and set(task.joints) <= names)),
vision=replace(profile.vision, views=views))
required = {profile.measurement.measurements[name].child_role for name in names}
if not required <= roles:
raise ValueError('draft_required_training_tags_missing:' + str(sorted(required-roles)))
return selected
class DraftCommandImages(UrdfCommandImages):
"""Shared image objective, with exact repeated-pose least-squares reduction."""
# Numerical stopping for an explicitly uncertified draft; release angular,
# image, observability and final-file acceptance limits remain independent.
solver_ftol = 1e-5
def __init__(self, profile, model, extrinsics, rows, tag_links, *, initialization_cycles=None,
training_initialization=None):
super().__init__(profile, model, extrinsics, rows, tag_links)
self.training_initialization = training_initialization
self.initialization_cycles = (dict(initialization_cycles) if initialization_cycles is not None
else {task.key: 0 for task in profile.motion.tasks})
if any(self.initialization_cycles.get(task.key) not in (0, 1) for task in profile.motion.tasks):
raise ValueError('draft_initialization_training_cycle_required')
self.original_observations = self.observations
self.observations, weights, within_group_error = {}, [], 0.
local_names = {cad: name for name, cad in self.cad_names.items()}
for role, (indices, pixels, matrices, objects) in self.original_observations.items():
channels = [self.channels[local_names[joint.name]] for joint in self.chains[role]
if joint.kind != 'fixed']
groups = defaultdict(list)
for position, index in enumerate(indices):
key = (tuple(self.commands[index, channels]), tuple(self.directions[index, channels]),
matrices[position].tobytes())
groups[key].append(position)
representatives, means = [], []
for positions in groups.values():
mean = pixels[positions].mean(axis=0)
within_group_error += float(np.sum((pixels[positions]-mean)**2))
representatives.append(positions[0])
means.append(mean)
weights.extend([np.sqrt(len(positions))]*8)
self.observations[role] = (indices[representatives], np.asarray(means),
matrices[representatives], objects)
self.residual_weights = np.asarray(weights)
self.within_group_error = within_group_error
self.original_residual_count = sum(len(data[0])*8 for data in self.original_observations.values())
def residual(self, parameters):
# sum ||prediction - pixel_i||² = n ||prediction - mean||² + scatter.
# Keep scatter in the objective, including the solver stopping test.
return np.r_[super().residual(parameters)*self.residual_weights,
np.sqrt(self.within_group_error), np.zeros(7)]
def jacobian(self, parameters):
weighted = super().jacobian(parameters)*self.residual_weights[:, None]
return np.vstack((weighted, np.zeros((8, len(parameters)))))
def original_residual(self, parameters):
predicted = self.project_observations(parameters, self.commands, self.directions, self.original_observations)
return np.concatenate([(predicted[role]-data[1]).ravel()
for role, data in self.original_observations.items()])
def audit_root_directions(self):
"""Reject opposite SDK signs for CAD-parallel roots, before fitting zeros.
Only complete training cycles enter this check. Both planar candidates
and both scan directions are retained; an ambiguous axis is not a
direction correction. Nothing here changes commands or chooses a pose.
"""
roots = [name for name in self.profile.zero.spatial.get('root_anchor_joints', ())
if name in self.joints]
axes = {}
for name in roots:
spec = self.profile.measurement.measurements[name]
task = task_for_joint(self.profile, name)
channel = self.channels[name]
observed = []
for direction in range(2):
indices = self.motion_indices(name, direction, self.initialization_cycles[task.key])
if len(indices) < 3:
continue
for branch in (0, 1):
_, poses = self.motion_poses(indices, spec.child_role, branch)
axis, phase = _axis(Rotation.from_matrix(np.asarray(poses)[:, :3, :3]))
if abs(phase[-1]-phase[0]) < np.radians(15):
continue
observed.append(axis*np.sign(phase[-1]-phase[0])
*joint_input_direction(self.profile, self.model, name))
axes[name] = observed
comparisons = []
for i, first in enumerate(roots):
a, _ = self.model.axis_line(self.cad_names[first], zero_offsets={}, joint_angles={})
for second in roots[i+1:]:
b, _ = self.model.axis_line(self.cad_names[second], zero_offsets={}, joint_angles={})
if abs(a @ b) < .999 or not axes[first] or not axes[second]:
continue
dots = np.asarray(axes[first]) @ np.asarray(axes[second]).T * np.sign(a @ b)
comparisons.append(dict(joints=[first, second], minimum_cosine=float(dots.min()),
maximum_cosine=float(dots.max()), reversed_for_all_candidates=bool(dots.max() < -.5)))
return dict(training_only=True, validation_used=False, comparisons=comparisons,
passed=not any(row['reversed_for_all_candidates'] for row in comparisons))
def initialize(self, branches):
roles = sorted(self.tags)
if len(branches) != min(6, len(roles)) or any(b not in (0, 1) for b in branches):
raise ValueError('draft_initialization_seed_invalid')
branch_by_role = {role: branches[min(i, len(branches)-1)] for i, role in enumerate(roles)}
initial = np.zeros(len(self.scale))
lines = {}
for name in self.joints:
task = task_for_joint(self.profile, name)
spec = self.profile.measurement.measurements[name]
channel = self.channels[name]
sign = joint_input_direction(self.profile, self.model, name)
for direction in range(2):
indices = self.motion_indices(name, direction, self.initialization_cycles[task.key])
if len(indices) < 3:
raise ValueError('draft_directional_training_missing:' + name)
indices, poses = self.motion_poses(indices, spec.child_role, branch_by_role[spec.child_role])
axis, phase = _axis(Rotation.from_matrix(poses[:, :3, :3]))
if (phase[-1]-phase[0])*sign < 0:
axis, phase = -axis, -phase
commands = self.commands[indices, channel]
phase -= np.interp(self.profile.command.baseline_values[channel], commands, phase)
self.set_initial_curve(initial, name, direction, np.interp(self.knots[name], commands, phase))
if direction == 0:
turns = poses[:, :3, :3] @ poses[0, :3, :3].T
blocks = np.eye(3)-turns
target = poses[:, :3, 3]-np.einsum('nij,j->ni', turns, poses[0, :3, 3])
lines[name] = (axis, np.linalg.lstsq(blocks.reshape(-1, 3), target.ravel(), rcond=1e-6)[0], indices[0])
angles = self.joint_angles(initial, self.commands, self.directions)
cad_axes, axes, cad_points, points = [], [], [], []
for name, (axis, point, index) in lines.items():
cad_axis, cad_point = self.model.axis_line(self.cad_names[name], zero_offsets={},
joint_angles={joint: values[index] for joint, values in angles.items()}, independent_mimic_angles=True)
cad_axes.append(cad_axis); axes.append(axis)
cad_points.append(cad_point); points.append(point)
if np.linalg.matrix_rank(np.asarray(cad_axes), tol=.01) < 2:
raise ValueError('draft_initialization_requires_nonparallel_observed_axes')
rotation, _ = Rotation.align_vectors(np.asarray(axes), np.asarray(cad_axes))
blocks = np.asarray([np.eye(3)-np.outer(axis, axis) for axis in axes])
targets = np.einsum('nij,nj->ni', blocks, np.asarray(points)-rotation.apply(cad_points))
initial[:3] = rotation.as_rotvec()
initial[3:6] = np.linalg.lstsq(blocks.reshape(-1, 3), targets.ravel(), rcond=1e-6)[0]
if self.training_initialization is not None:
hint = self.training_initialization
initial[:6] = hint['palm']
for name, mapping in hint['mappings'].items():
if name not in self.joints:
continue
if name in self.zero_columns:
initial[self.zero_columns[name]] = hint['offsets'][name]
for direction, values in enumerate((mapping.increasing_rad, mapping.decreasing_rad)):
self.set_initial_curve(initial, name, direction,
np.interp(self.knots[name], mapping.knots, values))
transforms = self.link_poses(initial, self.commands, self.directions)
for role, column in self.mount_columns.items():
index = self.observations[role][0][0]
initial[column:column+6] = pose_vector(np.linalg.inv(transforms[self.tag_links[role]][index])
@ self._pose(index, role, branch_by_role[role]))
self._initialization_costs[tuple(branches)] = self._refine_mount_guesses(initial, transforms,
observations=self.original_observations)
return initial
def fit_draft(bundle, candidate_completed=lambda _row: None):
"""Training-only representative; image precision is reported, not certified."""
candidates, audit = [], []
seed_width = min(6, len(bundle.tags))
for seed in product((0, 1), repeat=seed_width):
row = dict(seed=list(seed), converged=False)
try:
fit = bundle.fit(seed)
if not fit.success or not np.all(np.isfinite(fit.x)) or not np.all(np.isfinite(fit.fun)):
raise ValueError('draft_optimizer_did_not_converge')
bundle.output_mappings(fit.x)
residual = bundle.original_residual(fit.x) if hasattr(bundle, 'original_residual') else fit.fun
errors = np.sqrt(np.mean(np.sum(residual.reshape(-1, 4, 2)**2, axis=-1), axis=-1))
row.update(converged=True, coordinate_rms_px=float(np.sqrt(np.mean(residual**2))),
tag_rms_p95_px=float(np.percentile(errors, 95)), maximum_tag_rms_px=float(errors.max()),
original_image_precision_passed=bool(errors.max() <= 1.5),
evaluations=int(fit.nfev+fit.initialization_evaluations))
candidates.append((row, fit.x.copy()))
except (ValueError, np.linalg.LinAlgError) as error:
row['reason'] = str(error)
audit.append(row)
candidate_completed(row)
# A reviewable draft needs a bounded representative search, not a
# release claim that all geometric alternatives have been excluded.
if len(candidates) == 4:
break
if not candidates:
raise ValueError('draft_no_converged_training_candidate')
selected, parameters = min(candidates, key=lambda pair: pair[0]['coordinate_rms_px'])
jacobian = bundle.jacobian(parameters)*bundle.scale[None, :]
_u, singular, vectors = np.linalg.svd(jacobian, full_matrices=jacobian.shape[0] < jacobian.shape[1])
rows = getattr(bundle, 'original_residual_count', jacobian.shape[0])
rank = int(np.count_nonzero(singular > singular[0]*max(rows, jacobian.shape[1])*np.finfo(float).eps))
nullspace = vectors[rank:]
unresolved = [name for name, column in bundle.zero_columns.items()
if len(nullspace) and np.linalg.norm(nullspace[:, column]) > 1e-6]
if unresolved:
raise ValueError('draft_estimated_zero_unobservable:' + ','.join(unresolved))
zeros = {name: dict(estimated_deg=float(np.degrees(bundle.zero_offsets(parameters)[name])),
candidate_spread_deg=float(np.degrees(np.ptp([bundle.zero_offsets(value)[name]
for _, value in candidates])))) for name in bundle.zero_columns}
return parameters, dict(selected_seed=selected['seed'], selection='minimum_training_coordinate_rms',
validation_used_for_selection=False, precision_certified=False, candidates=audit,
solver_stopping_ftol=getattr(bundle, 'solver_ftol', 1e-9),
candidate_search_policy='uncertified_draft_up_to_four_converged_v1',
initializations_evaluated=len(audit), available_initializations=2**seed_width,
candidate_search_exhaustive=len(audit) == 2**seed_width,
estimated_zeros=zeros, local_parameter_rank=rank, parameter_count=len(bundle.scale)), candidates
@@ -0,0 +1,176 @@
"""Training-only candidate sets for the CAD-constrained image graph."""
from dataclasses import dataclass
import numpy as np
from ..geometry.tag_pose.finger_chain_images import chain_candidate_seeds
@dataclass(frozen=True)
class RawJointCandidate:
seed: tuple
parameters: np.ndarray
covariance: np.ndarray
rms_px: float
def __post_init__(self):
for field in ('parameters', 'covariance'):
value = np.array(getattr(self, field), copy=True)
value.flags.writeable = False
object.__setattr__(self, field, value)
@dataclass(frozen=True)
class RawJointFit:
candidates: tuple[RawJointCandidate, ...]
representative: int
uncertainty: dict
training_audit: tuple
training_selection: dict | None = None
def physical_outputs(bundle, parameters):
"""Physical quantities only: differing installation poses are not vetoes."""
offsets = bundle.zero_offsets(parameters)
values = bundle.mappings(parameters)
zeros = np.asarray([offsets[name] for name in bundle.joints])
# Absolute angles retain covariance between the zero and command curve.
commands = np.concatenate([(values[name]+offsets[name]).ravel() for name in bundle.joints])
axes, points = [], []
probes = [list(bundle.profile.command.baseline_values)]
for name, channel in bundle.channels.items():
for knot in bundle.knots[name]:
probe = probes[0].copy(); probe[channel] = knot; probes.append(probe)
probes = np.asarray(probes)
fk_points, fk_rotations = [], []
for direction in (0, 1):
poses = bundle.link_poses(parameters, probes, np.full(probes.shape, direction))
if direction == 0:
for name in bundle.joints:
joint = bundle.model.joints[bundle.cad_names[name]]
frame = poses[joint.parent][0] @ joint.origin
axes.append(frame[:3, :3] @ joint.axis)
points.append(frame[:3, 3])
for link in sorted(set(bundle.tag_links.values())):
fk_points.extend(poses[link][:, :3, 3])
fk_rotations.extend(poses[link][:, :3, :3].reshape(-1, 9))
return dict(zero_rad=zeros, command_rad=commands, axis=np.asarray(axes),
axis_point_m=np.asarray(points), fk_point_m=np.asarray(fk_points),
fk_rotation=np.asarray(fk_rotations))
def candidate_uncertainty(bundle, candidate):
from .raw_joint_uncertainty import physical_intervals
return physical_outputs(bundle, candidate.parameters), physical_intervals(bundle, candidate)
def certify_candidate_union(bundle, candidates):
if not candidates:
raise ValueError('raw_joint_no_training_candidate')
evaluated = [candidate_uncertainty(bundle, candidate) for candidate in candidates]
limits = dict(zero_rad=np.radians(1), command_rad=np.radians(1),
axis=2*np.sin(np.radians(3)/2), axis_point_m=.0015,
fk_point_m=.003, fk_rotation=2*np.sqrt(2)*np.sin(np.radians(1)/2))
audit, failures, radii = {}, [], {}
for key, limit in limits.items():
values = np.asarray([item[0][key] for item in evaluated])
widths = np.asarray([item[1][key] for item in evaluated])
radii[key] = []
for first in range(len(candidates)):
difference = values-values[first]
if difference.ndim == 3:
difference = np.linalg.norm(difference, axis=-1)
else:
difference = np.abs(difference)
radii[key].append(float(np.max(difference+widths)))
# Choose a physical minimax representative only AFTER forming the union.
# Pixel scores and installation uniqueness never choose the exported zero.
representative = int(np.argmin(np.max(np.asarray([np.asarray(radii[k])/v
for k, v in limits.items()]), axis=0)))
for key, limit in limits.items():
radius = radii[key][representative]
audit[key] = dict(maximum_union_radius=radius, limit=float(limit), passed=bool(radius <= limit))
if radius > limit:
failures.append(key)
values = np.asarray([item[0][key] for item in evaluated])
widths = np.asarray([item[1][key] for item in evaluated])
difference = values-values[representative]
difference = np.linalg.norm(difference,axis=-1) if difference.ndim == 3 else np.abs(difference)
affected = np.flatnonzero(np.max(difference+widths,axis=0) > limit)
if key in {'zero_rad','axis','axis_point_m'}:
names = {bundle.joints[int(index)] for index in affected}
elif key == 'command_rad':
labels = [name for name in bundle.joints for _ in range(2*len(bundle.knots[name]))]
names = {labels[int(index)] for index in affected}
else:
probes = 1+sum(len(nodes) for nodes in bundle.knots.values())
links = sorted(set(bundle.tag_links.values()))
affected_links = {links[(int(index)//probes)%len(links)] for index in affected}
names = {name for name in bundle.joints if bundle.model.joints[bundle.cad_names[name]].child in affected_links}
audit[key]['affected_joints'] = sorted(names)
audit[key]['required_observations'] = [dict(joint=name,
task=next(task.key for task in bundle.profile.motion.tasks if name in task.joints),
view=bundle.profile.measurement.measurements[name].view,
tag_roles=[bundle.profile.measurement.measurements[name].parent_role,
bundle.profile.measurement.measurements[name].child_role],
need=('bidirectional_steady_command_support' if key == 'command_rad' else
'simultaneous_link_observations_with_distinct_axis_or_held_pose_geometry'),
repeat_identical_trajectory_automatically=False) for name in sorted(names)]
if any(abs(v) > np.radians(20) for candidate in candidates
for v in bundle.zero_offsets(candidate.parameters).values()):
failures.append('absolute_zero_exceeds_20deg')
audit['passed'] = not failures
audit['failures'] = failures
audit['scope'] = 'training_command_grid_and_single_joint_fk; not_arbitrary_multiaxis'
return audit, representative
def fit_raw_joint_candidates(bundle, *, cancelled=lambda: False, candidate_completed=lambda _row: None,
workers=1):
"""The existing 64 seeds / 200 evaluations; validation is not an input."""
from concurrent.futures import ThreadPoolExecutor
if type(workers) is not int or not 1 <= workers <= 4:
raise ValueError('raw_joint_worker_budget_invalid')
candidates, audit = [], []
def solve(seed):
if cancelled():
raise ValueError('operator_abort:no_artifact_publication')
row = dict(seed=list(seed), converged=False, accepted_for_uncertainty=False)
try:
fit = bundle.fit(seed)
row.update(converged=bool(fit.success), evaluations=int(fit.nfev)+fit.initialization_evaluations,
joint_evaluations=int(fit.nfev), initialization_evaluations=fit.initialization_evaluations,
rms_px=float(np.sqrt(np.mean(fit.fun**2))))
if not fit.success:
raise ValueError('optimizer_budget_exhausted')
from ..geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS
errors = np.sqrt(np.mean(fit.fun.reshape(-1, 4, 2)**2, axis=(1, 2))*2)
row['maximum_training_tag_rms_px'] = float(np.max(errors))
if np.max(errors) > DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px:
row['training_excluded'] = 'declared_image_reprojection_limit'
return None, row
covariance = bundle.covariance(fit)
bundle.output_mappings(fit.x) # Reject nonmonotonic physical curves.
candidate = RawJointCandidate(tuple(seed), fit.x, covariance, row['rms_px'])
row['accepted_for_uncertainty'] = True
except (ValueError, np.linalg.LinAlgError) as error:
# Failure to initialize/converge cannot prove an alternative false.
row['unresolved_reason'] = str(error)
candidate = None
return candidate, row
with ThreadPoolExecutor(max_workers=workers) as pool:
for candidate, row in pool.map(solve, chain_candidate_seeds()):
if candidate is not None:
candidates.append(candidate)
audit.append(row)
candidate_completed(row)
if any(not row['accepted_for_uncertainty'] and not row.get('training_excluded') for row in audit):
raise ValueError('raw_joint_unresolved_training_candidates:see_raw_joint_training.json')
from .raw_joint_selection import reject_training_distinguishable_candidates
candidates, selection = reject_training_distinguishable_candidates(bundle, tuple(candidates))
excluded = {tuple(seed) for seed in selection.get('excluded_seeds', ())}
for row in audit:
if tuple(row['seed']) in excluded:
row.update(accepted_for_uncertainty=False, training_excluded='paired_training_loss_holm')
uncertainty, representative = certify_candidate_union(bundle, candidates)
return RawJointFit(candidates, representative, uncertainty, tuple(audit), selection)
@@ -0,0 +1,80 @@
"""Training-only rejection of distinguishable image hypotheses.
Repeated holds and optimizer seeds are not independent observations. Compare
one median paired loss per distinct command/direction pose, with the existing
0.03 px resolution margin and Holm familywise correction. Survivors, including
physically different tied solutions, all proceed to covariance propagation.
"""
from collections import defaultdict
import numpy as np
from scipy.stats import ttest_1samp, wilcoxon
from ..geometry.tag_pose.image_model_selection import holm_adjusted_probabilities
from ..geometry.tag_pose.parameters import DEFAULT_REPROJECTION_TIE_PX
def _families(bundle, candidates):
families = []
for index, candidate in enumerate(candidates):
for family in families:
if all(np.max(np.abs(candidate.parameters-candidates[other].parameters)/bundle.scale) <= 1e-5
for other in family):
family.append(index)
break
else:
families.append([index])
return families
def _probability(differences):
if len(differences) < 6 or not np.all(np.isfinite(differences)) or np.mean(differences) <= 0:
return 1.
if np.ptp(differences) < 1e-12:
return 2.**-len(differences)
mean = float(ttest_1samp(differences,0.,alternative='greater').pvalue)
rank = float(wilcoxon(differences,alternative='greater',zero_method='wilcox').pvalue)
return max(mean,rank) if np.isfinite(mean) and np.isfinite(rank) else 1.
def reject_training_distinguishable_candidates(bundle, candidates):
if not candidates:
return (), dict(policy='training_paired_pose_losses_holm_v1', families=[], comparisons=[])
families = _families(bundle,candidates)
anchor = min(range(len(candidates)),key=lambda i:candidates[i].rms_px)
predictions = [bundle.project_observations(c.parameters,bundle.commands,bundle.directions,bundle.observations)
for c in candidates]
probabilities, comparisons = [], []
for family_index,family in enumerate(families):
if anchor in family:
continue
for role,(indices,measured,_matrices,_objects) in bundle.observations.items():
base = np.sqrt(np.mean(np.sum((predictions[anchor][role]-measured)**2,axis=2),axis=1))
# Only commanded ancestors move this role. Other fingers' repeated
# holds cannot multiply the evidence for its installation branch.
ancestors = {joint.name for joint in bundle.chains[role]}
channels = sorted(bundle.channels[name] for name in bundle.joints if bundle.cad_names[name] in ancestors)
strata = defaultdict(list)
for local,index in enumerate(indices):
key = tuple((float(bundle.commands[index,ch]),int(bundle.directions[index,ch])) for ch in channels)
strata[key].append(local)
pvalues = []
for member in family:
other = np.sqrt(np.mean(np.sum((predictions[member][role]-measured)**2,axis=2),axis=1))
losses = other**2-(base+DEFAULT_REPROJECTION_TIE_PX)**2
differences = np.asarray([np.median(losses[items]) for items in strata.values()])
pvalues.append(_probability(differences))
index = len(comparisons)
probabilities.append((index,max(pvalues)))
comparisons.append(dict(family=family_index,role=role,pose_strata=len(strata),
probability=max(pvalues)))
excluded = set()
for index,adjusted in holm_adjusted_probabilities(probabilities):
comparisons[index]['holm_probability'] = adjusted
if adjusted < .01:
excluded.update(families[comparisons[index]['family']])
retained = tuple(c for i,c in enumerate(candidates) if i not in excluded)
return retained, dict(policy='training_paired_pose_losses_holm_v1',
training_comparison_anchor=list(candidates[anchor].seed), pixel_margin=DEFAULT_REPROJECTION_TIE_PX,
familywise_alpha=.01, families=[[list(candidates[i].seed) for i in family] for family in families],
excluded_seeds=[list(candidates[i].seed) for i in sorted(excluded)], comparisons=comparisons)
@@ -0,0 +1,91 @@
"""Exact first derivatives of physical outputs, including full covariance."""
import numpy as np
from ..geometry.tag_pose.urdf_command_images import rigid, right_jacobian, skew
def angle_derivatives(bundle, commands, directions):
derivatives = {}
width = len(bundle.scale)
for name, channel in bundle.channels.items():
jac = np.zeros((len(commands), width))
if name in bundle.zero_columns:
jac[:, bundle.zero_columns[name]] = 1.
weights = bundle.curve_weights(name, commands[:,channel], directions[:,channel])
for direction in range(2):
for node, column in enumerate(bundle.curve_columns[name][direction]):
if column >= 0:
jac[:,column] += weights[:,direction,node]
derivatives[bundle.cad_names[name]] = jac
return derivatives
def pose_derivatives(bundle, parameters, commands, directions):
poses = bundle.link_poses(parameters, commands, directions)
angles = angle_derivatives(bundle, commands, directions)
shape = (len(commands), 3, len(parameters))
position, rotation = np.zeros(shape), np.zeros(shape)
position[:,:,3:6] = np.eye(3)
rotation[:,:,:3] = rigid(parameters[:6])[:3,:3] @ right_jacobian(parameters[:3])
jacobians = {bundle.model.root_link: (position, rotation)}
frames = {}
for chain in bundle.chains.values():
for joint in chain:
if joint.child in jacobians:
continue
parent_position, parent_rotation = jacobians[joint.parent]
frame = poses[joint.parent] @ joint.origin
displacement = frame[:,:3,3]-poses[joint.parent][:,:3,3]
point = parent_position-skew(displacement) @ parent_rotation
angular = parent_rotation.copy()
if joint.kind != 'fixed':
axis = frame[:,:3,:3] @ joint.axis
angular += axis[:,:,None]*angles[joint.name][:,None,:]
frames[joint.name] = (point, -skew(axis) @ parent_rotation)
jacobians[joint.child] = point, angular
return poses, jacobians, frames
def physical_intervals(bundle, candidate):
parameters, covariance = candidate.parameters, candidate.covariance
def interval(jac):
# Scalar or vector three-sigma radius, preserving every correlation.
shape = jac.shape[:-1]
flat = jac.reshape(-1, len(parameters))
from scipy.sparse import csr_matrix
sparse = csr_matrix(flat)
variance = np.asarray(sparse.multiply(sparse @ covariance).sum(axis=1)).ravel().reshape(shape)
return 3*np.sqrt(np.maximum(variance, 0.).sum(axis=-1) if len(shape)>1 else np.maximum(variance, 0.))
zeros = np.zeros((len(bundle.joints), len(parameters)))
commands = []
for i, name in enumerate(bundle.joints):
if name in bundle.zero_columns:
zeros[i,bundle.zero_columns[name]] = 1.
for direction in (0, 1):
basis = bundle.curve_weights(name, bundle.knots[name],
np.full(len(bundle.knots[name]), direction))
for weights in basis:
row = zeros[i].copy()
for column, weight in zip(bundle.curve_columns[name].ravel(), weights.ravel()):
if column >= 0:
row[column] += weight
commands.append(row)
result = dict(zero_rad=interval(zeros), command_rad=interval(np.asarray(commands)))
probes = [list(bundle.profile.command.baseline_values)]
for name, channel in bundle.channels.items():
for knot in bundle.knots[name]:
probe = probes[0].copy(); probe[channel] = knot; probes.append(probe)
probes = np.asarray(probes)
points, rotations = [], []
for direction in (0,1):
poses, jacobians, frames = pose_derivatives(bundle, parameters, probes, np.full(probes.shape, direction))
if direction == 0:
result['axis_point_m'] = interval(np.asarray([frames[bundle.cad_names[n]][0][0] for n in bundle.joints]))
result['axis'] = interval(np.asarray([frames[bundle.cad_names[n]][1][0] for n in bundle.joints]))
for link in sorted(set(bundle.tag_links.values())):
point, angular = jacobians[link]
points.extend(interval(point).tolist())
# ||dR||_F = sqrt(2)||angular velocity||, exact rotation metric.
rotations.extend((np.sqrt(2)*interval(angular)).tolist())
result['fk_point_m'], result['fk_rotation'] = np.asarray(points), np.asarray(rotations)
return result
@@ -0,0 +1,88 @@
"""Constrained batch updates of the existing image objective, without motion.
The caller supplies the new batch's camera geometry and immutable reference
parameters. Unselected joints and tag installations cannot move in the fit.
"""
import numpy as np
from scipy.optimize import least_squares
def check_update_zero_observability(bundle, parameters, active, joints):
"""Reject zero gauges, conditional on the explicitly frozen reference."""
jacobian = bundle.jacobian(parameters)[:, active]*bundle.scale[active]
_, singular, vectors = np.linalg.svd(jacobian,
full_matrices=jacobian.shape[0] < jacobian.shape[1])
rows = getattr(bundle, 'original_residual_count', jacobian.shape[0])
rank = int(np.count_nonzero(singular > singular[0]*max(rows, len(active))*np.finfo(float).eps))
nullspace = vectors[rank:]
positions = {column: i for i, column in enumerate(active)}
unresolved = [name for name in joints if name in bundle.zero_columns
and (bundle.zero_columns[name] not in positions or (len(nullspace)
and np.linalg.norm(nullspace[:, positions[bundle.zero_columns[name]]]) > 1e-6))]
if unresolved:
raise ValueError('batch_update_zero_unobservable:' + ','.join(unresolved))
return rank
def transfer_parameters(source, parameters, target):
"""Copy semantic parameters; array positions may differ between scopes."""
result = np.zeros(len(target.scale))
result[:6] = parameters[:6]
for name, columns in target.curve_columns.items():
if not np.array_equal(source.knots[name], target.knots[name]):
raise ValueError('batch_update_training_knots_changed:' + name)
mask = columns >= 0
original = source.curve_columns[name]
if not np.array_equal(mask, original >= 0):
raise ValueError('batch_update_baseline_changed:' + name)
result[columns[mask]] = parameters[original[mask]]
for name, column in target.zero_columns.items():
result[column] = source.zero_offsets(parameters)[name]
for role, column in target.mount_columns.items():
if source.tag_links[role] != target.tag_links[role]:
raise ValueError('batch_update_tag_link_changed:' + role)
original = source.mount_columns[role]
result[column:column+6] = parameters[original:original+6]
return result
def fit_joint_update(bundle, initial, joints, mount_roles, *, fit_palm=True):
"""Use the shared bounded solver while keeping the reference hand fixed."""
if not set(joints) <= set(bundle.joints) or not set(mount_roles) <= set(bundle.tags):
raise ValueError('batch_update_scope_invalid')
columns = list(range(6)) if fit_palm else []
for name in joints:
if name in bundle.zero_columns:
columns.append(bundle.zero_columns[name])
columns.extend(bundle.curve_columns[name][bundle.curve_columns[name] >= 0])
for role in mount_roles:
start = bundle.mount_columns[role]
columns.extend(range(start, start+6))
proposed = np.asarray(sorted(set(columns)), dtype=int)
# Unobserved increments cannot acquire invented travel through optimizer
# drift. Retain their input values and report that they were not fitted.
supported = np.linalg.norm(bundle.jacobian(initial)[:, proposed], axis=0) > 1e-8
active = proposed[supported]
if not len(active):
raise ValueError('batch_update_no_observed_parameters')
def unpack(values):
parameters = initial.copy()
parameters[active] = values
return parameters
options = dict(bundle.solver_options)
lower, upper = options.pop('bounds')
fit = least_squares(lambda x: bundle.residual(unpack(x)), initial[active],
jac=lambda x: bundle.solver_jacobian(unpack(x))[:, active],
bounds=(lower[active], upper[active]), x_scale=bundle.scale[active],
max_nfev=100, ftol=bundle.solver_ftol, **options)
parameters = unpack(fit.x)
rank = check_update_zero_observability(bundle, parameters, active, joints)
derived = {row['column'] for rows in getattr(bundle, 'turnaround_constraints', {}).values() for row in rows}
audit = dict(converged=bool(fit.success), evaluations=int(fit.nfev),
coordinate_rms_px=float(np.sqrt(np.mean(bundle.original_residual(parameters)**2))),
active_columns=active.tolist(),
derived_turnaround_columns=sorted(derived.intersection(proposed.tolist())),
unobserved_retained_columns=[int(column) for column in proposed[~supported] if column not in derived],
validation_used=False, fixed_cad_axes=True, conditional_parameter_rank=rank,
observability_condition='unselected_joints_and_tag_mounts_fixed')
return parameters, audit

Some files were not shown because too many files have changed in this diff Show More