离线拟合

This commit is contained in:
lxp
2026-09-24 12:22:48 +08:00
parent b362e9bb40
commit 50ff372ca3
19 changed files with 290 additions and 8 deletions
@@ -34,6 +34,11 @@ ros2 run linkerhand_calibration calibrate_hand \
后续恢复省略 `--no-resume`,仍需提供当前安装条件下的光学观测。若安装参考不可核验,按拒绝原因处理,不能改哈希绕过。 后续恢复省略 `--no-resume`,仍需提供当前安装条件下的光学观测。若安装参考不可核验,按拒绝原因处理,不能改哈希绕过。
2026-09-22 用户明确要求复用已完成的相机标定:可将上述光学观测参数换为
`--reuse-camera-calibration`。此入口仍核对已有相机标定质量、受保护文件和实时
CameraInfo;日志及发布审计记录 `current_optical_verification: not_performed`,
不宣称本次重新做过光学核验。第三轮独立关节精度、候选不确定度与 URDF 发布检查不变。
现有 `three_camera_extrinsics.launch.py` 增加 `verification_extrinsics_file`:指定受保护外参文件、另设 JSON `output_file`,沿用棋盘格采集及保存操作。该模式不重拟合相机参数,保存同步多姿态角点,并验证冻结内外参;每个机位至少 15 组,跨机位同步不超过 50 ms,同时检查图像/倾角覆盖及原重投影门限。正常外参标定模式不变。核验模式的角点 JSON 用于上述启动参数。 现有 `three_camera_extrinsics.launch.py` 增加 `verification_extrinsics_file`:指定受保护外参文件、另设 JSON `output_file`,沿用棋盘格采集及保存操作。该模式不重拟合相机参数,保存同步多姿态角点,并验证冻结内外参;每个机位至少 15 组,跨机位同步不超过 50 ms,同时检查图像/倾角覆盖及原重投影门限。正常外参标定模式不变。核验模式的角点 JSON 用于上述启动参数。
关键文件: 关键文件:
+7
View File
@@ -554,6 +554,13 @@ Schur 协方差与完整逆矩阵一致、冻结模型拒绝无法解释的阶
## O30 raw_joint 正式链路(2026-09-22) ## O30 raw_joint 正式链路(2026-09-22)
用户指定停滞重试(2026-09-22):仅 O30 的 `mechanical_stall` 增加同一运动
首次尝试加两次重试,每次重发前等待 3 秒,第 3 次失败停止。等待不发送新位置
指令,不作为有效采样;失联、控制权冲突及其他保护仍检查。真实 coordinator
虚拟时钟验证三次失败、两次等待和等待期间保护:3 passed,1.00 秒。
既有安全及反馈组 44 passed;另一条旧启动测试补充显式复用相机参数后通过。
测试夹具曾缺少 SDK 心跳,已补齐模拟心跳,未因此改变生产通信保护。
新默认协议及完整边界见 `O30_RAW_PROTOCOL_IMPLEMENTATION.md`。 新默认协议及完整边界见 `O30_RAW_PROTOCOL_IMPLEMENTATION.md`。
针对性验证(不同组有重叠,不相加宣称一次全量回归): 针对性验证(不同组有重叠,不相加宣称一次全量回归):
@@ -306,6 +306,7 @@ def _launch_stack(context):
"capture_plan_expected_sha256": ParameterValue( "capture_plan_expected_sha256": ParameterValue(
LaunchConfiguration("capture_plan_expected_sha256"), value_type=str), LaunchConfiguration("capture_plan_expected_sha256"), value_type=str),
"initial_command_file": LaunchConfiguration("initial_command_file"), "initial_command_file": LaunchConfiguration("initial_command_file"),
"reuse_camera_calibration": ParameterValue(LaunchConfiguration("reuse_camera_calibration"), value_type=bool),
"camera_optical_observations_file": LaunchConfiguration("camera_optical_observations_file"), "camera_optical_observations_file": LaunchConfiguration("camera_optical_observations_file"),
"command_topic": command_topic, "command_topic": command_topic,
"state_topic": state_topic, "state_topic": state_topic,
@@ -469,6 +470,7 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("preparation_witnesses_json", default_value=""), DeclareLaunchArgument("preparation_witnesses_json", default_value=""),
DeclareLaunchArgument("capture_plan_expected_sha256", default_value=""), DeclareLaunchArgument("capture_plan_expected_sha256", default_value=""),
DeclareLaunchArgument("initial_command_file", default_value=""), DeclareLaunchArgument("initial_command_file", default_value=""),
DeclareLaunchArgument("reuse_camera_calibration", default_value="false"),
DeclareLaunchArgument("camera_optical_observations_file", default_value=""), DeclareLaunchArgument("camera_optical_observations_file", default_value=""),
DeclareLaunchArgument( DeclareLaunchArgument(
"calibration_config", "calibration_config",
@@ -82,10 +82,18 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur
quality_limits=profile.vision.extrinsics_quality_limits, quality_limits=profile.vision.extrinsics_quality_limits,
minimum_capture_counts=profile.vision.minimum_capture_counts) minimum_capture_counts=profile.vision.minimum_capture_counts)
checks = [r for r in records if r.get('kind') == 'measured_optical_verification'] checks = [r for r in records if r.get('kind') == 'measured_optical_verification']
if not checks: reuse = [r for r in records if r.get('kind') == 'existing_camera_calibration_reused']
if checks:
optical = verify_optical_observations(checks[-1]['observations'], extrinsics, matrices,
extrinsics_sha256=protected_inputs['camera_extrinsics_sha256'])
elif len(reuse) == 1:
from ..optical_verification import accept_existing_camera_calibration
optical = accept_existing_camera_calibration(profile, extrinsics_path,
protected_inputs['camera_extrinsics_sha256'])
if reuse[0]['acceptance'] != optical:
raise ValueError('raw_joint_camera_reuse_evidence_changed')
else:
raise ValueError('raw_joint_measured_optical_verification_missing') raise ValueError('raw_joint_measured_optical_verification_missing')
optical = verify_optical_observations(checks[-1]['observations'], extrinsics, matrices,
extrinsics_sha256=protected_inputs['camera_extrinsics_sha256'])
model = UrdfKinematicModel(source) model = UrdfKinematicModel(source)
bundle = UrdfCommandImages(profile, model, extrinsics, training, compile_tag_links(profile, model)) bundle = UrdfCommandImages(profile, model, extrinsics, training, compile_tag_links(profile, model))
audit = [] audit = []
@@ -125,7 +133,9 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur
serial_number=serial_number, protected_inputs=protected_inputs) serial_number=serial_number, protected_inputs=protected_inputs)
from ...core.domain.capture_plan import evidence_digest from ...core.domain.capture_plan import evidence_digest
report['raw_joint_audit'].update(installation_reference_sha256=evidence_digest(reference), report['raw_joint_audit'].update(installation_reference_sha256=evidence_digest(reference),
optical_observations_sha256=optical['observations_sha256']) camera_acceptance=optical)
if 'observations_sha256' in optical:
report['raw_joint_audit']['optical_observations_sha256'] = optical['observations_sha256']
naming = dict(serial_number=serial_number, side=profile.key.side, model=profile.key.model.lower()) naming = dict(serial_number=serial_number, side=profile.key.side, model=profile.key.model.lower())
json_path = directory/profile.artifacts.calibration_filename.format(**naming) json_path = directory/profile.artifacts.calibration_filename.format(**naming)
urdf_path = directory/profile.artifacts.corrected_urdf_filename.format(**naming) urdf_path = directory/profile.artifacts.corrected_urdf_filename.format(**naming)
@@ -28,7 +28,11 @@ class RawJointArtifactValidator:
report = json.loads((json_path.parent / REPORT_FILENAME).read_text(encoding='utf-8')) report = json.loads((json_path.parent / REPORT_FILENAME).read_text(encoding='utf-8'))
if evidence_digest(report) != evidence_digest(self.report) or payload != from_report(self.report): if evidence_digest(report) != evidence_digest(self.report) or payload != from_report(self.report):
raise ValueError('raw_joint_serialized_report_changed') raise ValueError('raw_joint_serialized_report_changed')
if not self.validation['passed'] or not self.optical['passed']: camera_accepted = self.optical.get('passed') is True or (
self.optical.get('policy') == 'reuse_validated_camera_calibration_v1'
and self.optical.get('accepted') is True
and self.optical.get('camera_extrinsics_sha256') == self.report['protected_inputs']['camera_extrinsics_sha256'])
if not self.validation['passed'] or not camera_accepted:
raise ValueError('raw_joint_independent_acceptance_failed') raise ValueError('raw_joint_independent_acceptance_failed')
bundle, profile = self.bundle, self.bundle.profile bundle, profile = self.bundle, self.bundle.profile
offsets = bundle.zero_offsets(self.candidate.parameters) offsets = bundle.zero_offsets(self.candidate.parameters)
@@ -85,6 +85,8 @@ class CalibrationCoordinator:
from .training import TrainingWorker from .training import TrainingWorker
self.training_worker = TrainingWorker() self.training_worker = TrainingWorker()
self.safety = SafetyPolicy(profile) self.safety = SafetyPolicy(profile)
from .stall_retry import StallRetry
self.stall_retry = StallRetry() if profile.key.model.upper() == 'O30' else None
self.reference_lock = self._new_reference_lock() self.reference_lock = self._new_reference_lock()
self.trackers = parameters.new_trackers(self.required_observation_views) self.trackers = parameters.new_trackers(self.required_observation_views)
self.capture = ObservationCapture(profile, reference_lock=self.reference_lock, self.capture = ObservationCapture(profile, reference_lock=self.reference_lock,
@@ -512,6 +514,8 @@ class CalibrationCoordinator:
if row.get("kind") in {"diagnostic_observation_frame", "raw_observation_frame"}: if row.get("kind") in {"diagnostic_observation_frame", "raw_observation_frame"}:
row.update(session_epoch=epoch, motion_version=motion_version, row.update(session_epoch=epoch, motion_version=motion_version,
motion_started_ns=self._motion_started_ns) motion_started_ns=self._motion_started_ns)
if self.stall_retry is not None and self.stall_retry.waiting:
row.update(sample_phase='stall_retry_wait', accepted_sample_window_open=False)
if motion.phase == "steady" and motion.minimum_hold_seconds > 0: if motion.phase == "steady" and motion.minimum_hold_seconds > 0:
row.update(diagnostic_hold_timing(command_history, target=motion.target, row.update(diagnostic_hold_timing(command_history, target=motion.target,
image_stamp_ns=stamp, session_epoch=epoch, motion_version=motion_version, image_stamp_ns=stamp, session_epoch=epoch, motion_version=motion_version,
@@ -658,12 +662,38 @@ class CalibrationCoordinator:
self.state_receive_times[-1] if self.state_receive_times else None, self.state_receive_times[-1] if self.state_receive_times else None,
tuple(current), tuple(self.latest_feedback), fresh, tuple(current), tuple(self.latest_feedback), fresh,
competing_controller=self.ports.command_publisher_count() > 1, competing_controller=self.ports.command_publisher_count() > 1,
motion_expected=self.segment.motion_expected if self.segment else False, motion_expected=(self.segment.motion_expected if self.segment else False)
and not (self.stall_retry and self.stall_retry.waiting),
motion_id=self.segment.identity if self.segment else "", motion_id=self.segment.identity if self.segment else "",
motion_goals=self.segment.goals if self.segment else ())) motion_goals=self.segment.goals if self.segment else ()))
if not decision.safe: if not decision.safe:
if (decision.code == 'mechanical_stall' and self.stall_retry is not None
and self.segment is not None):
retry = self.stall_retry.failed(self.segment.identity, now)
append_jsonl(self.raw_path, dict(kind='motion_stall_retry',
stamp_ns=self.ports.clock_ns(), motion_id=self.segment.identity,
failure_count=self.stall_retry.failures, maximum_failures=3,
delay_seconds=3.0 if retry else 0., retry_scheduled=retry,
target=list(self.segment.command.target), reason=decision.details))
if retry:
self._steady_capture_after_ns = None
self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/3:delay_seconds=3'
return
self._pause(f'{decision.code}:{decision.reason}:{decision.details},consecutive_failures=3')
return
self._pause(f"{decision.code}:{decision.reason}:{decision.details}") self._pause(f"{decision.code}:{decision.reason}:{decision.details}")
return return
if self.stall_retry is not None and self.stall_retry.waiting:
elapsed = self.stall_retry.resume(now)
if elapsed is None:
self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/3:remaining={self.stall_retry.remaining(now):.1f}s'
return
self.segment.suspend_elapsed(elapsed)
self._steady_capture_after_ns = None
self._steady_rows.clear()
append_jsonl(self.raw_path, dict(kind='motion_stall_retry_resumed',
stamp_ns=self.ports.clock_ns(), motion_id=self.segment.identity,
attempt=self.stall_retry.failures+1, waited_seconds=elapsed))
if self.visual_motion is not None and self.segment is not None: if self.visual_motion is not None and self.segment is not None:
reason = self.visual_motion.check(self.segment, now) reason = self.visual_motion.check(self.segment, now)
if reason: if reason:
@@ -1543,6 +1573,17 @@ class CalibrationCoordinator:
from .optical_verification import verify_optical_observations from .optical_verification import verify_optical_observations
from .camera_preflight import CameraCheck from .camera_preflight import CameraCheck
path = self.parameters.camera_optical_observations_file path = self.parameters.camera_optical_observations_file
if path is None and self.parameters.reuse_camera_calibration:
from .optical_verification import accept_existing_camera_calibration
report = accept_existing_camera_calibration(self.profile,
self.parameters.camera_extrinsics_file,
self.parameters.protected_inputs['camera_extrinsics_sha256'])
append_jsonl(self.raw_path, dict(kind='existing_camera_calibration_reused',
stamp_ns=self.ports.clock_ns(), acceptance=report))
atomic_write_json(self.parameters.session_dir / 'camera_preflight.json',
dict(kind='camera_preflight', stage='at_start',
**self._camera_preflight.as_dict(), camera_acceptance=report))
return
if path is None: if path is None:
raise ValueError('missing_measured_optical_observations') raise ValueError('missing_measured_optical_observations')
dataset = json.loads(path.read_text(encoding='utf-8')) dataset = json.loads(path.read_text(encoding='utf-8'))
@@ -132,6 +132,14 @@ class MotionExecution:
raise ValueError("invalid measured feedback target") raise ValueError("invalid measured feedback target")
self.reference_tolerance = 2*self.stability self.reference_tolerance = 2*self.stability
def suspend_elapsed(self, seconds):
"""Exclude a retry delay from motion and settling clocks."""
self.started_at += seconds
if self.finished_at is not None:
self.finished_at += seconds
self.history.clear()
self.stamped_history.clear()
def sample(self, now: float) -> tuple[float, ...]: def sample(self, now: float) -> tuple[float, ...]:
distance = max((abs(v) for v in self.deltas), default=0.0) distance = max((abs(v) for v in self.deltas), default=0.0)
elapsed = max(0.0, float(now)-self.started_at) elapsed = max(0.0, float(now)-self.started_at)
@@ -12,6 +12,24 @@ from ..core.geometry.tag_pose.urdf_command_images import rigid
from ..core.geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS from ..core.geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS
def accept_existing_camera_calibration(profile, path, expected_sha256):
"""Explicit reuse of prior calibration, without claiming a fresh optical check."""
import hashlib
from pathlib import Path
from ..core.geometry.extrinsics import load_camera_extrinsics
source = Path(path)
if hashlib.sha256(source.read_bytes()).hexdigest() != expected_sha256:
raise ValueError('existing_camera_calibration_changed')
calibrated = load_camera_extrinsics(source, required_views=profile.vision.view_names,
reference_view=profile.vision.extrinsic_reference_view,
quality_limits=profile.vision.extrinsics_quality_limits,
minimum_capture_counts=profile.vision.minimum_capture_counts)
return dict(policy='reuse_validated_camera_calibration_v1', accepted=True,
passed=False, current_optical_verification='not_performed',
camera_extrinsics_sha256=expected_sha256, prior_quality=dict(calibrated.quality),
intrinsics_updated=False, extrinsics_updated=False)
def verify_optical_observations(dataset, extrinsics, matrices, *, extrinsics_sha256): def verify_optical_observations(dataset, extrinsics, matrices, *, extrinsics_sha256):
if (dataset.get('protocol') != 'rectified_optical_check_v1' if (dataset.get('protocol') != 'rectified_optical_check_v1'
or dataset.get('camera_extrinsics_sha256') != extrinsics_sha256 or dataset.get('camera_extrinsics_sha256') != extrinsics_sha256
@@ -43,6 +43,7 @@ class RuntimeParameters:
initial_command: tuple[float, ...] = () initial_command: tuple[float, ...] = ()
initial_device_uid: str = "" initial_device_uid: str = ""
resume_mode: str = "verify" resume_mode: str = "verify"
reuse_camera_calibration: bool = False
camera_optical_observations_file: Path | None = None camera_optical_observations_file: Path | None = None
def new_trackers(self, views): def new_trackers(self, views):
@@ -1,5 +1,5 @@
"""Reference-bound replay of the raw protocol's finite acquisition schedule.""" """Reference-bound replay of the raw protocol's finite acquisition schedule."""
from dataclasses import dataclass from dataclasses import dataclass, replace
from collections import defaultdict, deque from collections import defaultdict, deque
import numpy as np import numpy as np
@@ -76,7 +76,11 @@ class RawCheckpoint:
checker = ResumeVerifier(required_fixed_views=profile.vision.view_names, checker = ResumeVerifier(required_fixed_views=profile.vision.view_names,
required_fixed_poses=profile.vision.view_names, required_fixed_poses=profile.vision.view_names,
required_hashes=current['protected_hashes']) required_hashes=current['protected_hashes'])
decision = checker.compare(fingerprint_from_mapping(previous), fingerprint_from_mapping(current)) # Moving-tag installation is verified below from baseline pixels and
# feedback. A PnP branch is not an installation identity in this protocol.
decision = checker.compare(
replace(fingerprint_from_mapping(previous), moving_tag_poses={}),
replace(fingerprint_from_mapping(current), moving_tag_poses={}))
if not decision.reuse: if not decision.reuse:
raise ValueError('raw_resume_reference_changed:' + str(decision.incompatible_fields)) raise ValueError('raw_resume_reference_changed:' + str(decision.incompatible_fields))
old = self.fixed_reference.get('raw_installation_reference', {}) old = self.fixed_reference.get('raw_installation_reference', {})
@@ -170,6 +170,10 @@ def reason_zh(
if reason.startswith("mechanical_stall:"): if reason.startswith("mechanical_stall:"):
timeout = re.search(r"(?:[:,])timeout_seconds=([0-9]+(?:\.[0-9]+)?)(?:,|$)", reason) timeout = re.search(r"(?:[:,])timeout_seconds=([0-9]+(?:\.[0-9]+)?)(?:,|$)", reason)
duration = f" {timeout.group(1)} 秒" if timeout else "两秒" duration = f" {timeout.group(1)} 秒" if timeout else "两秒"
if 'consecutive_failures=3' in reason:
return ('MOTION-STALL-303',
f'同一运动连续 3 次失败,每次反馈{duration}未推进;两次重发前均等待 3 秒,已停止。',
'检查机械阻挡、执行器和实际行程;本次重试预算已用尽。')
return ( return (
"MOTION-STALL-303", "MOTION-STALL-303",
f"目标电机连续{duration}没有向目标推进,程序已保持当前位置。", f"目标电机连续{duration}没有向目标推进,程序已保持当前位置。",
@@ -33,6 +33,7 @@ def parameter_defaults(profile) -> dict[str, Any]:
"preparation_witnesses_json": "", "preparation_witnesses_json": "",
"capture_plan_expected_sha256": "", "capture_plan_expected_sha256": "",
"initial_command_file": "", "initial_command_file": "",
"reuse_camera_calibration": False,
"camera_optical_observations_file": "", "camera_optical_observations_file": "",
"command_topic": f"/{model}/cb_right_hand_control_cmd", "command_topic": f"/{model}/cb_right_hand_control_cmd",
"state_topic": f"/{model}/cb_right_hand_state", "state_topic": f"/{model}/cb_right_hand_state",
@@ -175,6 +176,7 @@ def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimePara
diagnostic_capture=diagnostic_capture, diagnostic_capture=diagnostic_capture,
initial_command=initial_command, initial_device_uid=device_uid, initial_command=initial_command, initial_device_uid=device_uid,
resume_mode=resume_mode, resume_mode=resume_mode,
reuse_camera_calibration=bool(value("reuse_camera_calibration")),
camera_optical_observations_file=(Path(str(value("camera_optical_observations_file"))).expanduser().resolve() camera_optical_observations_file=(Path(str(value("camera_optical_observations_file"))).expanduser().resolve()
if value("camera_optical_observations_file") else None), if value("camera_optical_observations_file") else None),
) )
@@ -117,6 +117,7 @@ def run_online(
config: ProductConfig, *, record_bag: bool = False, config: ProductConfig, *, record_bag: bool = False,
commands_enabled: bool = True, allow_resume: bool = True, commands_enabled: bool = True, allow_resume: bool = True,
diagnostic_capture: str = "", startup_timeout: float = 120.0, diagnostic_capture: str = "", startup_timeout: float = 120.0,
reuse_camera_calibration: bool = False,
camera_optical_observations_file: Path | None = None, camera_optical_observations_file: Path | None = None,
) -> int: ) -> int:
import rclpy import rclpy
@@ -231,6 +232,8 @@ def run_online(
else: else:
print(f"{diagnostic_capture_label(diagnostic.mode)}:仅执行声明任务各一次往返,保留准备和零位采集;不复用断点、不拟合或发布标定产物。", flush=True) print(f"{diagnostic_capture_label(diagnostic.mode)}:仅执行声明任务各一次往返,保留准备和零位采集;不复用断点、不拟合或发布标定产物。", flush=True)
launch_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} launch_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {}
if reuse_camera_calibration:
launch_options["reuse_camera_calibration"] = True
if camera_optical_observations_file is not None: if camera_optical_observations_file is not None:
launch_options["camera_optical_observations_file"] = camera_optical_observations_file launch_options["camera_optical_observations_file"] = camera_optical_observations_file
if commands_enabled and profile.vision_motion: if commands_enabled and profile.vision_motion:
@@ -346,6 +349,8 @@ def main(args: list[str] | None = None) -> None:
diagnostic_options.add_argument("--diagnostic-capture", choices=sorted(DIAGNOSTIC_CAPTURE_PRESETS), diagnostic_options.add_argument("--diagnostic-capture", choices=sorted(DIAGNOSTIC_CAPTURE_PRESETS),
default="", help="执行已声明的诊断任务,只保存诊断数据,不发布标定产物") default="", help="执行已声明的诊断任务,只保存诊断数据,不发布标定产物")
parser.add_argument("--offline-raw", default="") parser.add_argument("--offline-raw", default="")
parser.add_argument("--reuse-camera-calibration", action="store_true",
help="明确复用已验收相机标定,不执行新的棋盘格核验;记录本次未复核光学状态")
parser.add_argument("--camera-optical-observations", type=Path, parser.add_argument("--camera-optical-observations", type=Path,
help="当前安装下的多姿态同步标定板角点 JSON;新 O30 协议启动前重新计算光学核验") help="当前安装下的多姿态同步标定板角点 JSON;新 O30 协议启动前重新计算光学核验")
parser.add_argument("--offline-output", default="") parser.add_argument("--offline-output", default="")
@@ -422,6 +427,8 @@ def main(args: list[str] | None = None) -> None:
if selected.offline_output or selected.publish_offline: if selected.offline_output or selected.publish_offline:
parser.error("--offline-output/--publish-offline require --offline-raw") parser.error("--offline-output/--publish-offline require --offline-raw")
online_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} online_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {}
if selected.reuse_camera_calibration:
online_options["reuse_camera_calibration"] = True
if selected.camera_optical_observations is not None: if selected.camera_optical_observations is not None:
online_options["camera_optical_observations_file"] = selected.camera_optical_observations online_options["camera_optical_observations_file"] = selected.camera_optical_observations
if selected.startup_timeout_seconds is not None: if selected.startup_timeout_seconds is not None:
@@ -98,10 +98,12 @@ def launch_command(
resume_from: Path | None = None, resume_from: Path | None = None,
diagnostic_capture: str = "", diagnostic_capture: str = "",
initial_command_file: Path | None = None, initial_command_file: Path | None = None,
reuse_camera_calibration: bool = False,
camera_optical_observations_file: Path | None = None, camera_optical_observations_file: Path | None = None,
) -> list[str]: ) -> list[str]:
"""Build the single launch invocation entirely from the product contract.""" """Build the single launch invocation entirely from the product contract."""
arguments = { arguments = {
"reuse_camera_calibration": str(reuse_camera_calibration).lower(),
"camera_optical_observations_file": str(camera_optical_observations_file or ""), "camera_optical_observations_file": str(camera_optical_observations_file or ""),
"model": config.model, "model": config.model,
"hand_type": config.side, "hand_type": config.side,
@@ -0,0 +1,32 @@
"""Bounded, clock-driven retries of the same motion after a progress timeout."""
from dataclasses import dataclass
@dataclass
class StallRetry:
maximum_failures: int = 3
delay_seconds: float = 3.0
motion_id: str = ''
failures: int = 0
waiting_since: float | None = None
@property
def waiting(self):
return self.waiting_since is not None
def failed(self, motion_id, now):
if self.motion_id != motion_id:
self.motion_id, self.failures = motion_id, 0
self.failures += 1
self.waiting_since = now if self.failures < self.maximum_failures else None
return self.waiting
def remaining(self, now):
return max(0., self.delay_seconds-(now-self.waiting_since)) if self.waiting else 0.
def resume(self, now):
if not self.waiting or self.remaining(now) > 0:
return None
elapsed = now-self.waiting_since
self.waiting_since = None
return elapsed
@@ -192,6 +192,7 @@ def test_ten_unit_tracking_offset_allows_motion_and_stable_capture(phase):
def test_production_o30_requires_fresh_feedback_before_start_and_during_motion(tmp_path): def test_production_o30_requires_fresh_feedback_before_start_and_during_motion(tmp_path):
from runtime_host_fixture import coordinator_fixture from runtime_host_fixture import coordinator_fixture
host, clock = coordinator_fixture(tmp_path, model="o30") host, clock = coordinator_fixture(tmp_path, model="o30")
host.parameters = replace(host.parameters, reuse_camera_calibration=True)
try: try:
for view in host.profile.vision.view_names: for view in host.profile.vision.view_names:
host.cameras.matrices[view] = np.eye(3) host.cameras.matrices[view] = np.eye(3)
@@ -44,3 +44,26 @@ def test_configuration_identity_cannot_replace_optical_observations(optical, err
dataset['captures'][0]['views']['b']['image_stamp_ns'] += 60_000_000 dataset['captures'][0]['views']['b']['image_stamp_ns'] += 60_000_000
with pytest.raises(ValueError, match='optical_'): with pytest.raises(ValueError, match='optical_'):
verify_optical_observations(dataset, extrinsics, matrices, extrinsics_sha256='a'*64) verify_optical_observations(dataset, extrinsics, matrices, extrinsics_sha256='a'*64)
def test_explicit_camera_reuse_checks_prior_quality_without_claiming_new_measurement(tmp_path):
import hashlib
from pathlib import Path
import yaml
from linkerhand_calibration.profiles import load_bundled_hand_profile
from linkerhand_calibration.runtime.optical_verification import accept_existing_camera_calibration
profile = load_bundled_hand_profile('o30_right_18')
source = Path(__file__).resolve().parents[3]/'config/o30_three_camera_extrinsics_20260922_101452.yaml'
path = tmp_path/'cameras.yaml'
path.write_bytes(source.read_bytes())
digest = hashlib.sha256(path.read_bytes()).hexdigest()
report = accept_existing_camera_calibration(profile, path, digest)
assert report['accepted'] is True and report['passed'] is False
assert report['current_optical_verification'] == 'not_performed'
with pytest.raises(ValueError, match='existing_camera_calibration_changed'):
accept_existing_camera_calibration(profile, path, '0'*64)
payload = yaml.safe_load(path.read_text())
payload['quality']['reprojection_rms_px'] = 100.
path.write_text(yaml.safe_dump(payload))
with pytest.raises(ValueError):
accept_existing_camera_calibration(profile, path, hashlib.sha256(path.read_bytes()).hexdigest())
@@ -66,6 +66,30 @@ def test_reordered_checkpoint_cannot_skip_work():
checkpoint.restore(session) checkpoint.restore(session)
def test_reference_uses_raw_installation_pixels_not_moving_pnp_branch():
from copy import deepcopy
from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION
selected = profile()
corners = [[0., 0.], [10., 0.], [10., 10.], [0., 10.]]
pose = dict(rotation_xyzw=[0., 0., 0., 1.], translation_xyz_m=[0., 0., 1.])
reference = dict(profile_id=selected.key.profile_id,
acquisition_policy_version=ACQUISITION_POLICY_VERSION,
protected_hashes={'source_urdf_sha256': 'a'*64},
fixed_corners_by_view={v: corners for v in selected.vision.view_names},
fixed_poses={v: pose for v in selected.vision.view_names},
moving_tag_poses={'front:thumb_mcp': pose},
raw_installation_reference={'front:thumb_mcp': dict(corners_xy=deepcopy(corners),
feedback=list(selected.command.baseline_values))})
current = deepcopy(reference)
current['moving_tag_poses'] = {}
checkpoint = RawCheckpoint({}, reference, (dict(kind='raw_observation_frame',
view='front', tags={'thumb_mcp': {}}),), frozenset())
checkpoint.verify_reference(selected, current)
current['raw_installation_reference']['front:thumb_mcp']['corners_xy'][0][0] += 20.
with pytest.raises(ValueError, match='raw_resume_installation_changed'):
checkpoint.verify_reference(selected, current)
def test_discovery_preserves_interrupted_first_attempt_without_any_passed_unit(tmp_path): def test_discovery_preserves_interrupted_first_attempt_without_any_passed_unit(tmp_path):
import json import json
from linkerhand_calibration.core.domain.capture_plan import CapturePlan from linkerhand_calibration.core.domain.capture_plan import CapturePlan
@@ -0,0 +1,87 @@
"""Exercise actual coordinator command delivery with a stationary motor."""
import json
from dataclasses import replace
import pytest
from runtime_host_fixture import coordinator_fixture, ready
from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
def blocked_host(tmp_path, monkeypatch):
host, clock = coordinator_fixture(tmp_path, model='o30')
ready(host, clock)
host.parameters = replace(host.parameters, reuse_camera_calibration=True)
assert host.start().success
host.execution.session.phase = Phase.SWEEP
start = host.profile.command.baseline_values
target = list(start); target[0] = 255.
task = host.profile.motion.tasks[0]
motion = MotionCommand('sweep', tuple(target), 200., task_key=task.key,
command_index=0, cycle=0, direction='increasing')
host._motion = motion
host.last_command = start
host.segment = MotionExecution(host.profile, motion, initial_command=start,
initial_feedback=start, now=clock.now, identity='test_stalled_segment')
monkeypatch.setattr(host.execution, 'motion', lambda current: motion)
return host, clock, start
def heartbeat(host, clock):
clock.health_receiver(json.dumps(dict(hand_type='right', model='O30', side='RIGHT',
uid='OFFLINE_DEVICE', joint_names=list(host.profile.command.names),
online=True, position_mode=True, active_faults=[], joint_faults={})))
def advance(host, clock, feedback, seconds=.1):
clock.now += seconds
heartbeat(host, clock)
host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns())
host.tick()
def test_two_delayed_resends_then_third_failure_stops(tmp_path, monkeypatch):
host, clock, feedback = blocked_host(tmp_path, monkeypatch)
try:
for _ in range(500):
if host.state == 'PAUSED':
break
waiting = host.stall_retry.waiting and host.stall_retry.remaining(clock.now) > .11
sent = len(clock.positions)
advance(host, clock, feedback)
if waiting:
assert len(clock.positions) == sent
assert host.state == 'PAUSED'
assert 'consecutive_failures=3' in host.reason
rows = [json.loads(line) for line in host.raw_path.read_text().splitlines()]
failures = [r for r in rows if r.get('kind') == 'motion_stall_retry']
resumes = [r for r in rows if r.get('kind') == 'motion_stall_retry_resumed']
assert [r['failure_count'] for r in failures] == [1, 2, 3]
assert [r['attempt'] for r in resumes] == [2, 3]
assert all(3. <= r['waited_seconds'] < 3.11 for r in resumes)
assert all(a['stamp_ns'] < b['stamp_ns'] for a,b in zip(failures,resumes))
finally:
host.close()
@pytest.mark.parametrize('fault', ['feedback_stale', 'duplicate_controller'])
def test_other_protection_still_stops_during_retry_delay(tmp_path, monkeypatch, fault):
host, clock, feedback = blocked_host(tmp_path, monkeypatch)
try:
for _ in range(200):
advance(host, clock, feedback)
if host.stall_retry.waiting:
break
assert host.stall_retry.waiting
if fault == 'feedback_stale':
clock.now += host.profile.acquisition.feedback_stale_seconds+.1
else:
clock.publishers = 2
heartbeat(host, clock)
host.tick()
assert host.state == 'PAUSED'
assert fault in host.reason
assert host.stall_retry.failures == 1
finally:
host.close()