离线拟合
This commit is contained in:
@@ -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 用于上述启动参数。
|
||||||
|
|
||||||
关键文件:
|
关键文件:
|
||||||
|
|||||||
@@ -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",
|
||||||
|
|||||||
+14
-4
@@ -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)
|
||||||
|
|||||||
+5
-1
@@ -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()
|
||||||
Reference in New Issue
Block a user