Complete calibration package import
This commit is contained in:
@@ -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")
|
||||
+14
@@ -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/` 中的归档,不要重新接入生产入口。
|
||||
+1
@@ -0,0 +1 @@
|
||||
"""Read-only compatibility for archived sessions and diagnostic tools."""
|
||||
+17
@@ -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",
|
||||
]
|
||||
+3
@@ -0,0 +1,3 @@
|
||||
"""Compatibility imports for the G20 model package."""
|
||||
from linkerhand_calibration.models.g20 import register_profiles
|
||||
__all__ = ["register_profiles"]
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+3
@@ -0,0 +1,3 @@
|
||||
"""Compatibility imports for the L6 model package."""
|
||||
from linkerhand_calibration.models.l6 import register_profiles
|
||||
__all__ = ["register_profiles"]
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+3
@@ -0,0 +1,3 @@
|
||||
"""Compatibility imports for the O12 model package."""
|
||||
from linkerhand_calibration.models.o12 import register_profiles
|
||||
__all__ = ["register_profiles"]
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+3
@@ -0,0 +1,3 @@
|
||||
"""Compatibility imports for the O6 model package."""
|
||||
from linkerhand_calibration.models.o6 import register_profiles
|
||||
__all__ = ["register_profiles"]
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+4
@@ -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")
|
||||
+25
@@ -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"]
|
||||
+251
@@ -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"
|
||||
)
|
||||
)
|
||||
+283
@@ -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)
|
||||
+107
@@ -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)
|
||||
+91
@@ -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
Reference in New Issue
Block a user