67 lines
3.5 KiB
Python
67 lines
3.5 KiB
Python
"""Fresh evidence checks that preserve the original zero and installation."""
|
|
|
|
import numpy as np
|
|
|
|
from .joint_resume import reference_pose_failures
|
|
from .motion_execution import feedback_tolerances
|
|
|
|
|
|
LEGACY_REVISIT_POLICY = "staged_revisit_v1"
|
|
REVISIT_POLICY = "staged_revisit_v2"
|
|
|
|
|
|
def revisit_feedback_limits(profile, verification=REVISIT_POLICY):
|
|
"""Separate within-hold stability from between-visit repeatability.
|
|
|
|
O30's operator-reported repeatability is 10 native SDK units. This does
|
|
not change motion settling, image precision, or another hand's limits.
|
|
Historical records retain their original feedback acceptance semantics.
|
|
"""
|
|
if verification not in {LEGACY_REVISIT_POLICY, REVISIT_POLICY}:
|
|
raise ValueError("staged_revisit_policy_unknown")
|
|
stability = feedback_tolerances(profile.command.unit).stability
|
|
repeatability = 2*stability
|
|
if verification == REVISIT_POLICY and profile.key.model.upper() == "O30" and profile.command.unit == "u8":
|
|
repeatability = 10.0
|
|
return dict(unit=profile.command.unit, within_hold_range=stability,
|
|
between_visit_difference=repeatability)
|
|
|
|
|
|
def verify_revisit(profile, previous, current, *, verification=LEGACY_REVISIT_POLICY,
|
|
feedback_limits=None):
|
|
limits = revisit_feedback_limits(profile, verification)
|
|
if ((verification == REVISIT_POLICY and feedback_limits != limits)
|
|
or (verification == LEGACY_REVISIT_POLICY and feedback_limits is not None)):
|
|
raise ValueError("staged_revisit_feedback_policy_changed")
|
|
failures = reference_pose_failures(previous, current)
|
|
identities = lambda ref: {(b.view, b.tag_role, b.motion_evidence_id) for b in ref.branch_references}
|
|
if not previous.branch_references or identities(previous) != identities(current):
|
|
failures.append("installation_evidence_changed")
|
|
for view in {s.view for s in previous.samples}:
|
|
old = [s for s in previous.samples if s.view == view]
|
|
new = [s for s in current.samples if s.view == view]
|
|
if not new or min(s.image_stamp_ns for s in new) <= max(s.image_stamp_ns for s in old):
|
|
failures.append("fresh_images_missing")
|
|
continue
|
|
if not profile.vision_motion:
|
|
before, after = np.asarray([s.feedback for s in old]), np.asarray([s.feedback for s in new])
|
|
if (before.shape != (len(old), profile.command.command_count)
|
|
or after.shape != (len(new), profile.command.command_count)
|
|
or not np.all(np.isfinite(before)) or not np.all(np.isfinite(after))):
|
|
failures.append("feedback_missing")
|
|
else:
|
|
if (np.any(np.ptp(after, axis=0) > limits["within_hold_range"])
|
|
or np.any(np.abs(after-np.median(before, axis=0)) > limits["between_visit_difference"])):
|
|
failures.append("held_feedback_changed")
|
|
if failures:
|
|
raise ValueError(f"revisit_reference_changed:{previous.joint}:{','.join(failures)}")
|
|
|
|
|
|
def revisit_record(profile, previous, current, cycle):
|
|
limits = revisit_feedback_limits(profile)
|
|
verify_revisit(profile, previous, current, verification=REVISIT_POLICY, feedback_limits=limits)
|
|
return {"kind": "joint_zero_motion_reference_verified", "verification": REVISIT_POLICY,
|
|
"feedback_limits": limits,
|
|
"joint": previous.joint, "task_name": previous.task_key, "cycle": cycle,
|
|
"current_reference": current.as_record(), "preserved_reference": previous.as_record()}
|