admin/linkerhand_retarget_ros2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

重构O6验证过

Browse Source
This commit is contained in:
lxp
2026-09-16 14:05:08 +08:00
parent 467651fc47
commit 7a04780b52
268 changed files with 26640 additions and 789 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+7
View File
@@ -63,6 +63,11 @@ Thumbs.db
/profiles/
/calibration_output/
/config/*_three_camera_extrinsics.yaml
# Superseded local O6 camera calibrations. Keep the active extrinsics and
# intrinsics referenced by o6_right_product.yaml available for version control.
/config/o6_three_camera_extrinsics_20260915.yaml
/config/o6_three_camera_extrinsics_intrinsics_20260915.yaml
/config/o6_three_camera_extrinsics_recalibrated.yaml
*.wear_check.json
*.checkpoint.json
*.verification.json
@@ -73,6 +78,8 @@ Thumbs.db
/src/linkerhand_calibration/urdf/*/*_calibrated_*.urdf
/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
/src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf
# Unreferenced local URDF experiment; never use as a protected CAD input.
/src/linkerhand_calibration/urdf/o6_right/linkerhand_o6_right111.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
AGENTS.md
+2 -1
View File
@@ -1 +1,2 @@
避免产生屎山代码，要求代码结构清晰可读性好
1.不要补丁式修复问题，避免产生屎山代码，要求代码结构清晰可读性好。
2.永远用中文回复。
config/o6_camera_intrinsics_20260915/hikrobot_DB2163739.yaml
+55
View File
@@ -0,0 +1,55 @@
image_width: 1624
image_height: 1240
camera_name: hikrobot_top_DB2163739
camera_matrix:
rows: 3
cols: 3
data:
- 3608.7915725635876
- 0.0
- 814.529414176388
- 0.0
- 3602.9474866035775
- 596.7866652774017
- 0.0
- 0.0
- 1.0
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data:
- -0.05774549441026762
- -0.5653814259405426
- -0.0034831870836035525
- -0.0007572304719307008
- 0.0
rectification_matrix:
rows: 3
cols: 3
data:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
projection_matrix:
rows: 3
cols: 4
data:
- 3590.9833984375
- 0.0
- 813.6314052451635
- 0.0
- 0.0
- 3591.4892578125
- 595.0547132430074
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
config/o6_camera_intrinsics_20260915/hikrobot_DB2163742.yaml
+55
View File
@@ -0,0 +1,55 @@
image_width: 1624
image_height: 1240
camera_name: hikrobot_front_DB2163742
camera_matrix:
rows: 3
cols: 3
data:
- 3520.83963745858
- 0.0
- 971.8024440668337
- 0.0
- 3503.4665942352735
- 636.2092891200001
- 0.0
- 0.0
- 1.0
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data:
- -0.06456607194260039
- -0.3209716179987898
- 0.0016533290649511723
- 0.014394085090833234
- 0.0
rectification_matrix:
rows: 3
cols: 3
data:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
projection_matrix:
rows: 3
cols: 4
data:
- 3485.41357421875
- 0.0
- 980.9585478080553
- 0.0
- 0.0
- 3500.062255859375
- 636.4474700149367
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
config/o6_camera_intrinsics_20260915/hikrobot_DB2163749.yaml
+55
View File
@@ -0,0 +1,55 @@
image_width: 1624
image_height: 1240
camera_name: hikrobot_side_DB2163749
camera_matrix:
rows: 3
cols: 3
data:
- 3564.4557723541934
- 0.0
- 633.5422135270659
- 0.0
- 3549.8257661608695
- 614.1194434338138
- 0.0
- 0.0
- 1.0
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data:
- -0.023159717917942222
- -0.6611401590594669
- 0.0010272016326466154
- -0.014523561130335804
- 0.0
rectification_matrix:
rows: 3
cols: 3
data:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
projection_matrix:
rows: 3
cols: 4
data:
- 3531.000244140625
- 0.0
- 623.4124604711214
- 0.0
- 0.0
- 3551.248291015625
- 614.0312247691472
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
config/o6_three_camera_extrinsics_20260915_192517.yaml
+60
View File
@@ -0,0 +1,60 @@
schema_version: 1
reference_view: front
cameras:
front:
serial_number: DB2163742
width: 1624
height: 1240
intrinsics_sha256: 3cdecb8fcede41561019882a48a342d7265a44855cf0282ec96ec7f69a9b1ee5
side:
serial_number: DB2163749
width: 1624
height: 1240
intrinsics_sha256: dbe0cdc73460dc1e51c0e3967395d14b4415a71cee952597e0717ad0babd3449
top:
serial_number: DB2163739
width: 1624
height: 1240
intrinsics_sha256: 315d5cd6f28964ad57ff67088222fff3d9679cce076acc3f05a22b76038e5352
front_from_view:
front:
translation_xyz_m:
- 0.0
- 0.0
- 0.0
quaternion_xyzw:
- 0.0
- 0.0
- 0.0
- 1.0
side:
translation_xyz_m:
- -0.8459732881345877
- -0.000555404488719817
- 0.9729938257334647
quaternion_xyzw:
- -0.04404064417946292
- 0.6703485002266404
- -0.01659275028535042
- 0.7405524900654377
top:
translation_xyz_m:
- -0.10806018719194868
- -0.5842672050520756
- 1.0419844264616385
quaternion_xyzw:
- 0.7284460604085318
- -0.016210595429770967
- 0.019511507802845565
- -0.6846333724953536
quality:
passed: true
reprojection_rms_px: 0.46496404895142424
maximum_rotation_repeatability_deg: 0.019287272875226844
maximum_translation_repeatability_m: 0.0002246640979668814
front_side_captures: 15
front_top_captures: 15
front_side_candidates: 15
front_top_candidates: 15
front_side_rejected: 0
front_top_rejected: 0
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py
+10
View File
@@ -7,6 +7,7 @@ from enum import Enum
from utils.open_can import OpenCan
from can.exceptions import CanError
from utils.color_msg import ColorMsg
from utils.feedback import ReceivedPositionFrames
current_dir = os.path.dirname(os.path.abspath(__file__))
target_dir = os.path.abspath(os.path.join(current_dir, ".."))
sys.path.append(target_dir)
@@ -178,6 +179,10 @@ class LinkerHandG20Can:
# 串联控制数据存储
self.x41, self.x42, self.x43, self.x44, self.x45 = [], [], [], [], []
frame_layout = [[(frame, index) for index in range(6)] for frame in range(0x41, 0x46)]
layout = self.joint_state_to_cmd_state(frame_layout)
self.position_feedback = ReceivedPositionFrames(
{frame: 6 for frame in range(0x41, 0x46)}, [item if item != 0 else None for item in layout])
self.x49, self.x4A, self.x4B, self.x4C, self.x4D = [0] * 6, [0] * 6, [0] * 6, [0] * 6, [0] * 6
self.x51, self.x52, self.x53, self.x54, self.x55 = [], [], [], [], []
self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], []
@@ -354,6 +359,8 @@ class LinkerHandG20Can:
response_data = msg.data[1:]
if len(list(response_data)) == 0:
return
if frame_type in range(0x41, 0x46):
self.position_feedback.accept(frame_type, response_data, msg.timestamp)
# 并联控制指令响应
if frame_type == 0x01: self.x01 = list(response_data)
elif frame_type == 0x02: self.x02 = list(response_data)
@@ -1040,6 +1047,9 @@ class LinkerHandG20Can:
return None
return self.joint_state_to_cmd_state(state=state)
def get_feedback_snapshot(self):
return self.position_feedback.snapshot()
def get_current_pub_status(self):
"""API接口:获取手指当前状态"""
self.get_current_status()
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py
+6
View File
@@ -6,6 +6,7 @@ import threading
import numpy as np
from utils.open_can import OpenCan
from utils.color_msg import ColorMsg
from utils.feedback import ReceivedPositionFrames
from can.exceptions import CanError
@@ -17,6 +18,7 @@ class LinkerHandL6Can:
self.open_can = OpenCan(load_yaml=yaml)
self.x01 = [0] * 6 # 关节位置
self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
self.x02 = [-1] * 6 # 转矩限制
self.x05 = [0] * 6 # 速度
self.x07 = [-1] * 6 # 加速度
@@ -233,6 +235,7 @@ class LinkerHandL6Can:
break
if not is_position_echo:
self.x01 = list(response)
self.position_feedback.accept(frame_type, response, msg.timestamp)
elif frame_type == 0x02: # 0x02
self.x02 = list(response_data)
elif frame_type == 0x05: # Set speed
@@ -326,6 +329,9 @@ class LinkerHandL6Can:
def get_current_pub_status(self):
return self.x01
def get_feedback_snapshot(self):
return self.position_feedback.snapshot()
def get_speed(self):
#self.send_frame(0x05, [],sleep=0.003)
#print("L6暂不支持读取实时速度")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py
+6
View File
@@ -4,6 +4,7 @@ import threading
import numpy as np
from utils.open_can import OpenCan
from utils.color_msg import ColorMsg
from utils.feedback import ReceivedPositionFrames
from can.exceptions import CanError
@@ -15,6 +16,7 @@ class LinkerHandO6Can:
self.open_can = OpenCan(load_yaml=yaml)
self.x01 = [0] * 6 # 关节位置
self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
self.x02 = [-1] * 6 # 转矩限制
self.x05 = [0] * 6 # 速度
self.x07 = [-1] * 6 # 加速度
@@ -224,6 +226,7 @@ class LinkerHandO6Can:
return
if frame_type == 0x01: # 0x01
self.x01 = list(response_data)
self.position_feedback.accept(frame_type, response_data, msg.timestamp)
elif frame_type == 0x02: # 0x02
self.x02 = list(response_data)
elif frame_type == 0x05: # Set speed
@@ -317,6 +320,9 @@ class LinkerHandO6Can:
def get_current_pub_status(self):
return self.x01
def get_feedback_snapshot(self):
return self.position_feedback.snapshot()
def get_speed(self):
self.send_frame(0x05, [],sleep=0.002)
#print("L6暂不支持读取实时速度")
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py
+8
View File
@@ -207,6 +207,14 @@ class LinkerHandApi:
getter = getattr(self.hand, "get_cached_current_status", None)
return getter() if getter is not None else None
@property
def supports_feedback_timestamps(self):
return callable(getattr(self.hand, "get_feedback_snapshot", None))
def get_feedback_snapshot(self):
getter = getattr(self.hand, "get_feedback_snapshot", None)
return getter() if getter is not None else None
def get_state_for_pub(self):
return self.hand.get_current_pub_status()
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/feedback.py
+65
View File
@@ -0,0 +1,65 @@
"""Atomic CAN position snapshots with the original receive timestamps."""
from dataclasses import dataclass
import math
from threading import Lock
@dataclass(frozen=True)
class FeedbackSnapshot:
positions: tuple
channel_stamps_ns: tuple[int, ...]
@property
def stamp_ns(self):
return max(self.channel_stamps_ns)
def as_dict(self, names):
return {"schema": "can_feedback_v1", "name": list(names),
"position": list(self.positions), "channel_stamps_ns": list(self.channel_stamps_ns),
"stamp_ns": self.stamp_ns}
class ReceivedPositionFrames:
"""Keep receipt identity even if stationary feedback has identical values.
``layout`` lists the frame and value index for each public SDK channel.
None represents a protocol-reserved constant-zero slot, whose freshness
is bounded by the oldest actual frame. No polling/publication clock is
allowed to update a snapshot's timestamps.
"""
def __init__(self, sizes, layout):
self.sizes, self.layout = dict(sizes), tuple(layout)
self._frames = {}
self._lock = Lock()
def accept(self, frame, values, timestamp):
if frame not in self.sizes or len(values) != self.sizes[frame]:
return False
if not isinstance(timestamp, (int, float)) or not math.isfinite(timestamp) or timestamp <= 0:
return False
stamp = int(round(timestamp * 1_000_000_000))
with self._lock:
previous = self._frames.get(frame)
if previous is not None and stamp <= previous[0]:
return False
self._frames[frame] = (stamp, tuple(values))
return True
def snapshot(self):
with self._lock:
if self._frames.keys() != self.sizes.keys():
return None
oldest = min(value[0] for value in self._frames.values())
positions, stamps = [], []
for item in self.layout:
if item is None:
positions.append(0)
stamps.append(oldest)
else:
frame, index = item
stamp, values = self._frames[frame]
positions.append(values[index])
stamps.append(stamp)
return FeedbackSnapshot(tuple(positions), tuple(stamps))
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
+31 -8
View File
@@ -168,6 +168,8 @@ class LinkerHand(Node):
self.last_position_command_time = None
self.last_hand_state = [-1] * 10
self.last_hand_state_stamp_ns = None
self._last_timed_feedback_stamps = None
self.last_hand_vel = [-1] * 10
self.force = [[-1] * 5] * 4
self.matrix_dic = {
@@ -245,6 +247,7 @@ class LinkerHand(Node):
COMMAND_QOS,
)
self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10)
self.timed_state_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_state_timed'), 30)
self.hand_info_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_info'), 10)
if self.is_touch == True:
if self.modbus != "None":
@@ -391,7 +394,7 @@ class LinkerHand(Node):
self.last_hand_vel_cmd = None
def _poll_state_if_due(self):
if self.hand_state_pub.get_subscription_count() < 1:
if self.hand_state_pub.get_subscription_count() < 1 and self.timed_state_pub.get_subscription_count() < 1:
return
now = time.monotonic()
if state_reads_deferred(
@@ -400,8 +403,7 @@ class LinkerHand(Node):
self.command_quiet_period,
self.defer_state_reads_while_commanding,
):
# pub_state continues to publish the last completed state as a
# heartbeat. A fresh blocking read is made after motion settles.
# A repeated heartbeat retains its original measurement time.
return
if not state_poll_due(
self.last_state_poll_time, now, self.state_poll_period
@@ -411,6 +413,7 @@ class LinkerHand(Node):
# another read on the following timer callback.
self.last_state_poll_time = now
self.last_hand_state = self.api.get_state()
self.last_hand_state_stamp_ns = self.get_clock().now().nanoseconds
time.sleep(0.003)
if state_poll_due(
self.last_velocity_poll_time, now, self.velocity_poll_period
@@ -478,9 +481,7 @@ class LinkerHand(Node):
def pub_state(self):
while True:
if self.hand_state_pub.get_subscription_count() > 0:
msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
self.hand_state_pub.publish(msg)
self._publish_feedback()
if self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
msg = Float32MultiArray()
msg.data = [float(val) for sublist in self.force for val in sublist]
@@ -498,6 +499,26 @@ class LinkerHand(Node):
self.hand_info_pub.publish(msg)
time.sleep(self.hz)
def _publish_feedback(self):
"""Publish one atomic measured state; never give cached values a new time."""
if self.api.supports_feedback_timestamps:
snapshot = self.api.get_feedback_snapshot()
if snapshot is None:
return
positions, stamp = snapshot.positions, snapshot.stamp_ns
if (snapshot.channel_stamps_ns != self._last_timed_feedback_stamps
and self.timed_state_pub.get_subscription_count() > 0):
message = String()
message.data = json.dumps(snapshot.as_dict(self.api.get_finger_order()))
self.timed_state_pub.publish(message)
self._last_timed_feedback_stamps = snapshot.channel_stamps_ns
else:
positions, stamp = self.last_hand_state, self.last_hand_state_stamp_ns
if stamp is None:
return
if self.hand_state_pub.get_subscription_count() > 0:
self.hand_state_pub.publish(self.joint_state_msg(positions, self.last_hand_vel, stamp_ns=stamp))
def pub_matrix_mass(self, dic):
"""发布矩阵数据合值 单位g 克 JSON格式"""
msg = String()
@@ -556,10 +577,12 @@ class LinkerHand(Node):
msg.data = json.dumps(self.matrix_dic)
self.matrix_touch_pub.publish(msg)
def joint_state_msg(self, pose,vel=[]):
def joint_state_msg(self, pose, vel=(), *, stamp_ns=None):
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = self.get_clock().now().to_msg()
if stamp_ns is None:
stamp_ns = self.get_clock().now().nanoseconds
joint_state.header.stamp.sec, joint_state.header.stamp.nanosec = divmod(int(stamp_ns), 1_000_000_000)
joint_state.name = self.api.get_finger_order()
joint_state.position = [float(x) for x in pose]
if len(vel) > 1:
src/linker_hand_ros2_sdk/test/test_feedback_timestamps.py
+73
View File
@@ -0,0 +1,73 @@
"""A CAN receipt, rather than a heartbeat, defines position sample identity."""
import json
from types import SimpleNamespace
from linker_hand_ros2_sdk.LinkerHand.utils.feedback import ReceivedPositionFrames
from linker_hand_ros2_sdk.linker_hand import LinkerHand
def test_cached_unchanged_and_out_of_order_frames_keep_measurement_identity():
frames = ReceivedPositionFrames({1: 2}, [(1, 0), (1, 1)])
assert frames.snapshot() is None
assert not frames.accept(1, [4], 1.)
assert not frames.accept(1, [4, 5], 0.)
assert frames.accept(1, [4, 5], 1.)
first = frames.snapshot()
assert first == frames.snapshot()
assert not frames.accept(1, [8, 9], .9)
assert not frames.accept(1, [8, 9], 1.)
assert frames.snapshot() == first
assert frames.accept(1, [4, 5], 2.) # A new stationary observation is real.
assert frames.snapshot().positions == first.positions
assert frames.snapshot().stamp_ns == 2_000_000_000
assert first.stamp_ns == 1_000_000_000
def test_split_finger_snapshot_does_not_retimestamp_held_channels():
frames = ReceivedPositionFrames({1: 2, 2: 2}, [(2, 1), (1, 0), None])
frames.accept(1, [10, 11], 1.)
assert frames.snapshot() is None
frames.accept(2, [20, 21], 1.02)
first = frames.snapshot()
assert first.positions == (21, 10, 0)
assert first.channel_stamps_ns == (1_020_000_000, 1_000_000_000, 1_000_000_000)
frames.accept(2, [20, 22], 1.04)
assert frames.snapshot().channel_stamps_ns == (1_040_000_000, 1_000_000_000, 1_000_000_000)
def test_sdk_republication_cannot_refresh_cached_state_timestamps():
frames = ReceivedPositionFrames({1: 2}, [(1, 0), (1, 1)])
joint_messages, timed_messages = [], []
publisher = lambda messages: SimpleNamespace(get_subscription_count=lambda: 1, publish=messages.append)
host = SimpleNamespace(api=SimpleNamespace(supports_feedback_timestamps=True,
get_feedback_snapshot=frames.snapshot, get_finger_order=lambda: ['a', 'b']),
_last_timed_feedback_stamps=None, last_hand_vel=[], hand_state_pub=publisher(joint_messages),
timed_state_pub=publisher(timed_messages))
host.joint_state_msg = lambda *args, **kwargs: LinkerHand.joint_state_msg(host, *args, **kwargs)
LinkerHand._publish_feedback(host)
assert not joint_messages and not timed_messages
frames.accept(1, [10, 20], 100.)
LinkerHand._publish_feedback(host)
LinkerHand._publish_feedback(host)
assert len(joint_messages) == 2 and len(timed_messages) == 1
assert all(message.header.stamp.sec == 100 for message in joint_messages)
assert json.loads(timed_messages[0].data)['channel_stamps_ns'] == [100_000_000_000]*2
frames.accept(1, [10, 20], 101.)
LinkerHand._publish_feedback(host)
assert len(timed_messages) == 2 and joint_messages[-1].header.stamp.sec == 101
def test_g20_timed_layout_matches_the_public_sdk_order():
from linker_hand_ros2_sdk.LinkerHand.core.can.linker_hand_g20_can import LinkerHandG20Can
hand = LinkerHandG20Can.__new__(LinkerHandG20Can)
layout = hand.joint_state_to_cmd_state([[(frame, index) for index in range(6)] for frame in range(0x41, 0x46)])
frames = ReceivedPositionFrames({frame: 6 for frame in range(0x41, 0x46)}, [item or None for item in layout])
raw = []
for finger, frame in enumerate(range(0x41, 0x46)):
raw.append([finger*10+i for i in range(6)])
frames.accept(frame, raw[-1], 100.+finger*.01)
snapshot = frames.snapshot()
assert snapshot.positions == tuple(hand.joint_state_to_cmd_state(raw))
assert snapshot.channel_stamps_ns[0] == 100_000_000_000
assert snapshot.channel_stamps_ns[4] == 100_040_000_000
src/linker_hand_ros2_sdk/test/test_l6_channel_order.py
+5 -2
View File
@@ -46,12 +46,14 @@ def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
sys.path.insert(0, str(linker_hand_root))
try:
from core.can.linker_hand_l6_can import LinkerHandL6Can
from utils.feedback import ReceivedPositionFrames
finally:
sys.path.remove(str(linker_hand_root))
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
hand.can_id = 0x27
hand.x01 = [10, 20, 30, 40, 50, 60]
hand.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
hand._position_echo_lock = threading.Lock()
command = (255, 2, 253, 253, 253, 253)
hand._pending_position_echoes = deque(
@@ -60,13 +62,14 @@ def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
hand._position_echo_timeout_seconds = 0.02
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)))
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)), timestamp=100.)
)
assert hand.x01 == [10, 20, 30, 40, 50, 60]
assert not hand._pending_position_echoes
measured = (250, 3, 252, 252, 252, 252)
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)))
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)), timestamp=101.)
)
assert hand.x01 == list(measured)
assert hand.get_feedback_snapshot().channel_stamps_ns == (101_000_000_000,) * 6
src/linkerhand_calibration/CALIBRATION_PIPELINE_REVIEW.md
+937
View File
@@ -0,0 +1,937 @@
# 四轮采集与图像拟合的修改前评估
日期：2026-09-16。目标：稳定完成采集，从可信标定 JSON 重建修正 URDF。
## 当前结论（2026-09-16）
当前 O6 按重新实测的 16.5 mm Tag 完成新采集，完整数据经正式离线流程验收通过，
已生成合格 JSON、修正 URDF 和发布清单。本次在线收尾发生系统内存耗尽，不能把离线
成功表述为在线全过程无中断。已修正收尾读取原始日志时的整文件副本开销，独立验证见后文。
没有增加 O6 专用容错分支或降低物理精度门限，也不据此宣称其他型号实机均已通过。
| 环节 | 当前证据与结论 |
| --- | --- |
| 四轮连续采集与必要稳态点 | 新会话 `20260916_105528` 完成 36/36，1 个小指方向自动重扫后通过；采集约 6 分 47 秒。随后收尾时发生 OOM，反馈超时暂停；完整数据已用于正式离线验收。此前两次 16 mm 会话也完成采集，但空间验收失败。 |
| 反馈时间 | 使用真实 CAN 接收时间；相同缓存不得伪装成新测量。逐通道时间保留至图像插值。 |
| 相机时间 | 三台实际设备的曝光事件与帧计数器吻合；设备锁存换算曝光中点，替换约晚 16.5 ms 的取帧时间。 |
| 调度 | ROS 控制/反馈顺序处理，图像独立有界队列；实机对照消除了共享锁造成的反馈排队。 |
| 映射输入 | 当前报告的分段线性反馈曲线保留曝光时刻的小数输入；实际字节命令与旧整数表保持量化。 |
| 图像证据 | 同一运动保存全部机位的原始角点，不依赖任务主机位或 PnP；最新会话保存 16,822 个图像记录。没有增加运动或停点。 |
| 图像验收 | 独立三维测量对原始角点仍须 ≤1.5 px；最终文件按原角度/位置门限检查，额外视角只提供物理容差内的图像相容性证据。已知真值反例和反向拒绝测试通过。 |
| 平行转轴零位 | 已修正参考点沿轴移动导致零位变化的公共公式；四型号源 URDF 的已知真值及任意轴上参考点检查通过。数学规则不再由型号开关选择。 |
| JSON → URDF | `20260916_105528/offline_165mm` 已生成通过验收的文件与 manifest；另外调用重建 CLI，仅从保存 JSON 和源 URDF 得到的文件与发布 URDF 逐字节相同。 |
| 整链空间精度 | 新 16.5 mm 会话已通过原门限。主观测运动/稳态独立验收中，各角色最差角度 P95 为 1.873°，位置 P95 为 2.122 mm；最终文件空间与图像检查通过。精度适用范围是观测关节、扫描方向与固定的其他关节姿态；未独立观测的关节仍按配置迁移。 |
| 尺度输入核验 | 用户实测更正为 **16.5×16.5 mm** Tag、**27×27 mm** 棋盘，与独立图像检查基本吻合。当前 O6 的 Profile、检测器和标定节点尺寸已同步并记录新指纹；未修改相机参数，未将旧 16 mm 数据冒充新尺寸采集。 |
先前的分机位空间不一致已在修正实测尺寸后的新数据上通过验收，未靠增加拟合参数
或放宽门限达成。当前稳定性问题集中在已确认的收尾内存峰值；保留完整采集数据直接
验证收尾修复，不为重复计算重新运动机械手。其余型号仍需对应硬件实测，O6 当前 Tag
布局中的其他三指曲线迁移不等于各关节独立标定。
上述两次未验收候选的三个主动零位差均小于 0.03°，说明当前分段求解可重复，
不能据此证明其绝对空间精度。正式发布仍必须通过整链与最终文件验收。
以下按发生顺序保留评估依据；早期 CAN 未连接等现场状态已由上述新实采结果更新。
## 已核实的问题
1. 当前生产采集要求准备运动先确定唯一 PnP/铰链姿态；不唯一时整场暂停，后续三轮训练数据尚未获得。
2. 准备阶段的自由铰链几何与最终保留骨长、轴向的 URDF 使用不同参数空间。
3. 后续姿态已受准备模型约束，不能把其重复性直接解释为原始图像上的整链正确性。
4. JSON 保存后读取并重建 URDF 已有独立字节一致性验证，应保留这条文件链。
## 修改前的离线实验
没有改动生产算法、现场硬件、原始采样或原始 URDF。以 `20260915_150015` 保存的原始
角点进行诊断：前三轮抽取 480 个 Tag 观测，第四轮 160 个观测仅用于冻结后的检查。
直接按源 URDF 的运动链拟合一个共同基座、三个允许修改的零位和每个 Tag 的固定安装；
另一个实验允许带固定基准的双向曲线修正。两个实验均使用记录时的相机参数。
- 固定曲线：39 个参数，32 次迭代收敛，约 0.60 秒。
- 联合曲线：119 个参数，23 次迭代收敛，约 1.90 秒，曲线最大修正约 1.38°。
- 带 0.05 px 噪声的已知真值实验，三个初值均收敛，三个零位最大误差约 0.023°。
- 实采第四轮仍有不合格残差；小指末节最大约 4.15 px。上述实验不是实机通过证据。
- 使用后来核验的相机参数进行的私有反事实诊断也没有消除全部残差，不能将旧数据改挂新参数发布。
脚本、参数、雅可比和报告位于
`calibration_output/O6_RIGHT_001/20260915_150015/review/architecture_assessment/`。
原型只证明统一参数空间可以计算、在所测合成条件下可恢复；没有证明全型号实机精度。
## 采用的边界
- 采集保存原始角点、相机身份、SDK 指令/反馈、方向、轮次与基准图像；固定基准漂移、
SDK 故障、时间同步和必要采样覆盖检查仍然有效。
- 稳定基准图像先冻结，三维解释后求解；未求解的图像不得标成已测准的角度或零位。
- 训练只使用前三轮；第四轮与独立稳态验证不参与几何、安装、曲线或候选的选择。
- 图像拟合使用与输出 URDF 一致的拓扑和可修改参数；不自动改变骨长、转轴、相机参数，
不新增 CAD 零位假设。显式复制关节继续保留来源声明。
- 像素残差小不是角度/位置精度合格的充分条件。原物理门限、参数可确定性和最终文件读取验证需保留。
- 模型尚未通过回归与实测验收前，不凭局部收敛结果替换正式发布逻辑，不降低门限以生成 PASS。
- 采集完成后的拟合失败应保留完整数据并支持离线重算，区别于运动中暂停。
## 第二轮评估：不采用未经验证的整体替换
对同一会话进一步测试直接从原始角点初始化、共同拟合 33 节点双向单调曲线的版本。
共 359 个参数、6160 个训练残差，150 次迭代、43.39 秒后仍未收敛；拇指末节
第四轮最大误差约 45.18 px，小指末节约 7.77 px。这个结果说明，前一个实验依赖
既有拟合初值的局部收敛，不足以证明新流程具备独立初始化和实机鲁棒性。
因此该原型**未接入生产**，相关采集/基准/拟合草稿已移出程序源码，连同源代码快照保存到
上述评估目录的 `unadopted_prototype/` 和 `prototype_source_snapshot.tar.gz`。
`monotonic_urdf_image_replay.json` 保留失败数据。下一步不能通过增加迭代次数、减小残差权重
或放宽门限将其直接上线。
经过四型号 FK 对照测试的投影基础 `core/fitting/urdf_image.py` 和对应测试也归档到
`unadopted_prototype/`，便于复查。生产源码不保留未接入的备用算法。
## 本次实际接入的流程修改
1. 四轮及稳态采集完成、数据同步落盘后，显式关闭图像采集，撤销尚未返回的图像回调。
拟合使用已经保存的图像与相机参数；此后相机消息/Tag 位置变化不再改变这份输入或触发
采集中暂停。运动采集期间的固定基准漂移检查保持生效。
2. 反馈接收、SDK 故障保护和用户中止在后台拟合期间持续生效。
3. 数值拟合、文件验收和后台阶段协议错误统一结束为 `FAILED`，保存失败阶段、原始数据路径、
完成/计划采集单元数和异常堆栈；不再写成采集中 `PAUSED`。
4. 状态输出始终提供原始数据路径，拟合失败提供诊断路径，提示先离线复算。
失败结果不会填写已通过的 JSON/URDF 路径，也不会被迟到观测或清理用中止覆盖。
5. 修复反馈曲线整链回放中丢失非活动关节到达方向的问题。比如下游关节正在弯曲，
上游关节保持不动时，上游仍应使用其到达方向对应的回差曲线，不能自动改用双向均值。
反馈回放和稳态指令回放现在使用同一个方向解析函数；记录了不完整或非法方向时明确拒绝。
独立两轴合成数据证明：旧处理会使固定 Tag 看起来有超过 4° 的安装误差，修复后
训练和第四轮最终 URDF 回放恢复真值。当前旧 O6 数据的对应影响小于 0.03°，不是
该次实采大误差的主要原因。数值见 `held_joint_direction_effect.json`。
6. 用户确认 O6 顶部 ID7 与正面 ID2 固定在同一末节后，明确设置其安装连杆为
`rh_thumb_distal`，不再从侧摆任务名称推断为侧摆根部连杆。Profile 哈希同步更新；
旧会话的配置身份保持原样。独立合成场景中的 O6 安装真值同步修正，其他型号不变。
单独侧摆且弯曲角固定时，错误连杆可以被固定安装矩阵吸收，因此不能声称该错误
一定解释先前全部残差；必须在正确绑定及完整关节状态下继续验证。
这组流程修改解决采集完成后继续受实时视觉影响和失败状态误导的问题；准备段的几何
不一致另按下面的评估处理。保持原文件生成与发布验收链路。
## 第三轮评估：使用输出 URDF 已保留的几何约束
源 URDF 中相邻平行转轴的平行关系及垂直轴距，在任意关节零位修正下均不变。
旧准备模型却允许这些量自由变化，求出的几何无法由最终保留骨长和轴向的 URDF 表达。
因此只对真实 Tag 安装连杆、观测拓扑和源 URDF 均满足条件的相邻平行轴，消去两个
自由轴倾角和一个自由轴距参数。相机、Tag 安装与逐帧角度仍由图像估计；不新增
CAD 零位约定、不使用 SDK 角度作为拟合先验、不引入额外惩罚权重。
适用性由共同编译器判断，没有 O6 专用分支；不满足条件时保留原图像模型。
冻结模型记录新的策略版本、约束及源 URDF 哈希，最终处理前从源文件重新推导验证。
旧模型的序列化字节结构保持不变，避免破坏旧证据哈希。内存记录与磁盘记录均保留会话头。
断点策略升级为 `confirmed_branch_v6_source_geometry`；旧自由几何会话仍可离线读取，
不能作为新约束流程的断点混入新采集。
对 `20260915_160040` 保存的小指准备段 104 帧原始角点：
- 原自由模型返回 `image_motion_families_not_distinguishable`。
- 使用源平行关系与 36.9995936 mm 轴距后，四个候选均在原 150 次迭代上限内收敛。
- 最优两个候选收敛到相同完整 Tag 姿态，其他候选仍用原统计检验排除。
- 最优候选留出帧最大误差约 0.696 px，低于原 1.5 px 门限；正式求解器约 4.69 秒返回成功。
- 原像素、角度/轴心不确定性、候选等价性及显著性门限均未放宽。
独立合成图像验证同向及反向平行轴的真值恢复；四型号源几何验证约束在独立零位变化下
仍成立。这证明该准备段可以使用与输出一致的模型解算，**尚不等于新四轮整手实机通过**。
结果保存在 `20260915_160040/review/cad_geometry_resolution.json` 及相邻诊断文件。
随后经实际 `BranchInitialization` 入口回放该会话三个准备段的原始图像，拇指侧摆
98 帧、拇指弯曲 88 帧、小指 104 帧均通过，耗时约 0.45 / 3.78 / 4.94 秒。
该回放使用用户已确认的当前 Tag 绑定，仅保存诊断；没有改写旧会话或发布新标定。
见 `all_preparations_source_geometry.json` 和同目录可复现脚本。
## 验证顺序
1. 受约束图像模型的已知真值、多初值、旧证据兼容和错误几何反例。
2. 四型号共同采集逻辑与数据身份；反馈/固定基准保护及中止持续生效。
3. 前三轮训练身份与第四轮隔离；完整性、重复图像、篡改与旧会话身份检查。
4. JSON 落盘后独立重建 URDF、标准 ROS 加载及最终文件验收。
5. O6 当前相机配置下重新实采；只有通过实际验收才更新发布结果。
## 本轮验证结果及实机阻塞
- 最终几何、真实图像入口、采集边界、来源身份、JSON 独立重建回归：87 项通过（39.81 秒）。
- 后台进程、诊断采集、四型号产物与 launch 相关集成检查：80 项通过（157.08 秒）。
- 新旧断点策略、恢复与原始图像凭据检查：92 项通过（4.85 秒）。
各组有交叉，不将数量相加作为独立测试总数。
- 已知真值覆盖两/三关节链和正/反向平行轴；故意把 37 mm 轴距改为 80 mm 时，
求解器因原重投影门限不合格而拒绝，不能仅凭源几何声明强行通过。
- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。
- 用户确认现场为 O6 后，清理了仍订阅同一控制话题的 O30 残留 SDK/GUI。
实际执行新的 `calibrate_hand --no-resume` 命令时，启动检查报告 `CAN interface does not exist: can0`。
系统仅有 lo/eno1/wlo1；采集未开始、未发送电机指令、没有发布本轮新标定。
需恢复 O6 原通信适配器后继续四轮实测；当前不能宣称整手实机精度或全部型号实机通过。
启动报告保存在上述 review 目录的 `o6_cad_geometry_startup.json` 和日志中。
## 第四轮评估：有限恢复、精简停点与最终原始角点验收
本次优化以保留四轮连续扫描、独立验证集和原精度门限为前提。没有采用直接用运动中
角度代替静态指令映射的办法，因为反馈滞后和方向回差会使两种角度不同。
### 准备段恢复
`runtime/zero_recovery.py` 为每组尚未冻结零位的关节提供一次局部恢复额度。
图像数量不足或候选暂时不确定时，复用该组原归零路径补采；几何已确认而零位图像
不足时，仅保持当前姿态重新打开图像窗口。恢复撤销旧观测回调，保留真实日志；
不替换已冻结几何/零位、不改变保持条件，不将设备故障当成视觉缺样处理。
两种恢复共享一次额度，持续失败仍暂停，不无限重做整场四轮运动。
### 训练停点精简
`core/fitting/command_sampling.py` 只读取前三轮连续数据，在原静态训练网格上求最少
保留节点，使每关节、每方向在原网格节点的插值变化不超过 0.25°。端点、baseline、
中间支撑点和显式非线性补充节点必须保留；训练不足或单调修正过大时保留完整网格。
该准则决定采样位置，不证明实际静态误差已经达标。每个保留点仍需采集新稳态图像；
原独立稳态验证点全部保留，只有这些验证与最终产物验收通过才可发布。
计划记录训练来源及自身哈希，采集、恢复、离线回放和拟合共同消费同一声明，禁止
使用第四轮或稳态验证点反过来选择训练节点。断点策略升级为
`confirmed_branch_v7_final_image_sampling`，旧数据不会混入新流程冒充新证据。
另一项优化只省去重复准备等待：上一段已经完成到位检查、下一目标的完整指令向量
相同、一秒内反馈未变化时复用该结果；真实位移、反馈变化、记录过期或归零准备段
仍执行原运动检查。原运动速度、避让和采样稳定性门限保持不变。
对 O6 `20260915_150015` 的已保存数据离线比较，稳态训练点由每方向 9/9/10 个变为
7/6/10 个；独立验证仍为 10/10/11 个，双向总停点由 118 次降至 108 次。
小指因训练曲线噪声保留原网格，不以强行减点换取精度风险。
原方案和精简方案均通过原独立稳态角度验证，全部关节最大误差仍小于 1.69°。
这是旧数据的训练点选择对照，不能当作新四轮实机或最终空间精度通过。
原始文件哈希保持不变，脚本及结果为该会话的
`review/assess_static_sampling.py`、`review/static_sampling_assessment.json`。
### 最终文件到原始图像
`runtime/artifacts/image_evidence.py` 从原始记录绑定图像身份、四角、Tag 尺寸以及
受哈希保护的相机外参和 CameraInfo。`core/urdf/image_acceptance.py` 读取最终文件，
使用冻结的安装和基座，把 JSON＋URDF 的 FK 直接投影到第四轮及独立稳态验证四角。
这里不调用 PnP、优化器或重新配准；同一最终产物还须通过原来的 3D 和角度门限。
每帧四角重投影 RMS 沿用 1.5 px 门限，超限保存 `final_image_diagnostics.json`。
原始证据缺失、相机身份变化、训练/验证重叠及重复图像都会被拒绝。
新软件的自动测试与离线数据检查不代表四型号已经逐一实测通过。
O6 仍需恢复 `can0` 后运行新的完整四轮，并通过上述最终文件验收。
本次新增链路的验证结果：
- 四型号精简采样策略及实际 JSON→URDF→角点验收共 11 项通过（96.41 秒），
包含静态验证异常仍被拒绝、第四轮不影响训练点选择等反例。
- 局部恢复、真实定时器流程和重复等待边界共 24 项通过（21.62 秒）。
持续缺少第十张零位图像在一次补采后仍停止，不能少采冒充合格。
- 最终角点、真实相机来源、混合实测/复制产物及后台进程共 23 项通过（25.13 秒）。
- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过（1.81 秒）。
- 构建后再次执行 O6 正式 `calibrate_hand --no-resume`，仍在配置检查阶段报
`CAN interface does not exist: can0`，没有进入运动采集；日志保存为
`20260915_160040/review/o6_optimized_flow_startup.log`。
这些组存在覆盖交叉，不相加作为独立总数；四型号整链使用独立构造的合成观测，
真实来源绑定另用 O6 已保存角点检查，两者均不能冒充新实机结果。
全包回归中同步修正两处旧测试约定：零位不足的用例须观察到唯一一次恢复结束，
而非要求第一次超时立即暂停；历史侧面准备图像须调用带源 URDF 约束的当前生产入口，
不能继续要求旧自由几何模型必然可解。针对这两处的 7 项复测通过（11.49 秒）。
同一份侧面历史数据 121 帧使用当前入口通过，训练/验证仍为互不重叠的 61/60 帧，
候选统计门限与绝对像素门限保持不变；没有为了通过测试修改原始数据或生产门限。
全包首次运行共 1497 项：1494 项通过、上述 2 项旧测试断言失败，另有 1 项因未提供
O12 本地实采与独立参考文件而跳过（1175.15 秒）。两处失败修正后的 7 项复测通过，
没有剩余已知失败。完整日志、专项回归和构建日志保存在
`20260915_160040/review/common_flow_validation/`；不将跳过的 O12 实采检查视为通过。
## 第五轮评估：训练选择与准备验证的边界
2026-09-15 19:32 使用新相机外参进行正式 O6 采集，拇指两任务完整采完，
小指准备阶段两次均报 `image_motion_families_not_distinguishable`。原始数据与修改前
评估保存在 `20260915_193254/review/solver_assessment/`；先完成诊断实验，再修改生产代码。
整体链路保留：准备几何 → 冻结零位 → 四轮连续扫描 → 必要稳态映射 → JSON → 回读生成
URDF → 独立图像验收。本次问题位于准备模型的选择和验证，不涉及末端非线性存储格式。
独立稠密 Jacobian 求解证实增加迭代无效；四个初值有三个收敛到同一解，剩余解与其
相差约 19°、9 mm，不能把真实空间差异当作浮点误差。
审查发现原实现按验证误差选择优胜模型，再用同一验证集证明它较好；拟合使用平方误差，
统计比较却使用 RMS 差；逐个立即返回还保留了最保守的固定多重比较惩罚。
现统一为训练选择、验证否决，候选所用的训练资格不受验证结果或验证异常影响；
验证失败不得改选。公共 `image_model_selection.py` 按同一平方损失比较，完成全部候选
比较后使用 Holm 校正，原始与校正后的概率均写入诊断。
1.5 px、0.03 px、1°/1 mm 等价界、原角度和空间验收门限均保留，名义族错误水平仍为 0.01。
同帧等价检查按角色名对齐，并拒绝缺失或重复角色；不再依靠 `zip` 恰好有相同顺序。
姿态策略升级为 `confirmed_branch_v8_training_model_selection`，旧会话仅供诊断，不能
自动续接为新策略采集。新增策略适用于公共流程，不增加 O6 专用回退或停点。
修改前的候选策略诊断中，两次失败数据均可区分（概率约 0.00373、0.00576），拇指保持通过，
真正歧义的合成数据保持拒绝；这些仅是修复依据，不代表整手新实机验收完成。
统计假设和条件性边界参考 [SciPy Wilcoxon 文档](https://docs.scipy.org/doc/scipy/reference/generated/scipy.stats.wilcoxon.html)，
多重比较方法参考 [Holm 方法文档](https://www.statsmodels.org/stable/generated/statsmodels.stats.multitest.multipletests.html)。
新策略实机 `20260915_195619` 完整采集 36/36 个方向单元，0 次暂停、0 次局部补采；
小指准备一次通过。146 项准备求解与产物链专项检查通过。最终空间验收仍失败，
候选 JSON/URDF 未发布；这与采集是否完整是两个不同结论。
## 第六轮评估：先修复测量时间，再判断空间求解
该会话的原始角点、生产重放和参数对照保存在 `20260915_195619/review/spatial_assessment/`。
轴线投影开关没有改善整体误差；共同 URDF 的 39 参数图像求解虽然收敛，第四轮仍有
4.124 px 误差，因此没有采用这一替换。相同反馈和相同方向的第四轮图像还能构成
与任何确定性查找表不相容的反例：两帧像素距离 3.753 px，最大误差必有一帧不低于
1.877 px，超过现有 1.5 px 门限。该反例不依赖拟合算法，也不证明硬件损坏。
SDK 代码确认了一个时间来源错误：发布线程给低频轮询缓存反复填上当前时间。
相机与发布消息的时间差很小，并不能证明相机与真实反馈同步。修复位于共同数据边界：
- CAN 接收端原子保存位置和接收时间，O6/L6 单帧、G20 按各指分帧保留。
- SDK 的标准状态保留真实接收时间；标定专用接口携带每通道时间，仅发布新增测量快照。
- 标定按每通道实际时间插值。重复、乱序、过期或同一接收身份对应不同值的记录不能刷新
稳态样本或反馈新鲜度；原始反馈快照落盘，便于复核。
- 共用采集策略升级为 `unified_engine_v6_measured_feedback_time`，四型号 Profile 同步声明。
源 URDF、相机和精度门限保持原样；旧采集不能改挂新时间策略发布。
时间契约、缓存、分帧、回归与稳态检查 150 项通过；四型号配置、ROS 接入、断点与产物链
集成检查 69 项通过；SDK 与标定两包构建通过。不运动的实机接口核验中，486 条标定反馈
全部匹配独立旁路收到的原始 CAN 帧，0 条无来源、0 条重复身份。真实测量频率约 25 Hz，
同一时段标准接口发布 1147 条消息，说明之前约 58 Hz 的发布频率不能作为测量频率。
该核验启动 SDK，SDK 仍执行其初始化速度/力矩设置；没有发送位置运动命令。
完整实机产物仍须用新的测量时间重新采集后验收。
## 第七轮评估：控制消息顺序处理，图像任务独立执行
真实时间采集 `20260915_202203` 暴露了图像缺样：一段约 0.30 秒的有效图像空白
内仍有 15 条真实 CAN 测量，不应扩大同步窗口来掩盖。最初加入每视角有界图像工作线程，
但 ROS 端仍保留多线程执行器；`20260915_203104` 恢复导入期间仍因反馈超过一秒暂停。
这项中间实现没有完成目标，不能算作修复成功。
进一步评估保存在 `20260915_203104/review/dispatch_profiling/`：真实 64 条记录编码
约 6 ms，不支持直接减小导入条数。实机锁计时和线程栈显示，多个 ROS 回调争用同一把
会话锁，图像队列还在持有自己的锁时反向查询会话。仅在诊断入口替换为单线程 ROS
执行器后，反馈进入回调的延迟由初段中位约 113 ms、P95 247 ms，降到约 9 ms、17 ms；
同一精度、采样和时间策略下已成功复用 8 个合格单元。
据此整理公共 ROS 接入，而不更换拟合器或增加型号特例：
- 控制、反馈、相机消息解析和状态更新由一个 ROS 执行器顺序处理，删除冗余回调组。
- 每个视角只保留一项执行中的图像任务和一项待处理任务，过载时保留最新待处理图像。
- 只有 coordinator 接受的新测量才能推进图像队列的时间水位；多帧协议使用最旧通道时间。
队列持锁时不调用 coordinator，开始新会话时清空旧水位和未执行图像。
- 原 50 ms 同步界限继续有效；图像估计仍在锁外，提交结果仍检查会话和运动版本。
- 图像线程异常传回控制入口，不将线程停止当成正常无图；检查点落盘规则保持原样。
调度、ROS 接入、真实测量时间、coordinator 和断点恢复共 51 项针对性检查通过；
图像元数据并发、过期结果提交和隔离计算共 28 项检查通过；标定包构建通过。
单线程对照运行及后续正式运行的终态以各会话记录为准；这里的延迟改善不能代替
四轮完整性、独立角点和 JSON＋URDF 的最终空间验收。
## 第八轮评估：相机曝光时间与取帧时间分离
单线程对照会话 `20260915_203750` 完成 36/36 单元（复用 8 个已验证单元）、0 次暂停，
生成候选 JSON 和由该 JSON 重建的 URDF，但顶部 ID7 的训练安装位置 P95 为 18.285 mm，
超过 3 mm，产物未发布。其余四个实测 Tag 角色通过该项空间检查。共同 URDF 的 39 参数
原始角点拟合虽然收敛，第四轮仍达 4.779 px，因而没有替换生产拟合器。
随后检查图像时间来源，发现相机驱动在 `MV_CC_GetImageBuffer` 返回后使用当前 ROS
时间，不能表示曝光时刻。实测三台相机的设备时钟、40 次独立时钟锁存，以及曝光开始/
结束事件，确认：
- 三台取帧时间分别比设备帧时间晚约 16.44 / 16.52 / 16.49 ms；不应靠拟合器吸收该延迟。
- 帧计数时间与独立曝光开始事件相差约 150 ns；固定曝光时可使用曝光中点时间配对。
- 当前固件的 `DeviceTimestampIncrement` 返回 100,000,000，实测约 10 ns/tick。
设备 XML 的单位说明与此不一致，因此程序启动必须用独立经过时间核验计数单位。
`camera_timing.py` 单独负责设备/主机时钟对应和曝光时间换算。每 0.5 秒重新锁存，
选择往返区间最短的有效测量；偶发缓慢事务丢弃后有界重试，不能刷新原两秒有效期。
错误单位、时钟跳变、计数器重置、重复帧和过期对应均不能伪装成新图像。
相机驱动在图像及 CameraInfo 中使用同一曝光中点时间，保留每次锁存和每帧换算记录
`camera_timing_<view>.jsonl`。固定外参和内参不因软件时间修正而改变。
共用采集策略升级为 `unified_engine_v7_camera_device_time`，四型号 Profile 及其受保护
哈希同步更新；旧会话保留诊断用途，不能直接续接到新时间规则。改动没有增加停点、
调整非线性查找表语义或放宽最终精度门限。
时钟、真实三相机记录、ROS 接入、配置、启动图和断点/队列共 57 项检查通过；构建通过。
随后实际驱动对三台相机各连续发布 242 帧、执行 16 次校时，无时序或时钟错误。
O6 运动期间另旁路监听 CAN：370 条位置命令的响应均不是原命令回显，仍可作为真实状态
观测；没有把 L6 的回显过滤规则未经核实套到 O6。
完整新策略实采会话为 `20260915_205704`，不复用断点、不使用调度诊断包装。
共完成 36/36 个单元，0 次人工暂停；拇指弯曲第四轮增加方向和小指第三轮增加方向
各自动重扫一次。候选 JSON/URDF 已生成，Tag 安装刚性训练检查失败，未发布 manifest。
正式失败时位置 P95：小指末节 3.108 mm、拇指末节 6.476 mm、拇指弯曲 3.875 mm、
顶部拇指 10.086 mm。会话中 `review/spatial_assessment/live_run_summary.json` 保留统计。
## 第九轮评估：反馈插值契约与空间模型边界
反馈拟合使用曝光时刻的连续插值值，报告声明 `piecewise_linear`；原 `JointMapping`
和 `SerializedJointMapping` 却对所有 `_u8` 输入再次取整，使训练和读取语义不同。
统一规则为：现代反馈曲线连续插值，实际 `command_u8` 量化，历史整数表保持原兼容行为。
序列化读取在量化前检查输入支持域，不能将越界小数取整后当成合法测量。
此修改不改变非线性曲线本身、不重拟合零位、不调整采集版本或原始记录。
先用相同 39 参数整链诊断做对照：保留原 URDF 骨长、轴向和已拟合曲线，前三轮拟合
共同基座、允许的零位及固定安装；独立第四轮不参与优化。取消反馈取整后，小指末节
最大像素误差由 3.390 降至 2.449 px，但仍超过原 1.5 px，因此该诊断模型未接入生产。
另加有界双向曲线修正的 119 参数实验，第四轮小指末节最大仍为 3.136 px，且出现
非单调局部曲线；增加参数并没有提供足以替换现有生产求解器的证据。
正式公共回放也验证：反馈取整修复不能消除空间不一致。位置 P95 为小指末节 3.080 mm、
拇指末节 6.489 mm、拇指弯曲 3.877 mm、顶部拇指 10.088 mm；保持失败且未发布。
原始会话与在线候选文件未覆盖；新回放位于 `20260915_205704/review/spatial_assessment/`
下的 `continuous_production_replay/`。同目录保留诊断脚本、参数、Jacobian 和测试日志。
补充整链可行性对照仍只使用前三轮、相同 39 个参数：将训练目标换成按原 2°/3 mm
尺度归一化的三维位姿残差，局部收敛后拇指弯曲零位触及原 20° 边界；第四轮顶部
位置 P95 仍为 5.148 mm，原始图像最大误差增至 28.718 px。此结果没有证明全局无解，
但否定了将这一局部三维优化结果直接用于替换现有求解器的做法。
原始保存位姿经受保护的相机变换投回同帧角点，各角色最大误差均小于 0.64 px；
没有发现这条原始公共坐标变换的方向或图像身份错配。检查见
`continuous_spatial.json`、`common_pose_projection_check.json`。
反馈输入、双映射、被动非线性与读取兼容检查 57 项通过；补充真实时间插值、四型号
合成产物链、JSON 重建、独立角点和标准 URDF 检查 49 项通过，两组有交叉。
SDK 和标定包最终构建通过，`git diff --check` 通过。
合成验证不等于其他型号实机通过；本轮 O6 的最终空间精度问题仍未解决。
## 第十轮评估：保留跨运动的原始图像证据
此前原始角点随任务主机位的姿态采集写入，缺少顶部 ID7 在弯曲中、正面 ID2 在侧摆中
的完整原始图像。这两块 Tag 已确认属于同一末节；只保存各自任务下的图像不足以独立
核查共享运动链。公共采集器现独立保存所有机位的有效原始角点，保留原检测质量、
窗口与会话身份，不依赖 PnP 是否成功。主采样门限、运动计划和稳态点数量均不变。
同机位图像时间去重只在成功提交时推进；迟到或窗口已撤销的图像不能推进水位。
采集策略升级为 `unified_engine_v8_all_view_images`，四型号 Profile 及产品哈希同步更新。
旧会话不修改哈希、不补造新策略记录。新增采集与公共并发检查 43 项通过，补充采集、
四型号配置与断点身份检查 58 项通过，两组有交叉；标定包构建通过。
新会话 `20260915_213337` 完成 36/36，0 次人工暂停；小指第二轮反向扫描自动重试一次。
三机位共保存 16,822 个原始图像记录。正式算法仍在 Tag 安装空间一致性处失败，
未发布 manifest。主动零位与上一会话差均小于 0.03°，不能以重复性替代正确性。
保持当前源 URDF、相机参数和 Tag 尺寸，前三轮拟合共享基座、零位和每块 Tag 的固定安装，
第四轮检查的新全视角对照仍未通过。39 参数固定曲线模型的最大像素误差约 3.52 px；
359 参数双向单调曲线模型最大约 4.32 px。后者按 SDK 输入均匀选择训练图像，
不再按停留时间重复加权；非负曲线增量和参考输入零约束仍不足以消除残差。
静止姿态也出现约 1%～3% 的投影边长偏差，因此另做一次受限的统一骨长尺度诊断。
其训练最优尺度约 1.0327，第四轮最大误差仍约 4.37 px；没有采用该尺度，没有修改
源 URDF 或相机文件。原始公共位姿可重投回其源角点，暂未发现转换方向错误。
上述原型和数值结果均仅保存在 `20260915_213337/review/whole_chain/`，未接入生产求解器。
仅用稳态共同视野核查 ID2/ID7 刚体关系时，训练 6 个姿态、留出 7 个姿态全为侧摆，
不能独立确定完整外参。连续扫描保存了两种动作的共同视野，可在原曝光时间上对齐后
进行独立于 URDF 和曲线的外参一致性诊断；诊断结果不能自动覆盖已记录的相机参数。
## 第十一轮评估：尺度一致性与全机位最终图像验收
对 ID2/ID7 同末节约束做独立检查，不使用 URDF 尺寸、轴向或 SDK 查找表。
连续图像按曝光时间对齐，线性/局部三次及留一插值的一致性误差不超过 0.1 px；
训练前三轮 96 个姿态，第四轮 32 个姿态仅检查冻结的参数。每帧刚体位姿作为局部未知量，
两个 Tag 之间始终只有一个固定安装变换。多初值只按训练平方误差选择。
| 诊断假设 | 第四轮 front/top 最大误差 | 结论 |
| --- | --- | --- |
| 记录的外参与两个 16 mm Tag | 1.28 / 1.69 px | 与原始图像仍有不一致。 |
| 自由估计 front/top 外参，Tag 均 16 mm | 0.44 / 0.48 px | 外参改变量约 2.43°/23.72 mm；条件三倍标准差约 1.96°/22.24 mm，不能作为精确外参替换。 |
| 记录外参，单独估计 ID7 尺度 | 0.84 / 0.49 px | 固定 ID2 为 16 mm 时 ID7 约 16.52 mm；不是两块 Tag 实测尺寸差的独立结论。 |
| 记录外参，同时估计两个 Tag 尺度 | 0.42 / 0.52 px | 有效边长 ID2 约 16.38 mm、ID7 约 16.52 mm；依赖现有相机模型和刚体/正方形假设，尚不能替代物理尺寸。 |
自由外参不能解释此前保存的棋盘格图像：其最大误差约 12.93 px，原外参约 0.56 px。
因此不能因为手指图像误差变小就认定相机移动或自动重写外参。只读采集当前静止画面后，
正面背景匹配显示约 3 px 变化，侧面约 1 px 内；顶部旧图被棋盘格遮满，没有足够共同背景，
不能判断顶部机位是否变化。该检查没有启动 SDK 或驱动手指。
另将整链中的所有相机基线或所有 Tag 尺寸分别只增加一个公共尺度参数：前三轮最优值
约为 0.97226 和 1.03073。两者都明显降低拇指正面误差，但第四轮小指末节最大仍约
4.29/4.27 px、顶部最大仍约 2.66/2.44 px；没有采用任何尺度修正。
条件协方差未包含相机、打印尺寸和模型的系统误差，不能用其很小的数值宣称物理尺寸测准。
已请求分别精测 ID2/ID7 黑框宽高及棋盘格跨五格长度，现有整数尺寸记录不足以区分尺度来源。
结果在 `review/whole_chain/rigid_*_assessment.json`、`board_camera_comparison.json`、
`static_background_comparison.json` 和 `all_view_*_scale.json`。
程序结构上，只有任务主机位参与最终图像验收会漏掉同一 Tag 在其他动作下的不一致。
现将相机身份核验和原始图像索引集中在 `ImageEvidence`，反馈和稳态指令各自使用独立窗口。
前三轮、未通过尝试和无同步反馈的原始图像不能成为最终验收测量；主观测必须在新原始记录
中找到完全一致的角点、SDK 值和方向。其他动作中的原始角点也直接进入冻结 JSON/URDF 投影，
不伪造 PnP 位姿，不参与训练，不增加运动或停点。历史主图像读取接口保留原证据语义。
实际 O6 输入核对：反馈原有 1,818 个主观测全部一致，增加 3,270 个其他观测；
稳态指令原有 312 个主观测全部一致，增加 677 个观测。此结果仅证明证据绑定正确，
不代表最终空间精度通过。专项和现有产物流程 36 项通过，补充最终检查、四型号合成产物、
JSON 重建及非有限输入反例 34 项通过；两组有交叉，不相加为独立测试总数。
标定包构建通过；实际数据证据核对和测试日志保存在
`review/whole_chain/all_view_final_source_verification.json` 与 `final_image_validation/`。
### 剩余运动残差的解释边界
固定上述未启用的候选相机尺度与整链几何，仅逐帧调整活动关节角度进行定位。
小指两关节最大改变量约 0.63°/0.49°，侧摆约 0.38°；图像最大残差分别可降至
0.43/0.94 px 和 0.47 px。最大原始残差集中在 SDK 约 244 的运动区间；
同一 Tag 静止、其他手指运动时的误差显著更小。
这项实验在每个被检查图像上估计局部角度，明确**不是独立第四轮验收**，也不证明
其条件几何正确。它提示剩余像素误差中包含运动映射误差，不能据此继续修改骨长或零位。
前三轮相同输入位置的角度改变量有约 0.1° 的轮间差，增加曲线节点也未必能消除。
后续需分别评估反馈分辨率、允许的运动映射误差与图像测量误差；不能直接把 PnP 的
像素测量门限等同于整条运动链的角度/位置精度要求。本轮没有修改任何验收门限。
完整逐帧结果见 `review/whole_chain/local_angle_residuals.json`。
## 第十二轮评估：分离图像测量误差与运动精度
先用完全独立的已知真值验证规则：单关节运动存在恒定 0.5°、0.873 mm 偏差，
满足原角度和位置要求。仅将相机焦距从 1000 改为 3500 px，最终 FK 的最大投影误差
就从 1.10 变成 3.84 px；原规则因此对相同物理精度作出不同结论。测量位姿对原始角点
的误差均近于零。这证明 PnP 的 1.5 px 图像测量门限被误用于含运动映射误差的总投影量。
公共验收现在分为以下职责：
1. 发布器原有的独立三维反馈、指令验收仍先执行，角度 MAE≤1°、P95≤2°、最大≤3°，
位置 P95≤3 mm。非线性被动关节仍读取完整 JSON，不以 mimic 近似替代。
2. `image_acceptance.py` 将独立位姿与原始角点、SDK 输入、方向和图像身份直接绑定。
测量位姿重投影 RMS≤1.5 px；最终 FK 的总投影误差保留为单独统计，不据此改变物理精度要求。
3. 额外视角无独立三维测量时，`image_consistency.py` 查找一个能解释原始角点的局部刚性位姿，
直接核验其像素残差及相对最终 FK 的物理位移。数值见证仅说明图像与容差相容，
**不是真实位姿测量，不证明最优，不替代独立三维精度**。求解器返回成功也不能单独使其通过。
不回写或重新估计共享的相机、安装、基座、零位和曲线；按运动任务分别统计，防止静止帧稀释误差。
4. manifest 明确记录 `independent_measurement_and_physical_consistency_v2`，
独立精度与额外机位相容性分开报告。缺失独立位姿不能由数值见证补齐。
解析投影 Jacobian 已用独立有限差分检查。已知真值、不同焦距、物理超差、角点/相机篡改、
证据身份和静止图像稀释等 42 项专项检查通过；四型号合成产物、非线性、JSON 重建及原物理
验收共 36 项通过；最后的规则版本/任务身份补充检查 15 项通过。测试集合有交叉，不相加。
标定包构建通过，`git diff --check` 通过。没有增加实机轮次或停点。
最新真实 O6 的 1,818 个第四轮及 312 个稳态主观测均核验了真实保存位姿与原始角点，
最大误差分别为 0.613/0.616 px。此检查没有拟合任何参数，属于测量证据核验，不是产物 PASS。
复算原训练安装检查仍得到拇指位置 P95 为 5.371/3.591/11.352 mm，仍失败，没有 manifest。
尺度/相机输入尚待现场精测区分，未采用之前任何诊断尺度，也未直接替换整链求解器。
已知真值脚本使用保存的旧规则源码复现反例，当前规则由生产测试验证；全部记录位于
`20260915_213337/review/whole_chain/image_error_budget/`。
## 第十三轮评估：轴线参考点不能决定零位
一条转轴可用轴上任意一点表示；沿轴移动该点不会改变物理几何。当前 O6 的旧零位公式
先把两轴参考点的差直接投到图像平面，没有消除沿轴任意坐标。相机斜看转轴时，这个
任意坐标被误解释成弯曲零位。旧代码已为 O12 增加可选开关，但 O6/L6/G20 仍使用原公式。
在真实 O6 的相同轴线输入上，仅将被动轴参考点沿轴移动 ±20 mm：拇指弯曲零位从
−9.8736° 变成 +1.0529° 或 −16.1650°，三个结果竟都通过原轴线角度检查。整个实验没有
改变相机、Tag 尺寸、运动图像或真实轴线，足以证明程序中存在几何表示相关的错误。
公共 `ObservationGeometry.phase_error` 现始终先取两轴的垂直间距向量，再投影到相机平面。
移除了 `project_axis_gauge_before_image` 型号开关及其编译参数；O12 删除已无必要的 YAML
声明并同步产品哈希，其数学行为保持原先已启用的正确规则。O6 的采集配置与受保护输入
不变，可以用原四轮数据离线复算，无须增加运动。
独立测试使用四型号的原始 URDF、自行生成已知 0.07 rad 零位，改变相机视角、共同坐标系，
并任意移动预测和观测的轴上参考点；所有平行轴对均恢复相同真值。源码专项、四型号完整
合成产物链、配置与 O12 安装/机位变化共 24 项检查通过；源代码的轴线/求解顺序 9 项通过。
构建后另核对实际加载的安装包与源码字节一致，安装包的轴线、图像与物理门限检查共
51 项通过，避免用旧安装文件检验新修改。各测试集合有交叉，不相加。
正式回放保留原始输入身份检查，输出到 `review/whole_chain/axis_gauge_production_replay/`。
拇指弯曲零位约 −0.5384°、小指约 −1.8044°，不再依赖轴上参考点；侧摆仍约 14.5179°。
JSON 已先保存并被读取生成候选 URDF；空间安装检查仍失败，位置 P95 如页首所列，
未生成 manifest。这项数学修正没有消除后续空间不一致，也不能替代尚待核实的尺度输入。
对最近两次已保存的轴线输入使用同一修正公式，侧摆、拇指弯曲、小指零位差分别为
0.02955°、0.01044°、0.02942°。这是条件零位的重复性对照，不是绝对精度通过；
没有改写旧 v7 会话的采集策略或将其伪装成 v8 发布。结果见 `corrected_zero_repeatability.json`。
旧公式及配置已备份到 `axis_gauge_before/`，`assess_axis_line_gauge.py` 从备份源码复现
历史反例；不将旧公式或型号条件保留在生产求解器中。数值对照见 `axis_line_gauge_assessment.json`，
源码、安装包测试及构建日志均在同一 `review/whole_chain/` 目录。
## 第十四轮评估：独立双目尺度检查
先使用已冻结的 ID2/ID7 刚体关系，在第四轮 32 个共同观测姿态上仅估计逐帧刚体位姿，
不使用 URDF 或 SDK 曲线。保持原相机与两个 16 mm Tag 时，front/top 图像最大误差仍为
1.280/1.693 px；与原主机位三维测量的位置 P95 分别相差 26.84/19.09 mm。
此前“调整外参”或“调整 Tag 尺寸”的条件模型均能把最大图像误差降至约 0.5 px，
但得到不同的三维位置，且部分与原主测量相差约 20～33 mm。因此不能将其中任意一个
条件重建当成独立真值，亦不能用小图像残差证明最终空间精度。共享参数均未在第四轮更新。
对照见 `stereo_primary_measurement_comparison.json`。
只读保存的静止画面还发现：侧面相机可以同时看见正面 ID1、ID2。用两机位对同一个 Tag
的四个对应角点直接三角测量，不预设 Tag 尺寸、刚体安装、URDF 或 LUT。两帧曝光相差
6.55 ms，保存时没有 SDK 控制进程、机械手静止。尺度仅来自记录的 front/side 外参基线。
- ID1 两组对边平均长度为 **16.496 / 16.503 mm**；两机位重投影 RMS 为 **0.536 / 0.443 px**。
这项独立对照进一步提示当前尺度与“16 mm”的记录不一致。
- ID2 两组对边约 **16.593 / 16.713 mm**，但重投影 RMS 为 **1.583 / 1.412 px**，
最大单角误差约 2.28 px，不能将该尺寸估计作为已通过质量检查的测量。
此检查仍依赖棋盘格确定的相机基线尺度，不能独立区分打印 Tag 偏大与棋盘格尺度偏差。
没有把 ID1 的尺寸复制给其他 Tag，没有更改原相机或尺寸配置，没有增加整手运动。
原始 PNG、相机元数据、逐角三维坐标及误差均可复查，见 `current_camera_images/`、
`same_tag_stereo_assessment.json` 与相应同名诊断脚本。
下一项必要现场证据仍是已请求的精确黑框尺寸与棋盘格跨五格长度。现有图像能发现
尺度不一致，但不足以无假设地决定应修改哪一项物理输入。尺寸证据到位后，应先核实输入、
评估与原始数据的兼容性，再复算整链；不能把某个局部拟合最优值直接写入生产配置。
## 第十五轮评估：固定已确认尺寸，排除图像坐标处理与局部拟合误导
2026-09-16 用户再次明确：全部 Tag 黑色码区为 16×16 mm，棋盘格为 27×27 mm。
这解除上一轮的尺寸信息等待；本轮按用户确认值作为固定物理输入，不把双目条件估计
当作新的 Tag 尺寸。以下诊断记录在 `20260915_213337/review/whole_chain/confirmed_dimensions_audit/`。
### 1. 原始图像与去畸变图像的数值核验
仅启动相机，机械手不运动；保存同一曝光时间戳的原始和去畸变 PNG。按 CameraInfo 的
K、D、R、P 独立执行 OpenCV remap，front/side/top 三机位与 image_proc 输出的最大、平均
灰度差均为 **0**。程序以 P 解释 image_rect、棋盘物点间距为 0.027 m；没有发现单位错误、
缩放裁剪、重复去畸变或原始 K 与去畸变图像混用。该证据只核验软件坐标契约，不证明
K/D 本身就是准确物理内参。结果见 `raw_rect_consistency.json`、`raw_rect_capture/`。
新静止图像中 ID1 的两组对边平均为 **16.5057/16.5078 mm**，两机位重投影 RMS 为
**0.830/0.687 px**。ID2 的重投影 RMS 为 1.836/1.635 px，继续不作为合格尺度测量。
因此上一轮发现的矛盾可再次观测，并非一次检测结果；见 `fresh_same_tag_stereo.json`。
### 2. 棋盘低像素误差不能单独决定新相机参数
使用已有原始角点，保持 27 mm 间距，对正面/侧面及正面/顶部训练对分别联合估计内参。
两组的留出误差均下降，但同一 front 的主点横坐标分别约 866/1078 px，明显不同。
又将单机位训练集分为互不重叠子集：side 的部分子集可给出明显不同焦距/主点，同时
在同一留出图像上仍保持约 0.45～0.48 px 的 RMS。不能凭这个像素指标选择物理参数。
结果见 `joint_intrinsics.json`、`intrinsic_stability.json`；未修改相机配置。
进一步以 OpenCV object-releasing 仅在训练图像中估计一个固定棋盘角点形状，保持
第一行两端 7×27 mm 间距。三个机位独立估计的形状均存在亚毫米偏差；其中 front、side
估出的形状相近。冻结形状后，未参与拟合的单机位图像 RMS 分别从
**0.276/0.465/0.317 px** 降为 **0.133/0.168/0.138 px**。
这提示理想平面棋盘假设可能影响内参，但仍是条件模型证据，不等于已测准棋盘变形。
为避免“各相机自由拟合都能变好”的误判，另只采用 front 训练图像估出的同一形状，
重新估计 side/top 内参，再用 19:25 棋盘原角点训练新的条件外参（每四组留一组验证）。
side 的留出双目 RMS 最大约 0.395/0.383 px；随后检查从未参加相机拟合的新静止 Tag，
ID1 两组对边仍为 **16.5038/16.5058 mm**。棋盘误差改善没有解决目标尺度矛盾，故明确
拒绝启用这组候选参数。见 `board_shape.json`、`shape_camera_transfer.json`。
### 当前下一步
已请求保持手和相机不动，将棋盘置于手旁，使 front/side 同时看到完整棋盘及 ID1，
采集同一时段的静止画面。这可直接核查已确认的两种尺度，减少先标棋盘、后拍手之间
场景变化的影响。另询问棋盘载体是否刚性平整，作为几何假设的现场证据。
本轮不增加整手运动、不启用候选相机参数、不改变 Tag 尺寸或精度门限；原数据和受保护
配置保留。整手发布条件仍未达成，无新的合格 manifest。
### 当日现场反馈与贴平后检查
用户补充“棋盘没有完全弯曲变形，tag贴纸有弯曲不平整”，随后确认“已贴平固定”。
因此不能把之前的平面 Tag 位姿当作已证实准确的物理测量，也不能把棋盘条件拟合
直接解释成全部误差的原因。正式复标需使用处理后的新图像，而非沿用旧 Tag 安装结果。
10:09 的贴平后静止对照，front/side 曝光差为 1.763 ms。ID2 的双目重投影 RMS 为
1.349/1.201 px，较处理前改善；ID1 为 2.254/1.872 px，仍不满足图像质量门限。
两者的条件尺度仍约 16.5 mm，不能据此宣布测量链修复完成。数据保存在
`confirmed_dimensions_audit/flattened_tags_20260916_100921/`。
用户表示棋盘已摆好后进行了实际拍摄，并持续开启无机械手控制的双目预览；至 10:10
两路画面实际均未出现棋盘。已提供正面/侧面画面帮助摆放，没有将空缺的棋盘观测标成
成功采集。新的同帧棋盘/Tag 核验脚本已用已知真值验证：正确相机下恢复 16 mm 各边，
误差小于 1e−5 mm；该合成检查不是现场相机验证。下一步仍需让棋盘实际进入两个机位，
目前未重新启动整手运动或输出新的合格产物。诊断汇总与受保护输入哈希见
`confirmed_dimensions_audit/audit_summary.json`。
## 第十六轮评估：分时核验棋盘与 Tag，取消不必要的同框限制
固定相机外参时，棋盘负责估计相机之间的刚体变换；Tag 的位置和姿态在检查时独立
估计。因此棋盘与机械手不必同时出现在画面中，手在两次拍摄间也不必保持原位置。
此前把同框设为必要条件，给现场带来了不必要的摆放限制，现已取消。只需相机及镜头
设置在两段采集中保持固定。这一修改属于诊断采集流程，不改变整手正式运动流程。
10:21～10:22 保存四组正面/侧面完整棋盘原图：
`board_placement_preview_20260916_102131`、`102213`、`102215`、`102217`。
原始双目曝光差 10.79～10.99 ms；每组以已确认的 27 mm 棋盘估计条件相机变换，
棋盘重投影 RMS 为 front 0.281～0.364 px、side 0.438～0.571 px。
四次相对首帧的变化最大为 0.0272°、0.458 mm。姿态相近的四组只说明条件重复性，
不能充当完整多姿态外参标定，也没有写入活动相机配置。
临时预览的自动保存器还曾设置 20 ms 配对条件，而一次启动后的相机帧相位差约
24.66 ms，造成没有进入检测。这是诊断脚本的问题，已采用原相机核验的 50 ms 配对
范围，并保留真实曝光时间；正式图像与空间精度门限不变。另一个临时触发条件要求
全部棋盘角点连续 2 秒变化均小于 0.4 px，未触发自动保存。上述四组是另行直接保存的
原始画面，经离线图像核验后使用；不能把预览中的 `saved_count=0` 改记成自动触发成功。
后续共面检查直接保存可见原图，在原图上评估几何质量，不先用这种触发条件丢弃证据。
用户移开棋盘、放回 O6 后，10:24 保存新的 Tag 图像，曝光差 11.75 ms，位于
`joint_board_tag_20260916_102403/`。分别使用上述四组棋盘估计的相机变换，得到：
- ID1 两组对边平均约 **16.487～16.500 mm**；自由角点双目重投影 RMS 为
front 1.040～1.107 px、side 0.878～0.935 px。
- 强制 ID1 为 16 mm 平面方形时，front RMS 仍为 **1.629～1.677 px**，超过 1.5 px。
- ID2 两组对边约 **16.499～16.585 mm**；自由角点的双目 RMS 约 0.41～0.51 px。
新旧手位姿分别估计，不参与棋盘相机参数的拟合。使用已知真值的另一个位置/姿态
Tag 验证过这一分时检查方法，两种来源均恢复 16 mm，误差小于 1e−5 mm。
结果记录在各棋盘目录的 `board_tag_consistency_joint_board_tag_20260916_102403.json`。
这仍不足以单独判定应改 Tag 尺寸、棋盘尺寸还是相机模型；没有因此运行新一轮整手运动。
### 共面长度比例的独立检查
用户提供同批备用 Tag，并把 ID1 固定在棋盘同一块板上。10:27 的三个原始正面图像
位于 `coplanar_capture_20260916_102710/frame_00` 至 `frame_02`。图中左侧的备用 ID1
与棋盘共面；右侧手掌 ID0 不共面，不将其按棋盘平面换算的数值解释为尺寸。
使用棋盘格交错的 20 个角点拟合平面单应变换，另 20 个角点检查投影误差，不使用
相机外参、URDF、SDK 或关节曲线。原始像素直接计算的 ID1 两组对边约
**16.474～16.478 / 16.595～16.602 mm**，留出棋盘角点 RMS 约 0.186～0.191 px。
用当前内参去畸变后约 **16.680～16.685 / 16.593～16.600 mm**；旧内参、未采用的
棋盘形状内参仅作预先规定的敏感性对照，结果也记录在同一报告中，不按 Tag 尺寸选模型。
该 ID1 接近原图左边缘，畸变模型之间仍有约 0.2 mm 的差异，所以未据此改配置。
随后请用户将备用 ID1 移到棋盘上方中间，再核验边缘畸变的影响。
### 中央位置复核与实测尺寸的矛盾
用户将备用 ID1 贴到棋盘中间上方后，10:43 保存三个新的原始正面图像，位于
`coplanar_capture_20260916_104349/frame_00` 至 `frame_02`。图中 ID1 位于同一底板上方，
纸张棋盘完整可见；手掌 ID0 仍不与棋盘共面，不解释其尺寸换算值。
沿用预先规定的棋盘交错训练/留出划分，三个图像的结果为：
- 原始像素直接计算的 ID1 两组对边为 **16.511～16.513 / 16.594～16.602 mm**；
留出棋盘角点 RMS 为 **0.188～0.197 px**。
- 当前内参去畸变后为 **16.521～16.523 / 16.636～16.645 mm**，相较原始像素仅变化约
0.01 / 0.04 mm，不能解释相对 16 mm 的约 0.5 mm 差异。
- 另用传统棋盘角点检测加亚像素细化，并分别使用全部角点、靠近 Tag 的三行或两行
拟合平面变换；不同检测方法和网格子集的边长变化小于 0.1 mm，比例差异仍存在。
这是同一图像上的方法敏感性检查，不是新增独立实物样本，也不是新的合格标定。
结果见各帧的 `coplanar_scale_assessment.json` 及汇总目录中的
`central_coplanar_sensitivity.json`。上述长度全部以 27 mm 棋盘间距、平面棋盘及
Tag 与棋盘共面为条件；图像本身不能无条件证明 Tag 的绝对毫米尺寸或两者物理共面。
这项检查未使用外参、URDF 或关节拟合，不能通过增加关节拟合参数解决其比例矛盾。
用户随后确认 16 mm 和 27 mm 均为打印后实物测量值。保留这两个生产输入，不能将
图像条件换算直接盖过实测值并断定打印错误。已请求测量工具、Tag 黑框宽高及棋盘连续
五格的未取整总长度，以区分测量精度与几何假设的问题。此次仅进行静止图像与离线
检查，没有新增整手运动，没有修改相机、尺寸或验收门限，没有生成合格发布 manifest。
## 第十七轮评估：依据实物复测修正 Tag 尺寸
用户再次量取后明确：“tag 是 16.5×16.5 mm，棋盘还是 27×27 mm”。这提供了独立的
实物输入修正依据，与先前双目约 16.49 mm、中央共面约 16.51×16.60 mm 基本一致。
不是从 URDF 拟合残差反推并选择一个易于通过的尺寸。
原先按 16 mm 计算 16.5 mm 的方形 Tag，会在同一单目姿态解下将平移距离缩小到
16/16.5，约低估 3.03%；跨相机变换中的基线平移来自 27 mm 棋盘，并不会随之缩放。
这构成明确的输入尺度不一致，不能通过增加运动曲线参数来合理消除。
修改范围仅为当前 O6 的以下配置及指纹：
- `profiles/o6_right_8.yaml`：8 个 Tag 均显式声明 `size_m: 0.0165`。
- `o6_right_8_tags.yaml`：检测器默认尺寸和每个 ID 的尺寸均为 0.0165 m。
- `o6_three_camera_calibration.yaml`：标定节点尺寸及全部 ID 覆盖值同步为 0.0165 m。
- `o6_right_product.yaml`：更新以上三个受保护文件的 SHA-256。
程序已有 Profile、检测器和标定节点的尺寸一致性检查，本次无需增加型号特判或
修改拟合算法。其他型号/打印批次没有新的实测依据，不自动替换其配置。棋盘尺寸未变，
没有因此修改相机内外参、源 URDF、旧采集文件或验收门限。非线性关系仍由完整 JSON
和修正 URDF 配合表达。改前配置与改后指纹见
`20260915_213337/review/whole_chain/tag_size_correction_20260916/`。
28 项配置加载、实际 launch 参数和 PnP 参数测试通过；已重新构建安装，核实四个
配置文件与源码逐字节一致，`--validate-only` 通过。用户移开棋盘及备用 ID1 并确认
O6 可运动后，启动新会话 `20260916_105528`，显式 `--no-resume` 完整采集。
最终是否合格须以该会话的独立验收及发布文件为准，不能仅凭尺寸改正确就声明成功。
### 新会话结果与收尾内存问题
36 个采集单元全部通过，小指第三轮 increasing 方向第一次存在反馈分箱空白 42，
按原最大空白 16 的门限自动重扫一次后通过。完整采集约 6 分 47 秒；保存 15,167 个
原始图像记录、6,828 个关节样本及 15,629 个 SDK 反馈样本。11:02:20 完成最后采集单元，
随后开始安全回位与收尾。11:05:28 系统内核明确记录 OOM、4 GiB swap 耗尽，终止了
一个 VS Code 进程；三相机同时出现长时间时钟读取中断，SDK 反馈超时，终端随后发生
BrokenPipeError。不能把该现象归因于机械手硬件，也不能将此会话记为在线无暂停完成。
原始日志完整保留，正式 `--offline-raw` 流程验证了配置指纹、全部采集单元、运动来源、
独立第四轮与稳态验证数据，生成 `offline_165mm/` 下的 JSON、URDF、报告和
`release_manifest.json`，退出码 0。JSON/URDF 的 SHA-256 与清单相同；已从保存 JSON
再独立调用重建 CLI，得到逐字节相同的 URDF。原来的角度 MAE/P95/max 1°/2°/3°、
位置 P95 3 mm 及对应图像门限没有修改。
同时发现 `read_journal_prefix` 先整文件读取、再 `splitlines()`，在解码对象之外还保留
两份原始文本。这份日志为 318,772,511 字节，额外副本会显著增加相机仍运行时的收尾
峰值。已改为按冻结字节边界逐行解析，保留全部记录、顺序和不可越过边界的校验；
不通过丢数据或修改安全超时规避问题。新增 16 MiB 日志的临时缓冲内存上限回归检查，
并验证短文件与非对象记录拒绝；独立进程、取消、真实 finalizer 等共 9 项测试通过。
已重新构建安装，进一步用同一实采数据检查独立收尾进程与已验收文件的一致性。
修改后的已安装独立进程以 `require_motion_evidence=True` 处理同一份完整原始数据，
约 **55.21 秒**完成，子进程峰值 RSS 为 **1,344,164 KiB（约 1.28 GiB）**；
JSON、URDF 两个文件的 SHA-256 均与正式离线验收版本相同。该检查没有启动相机或 SDK，
也没有重新运动机械手，不能单凭它宣称修复后在线全过程已经完成第二次实机验证。
报告为 `20260916_105528/isolated_finalization_verification.json`。
通过已验收文件的字节/哈希与 JSON→URDF 对应关系复核后，使用现有发布器将相同文件
发布到产品根目录的 `20260916_105528_verified_165mm/`，并更新 `latest_partial_passed`。
原始中断会话、离线验收目录与独立进程检查目录全部保留；没有把原始 PAUSED 状态改为
COMPLETE。正式目录中的 `release_origin.json` 记录已验收来源；完整结论和实际采集范围
见 `20260916_105528/verification_summary.json`。当前布局直接测量拇指和小指共 5 个关节，
食指、中指、无名指共 6 个关节的运动曲线按原配置从小指迁移，不宣称这些关节独立实测。
## 第十八轮评估：被动关节由 JSON 提供角度，消除线性导出冲突
用户指出拇指 IP 在修正 URDF 中明显弯得更小。复核确认，原 URDF 的线性倍率为 1.86，
旧修正产物的倍率为 1.16556243135602，而实测 JSON 的 IP/pitch 比例随行程约从 1.71
变化到 1.94。父子下限分别为 -0.00172483145889436、-0.0020103987491093 rad，
旧“共同零点＋整段不越界”约束把这两个小负端点的比值变成全行程倍率上限。
这不是非线性 JSON 的拟合失败，而是将独立实测曲线再次约束为一条直线的导出设计问题。
旧完整 JSON＋自定义 FK 验收虽通过，标准 ROS mimic 仍会覆盖被动角，因此显示与验收不一致。
评估后采用统一的 JSON 角度来源规则，不修补 O6 的某个倍率：
- 新 v3 修正 URDF 移除原被动关节的 `<mimic>`，全部关节角由 JSON 明确给出。
父子链、转轴、连杆、零位与限位保持原有受检语义；原被动关节没有新增实机驱动通道。
- `urdf_correction` 升为版本 2，保存 `joint_angle_source: calibration_json`、
`passive_joint_sources` 和明确的删除操作。只有完整覆盖运动关节、来源与原 URDF 一致、
被动关节与来源关节共享 SDK 通道时，才能授权移除；其他数值编辑仍使用 Profile 原授权。
导出版本与采集配置分开，未修改 Profile 指纹或原始记录来绕过保护。
- JSON 先保存，重新读取后重建 URDF；普通 FK、角度/位置验证及原始图像验收使用该最终文件。
标准 `robot_state_publisher` 可直接接收全部关节状态，不需要忽略 mimic 的定制算法。
- 线性拟合仅保留报告诊断，标记 `diagnostic_only_not_exported`，不再按限位限制拟合倍率，
不参与导出和运行角度。旧 correction v1 的精确重建和旧读取方式继续兼容。
专项回归覆盖微小负限位、非线性行程、普通 FK、JSON 重建字节一致性、错通道、缺失曲线、
非法来源、未经授权删除与拓扑改动拒绝。真实 ROS TF 测试在独立命名空间验证三个弯曲点，
未连接 SDK；末节角度不再被覆盖。O6/G20/L6/O12 的合成完整收尾、独立图像验收、迁移来源、
双向映射及旧格式检查通过；这不是其他三型号的新增实机精度证明。
已构建安装，并用 `20260916_105528/raw_samples.jsonl` 的完整 16.5 mm Tag 实采数据
执行正式离线回放与发布，退出码 0。新目录为
`calibration_output/O6_RIGHT_001/20260916_123347_json_driven_165mm/`，
`latest_partial_passed` 已更新。原来的角度、位置和原始图像门限全部通过，没有放宽。
对新旧发布文件逐项比较：11 个关节的全部指令表、反馈映射、零位和限位不变；
URDF 的结构差异仅为移除 5 个线性 mimic。重新读取新 JSON 重建得到逐字节相同的 URDF。
相同输入的拇指结果如下（单位：度）：
| SDK 指令 | CMC pitch | 旧 URDF 线性 IP | 新 JSON＋URDF IP |
| --- | ---: | ---: | ---: |
| 192 | 9.3781 | 10.9307 | 16.0523 |
| 128 | 18.8914 | 22.0191 | 33.0907 |
| 64 | 27.2887 | 31.8067 | 49.6742 |
| 0 | 34.2391 | 39.9078 | 66.3515 |
复核脚本与结果保存在原会话的 `verify_json_driven_export.py`、
`json_driven_export_verification.json`，正式回放日志为 `json_driven_replay.log`。
旧发布三文件的哈希未变。本次没有重新运动机械手，沿用原 5 个独立观测关节、6 个迁移关节
及 CAD 零位假设的精度边界。使用新产物时须通过 JSON 同时驱动全部关节；单独拖动 URDF
父关节滑条不再自动联动末节，外部仿真也须移除与 JSON 冲突的旧线性更新逻辑。
## 第十九轮评估：按用户要求保留标准 mimic 近似联动
用户明确使用 `https://viewer.robotsfan.com/` 直接预览 URDF，并进一步说明：
非线性关节也必须保留 `<mimic>` 自动联动，单独 URDF 尽量接近实机；
需要实测非线性结果时再由标定 JSON 查找表控制。上一轮删除 mimic 的正式导出方向
不符合这一使用要求。已核对网站当前构建及官方 URDFAdapter：滑条直接设置 URDF 关节，
没有本项目标定 JSON 的读取逻辑，不能期望它自动获得 JSON 中的非线性曲线。
### 方案评估与实现
标准 mimic 使用 `q_child = a*q_parent+b`。将 `b` 强制为 0，再约束父子全行程，
使拇指的两个微小负端点决定全局倍率上限，这是旧版 1.16556 倍率失真的原因。
仅恢复 `<mimic>`、手填倍率或扩大限位都不能解决模型与约束之间的矛盾。
现在使用两端子关节角度作为拟合变量：
- 父关节输出范围 `[p0,p1]` 保持不变，拟合 `c0=q_child(p0)`、`c1=q_child(p1)`。
- 两个变量分别受子关节原输出限位约束；区间内的线性插值自然不会越界。
- 由 `a=(c1-c0)/(p1-p0)`、`b=c0-a*p0` 导出标准 mimic 倍率和偏移。
- 沿用前三轮成对视觉样本与稳健损失拟合，第四轮只验证；不改变 SDK→角度表、
坐标零位、Tag 安装、相机参数或训练支持的限位。
- `b` 是线性近似在零点的误差，不是新测出的机械零位。报告同时保存
`baseline_error_rad`、`physical_zero_correction: false` 和实际残差。
生产 `urdf_correction` 使用版本 3，并声明
`mimic_policy: range_bounded_affine_approximation`、`joint_angle_source: calibration_json`。
仍先保存 JSON，再读取生成 URDF。所有原 mimic 来源与 XML 拓扑保留；
数值修改继续使用已有 Profile 授权，没有更改采集指纹。版本 1、2 产物保留重建兼容，
不原地覆盖。完整 JSON 回放显式使用非线性被动角，URDF 单独回放使用 mimic；
两者分别报告精度，线性近似不能冒充 JSON 的实测精度。
专项检查覆盖微小负限位不压低全程倍率、正负运动方向、全行程限位、留出样本禁止参与拟合、
重建字节一致性及缺失/降级导出规则拒绝。真实标准 ROS TF 检查只给父关节角度，
验证被动关节按导出 mimic 联动；历史无 mimic 版本的独立关节测试也继续通过。
### O6 实采数据复验与发布结果
相关 121 项检查通过，覆盖四型号合成产物、迁移来源、双向表、历史格式及标准 ROS 联动。
构建安装后，以同一份完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0，
生成并发布 `20260916_124718_mimic_json_165mm/`，更新 `latest_partial_passed`。
JSON＋URDF 的角度、位置及独立原始图像验收通过，原精度门限不变。
新旧三版的全部 11 关节指令/反馈曲线、几何零位、限位逐项一致；5 组 mimic 来源与
原始 CAD 相同。拇指 IP 倍率为 **1.86604745134131**，偏移为
**0.00120821859875384 rad（0.06923°）**。小指及三个迁移末节的倍率为
**0.870364582419037**，偏移为 **0.0348981257618717 rad（1.99952°）**。
这些偏移是近似关系在零点的残差，完整 JSON 的 baseline 仍为 0。
| SDK 指令 | CMC pitch | 旧 URDF IP | 新 mimic IP | JSON IP |
| --- | ---: | ---: | ---: | ---: |
| 255 | 0.0000° | 0.0000° | 0.0692° | 0.0000° |
| 192 | 9.3781° | 10.9307° | 17.5691° | 16.0523° |
| 128 | 18.8914° | 22.0191° | 35.3214° | 33.0907° |
| 64 | 27.2887° | 31.8067° | 50.9912° | 49.6742° |
| 0 | 34.2391° | 39.9078° | 63.9611° | 66.3515° |
相对 JSON 的 256 项指令网格，拇指线性近似最大差约 **2.3905°**，小指及对应迁移末节
最大差约 **4.1965°**。这是两个表示之间的差异，不是新增的实机独立精度测量；
独立留出结果分别保存在 manifest 的 `full_json_urdf_holdout` 和 `urdf_mimic_approximation`。
因此网站中的联动预览仍是近似，需要实测非线性结果时必须使用 JSON。
另行读取新 JSON 重建得到逐字节相同的 URDF。所有旧发布文件哈希不变，
没有重新运动机械手，也没有改写旧采集结果。复核脚本、结果及正式回放日志分别为
原始会话下的 `verify_affine_mimic_export.py`、`affine_mimic_export_verification.json`、
`affine_mimic_replay.log`。本次仍只独立观测拇指、小指 5 个关节，其他 6 个关节沿用原迁移关系。
## 第二十轮评估：全型号保留原始 mimic，实测非线性由 JSON 提供
用户确认统一采用原始 mimic 作为基础联动，暂时取消正式标定中的 mimic 参数优化。
该规则替代第十九轮新产物的拟合策略；历史产物保持原样及原有读取语义。
### 全型号依据与边界
检查 L6、O6、O12、G20 的左右手原始 URDF，以及当前四套右手产品配置。
L6/O6 部分曲线从小指迁移，O12 包含两级 mimic 链和无名指曲线迁移，G20 配置覆盖全部实测关节。
左右手系数不能互相套用：O6 拇指左右分别为 2.29/1.86，G20 为 1.02/1.03。
原始文件也不等于限位自洽：按原始右手父关节全行程，L6 拇指约超出子限位 3.6795°，
O12 无名指中节约 0.7162°，G20 四个 DIP 约 0.4297°。这些是文件内冲突，不是硬件越限结论。
O6 的已声明实测范围策略不能自动授予其他型号放宽 CAD 硬限位的权利。
### 统一实现
- 新 correction schema v4：`mimic_policy=preserve_source_mimic`，`joint_angle_source=calibration_json`。
- measured 正式拟合及收尾不再调用 mimic 参数优化，原 mimic 来源、倍率、偏移及省略属性全部保留。
- `source_mimic.py` 统一计算负倍率、非零偏移及串联关系的可达范围，不含型号分支。
- `mimic_envelopes` 单独记录基础范围、联动可达范围和最小导出包络；不会改写实测范围证据。
- 包络扩展只适用于已有实测范围策略；默认 CAD 硬限位和独立安全限位继续生效，冲突明确报告关节及范围。
- 零位仍修正 origin。原始 mimic 只作为修正坐标中的预览近似，精确 FK 使用 JSON 提供的各关节角。
- 新 JSON 禁止请求 mimic 参数修改或删除；最终验收对照受保护原文件核对 mimic 元素和可信范围证据。
- correction v1/v2/v3 继续兼容重建与读取，既有原始记录和配置指纹不变。
### 验证与 O6 发布
两组相关检查分别 **68 项、72 项通过，共 140 项**。覆盖八份左右手源文件、负倍率、零倍率、
非零偏移、多级链、独立安全界限、非法 JSON 修正、历史版本重建及标准 ROS 的父关节联动。
四型号合成完整流水线同时验证：禁止调用 mimic 优化函数，仍能完成 JSON→URDF→独立角点验收→发布→读取。
这些合成检查不代表其他型号已经完成实机标定。
构建后以完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0，发布：
`20260916_134154_source_mimic_165mm/`，`latest_partial_passed` 指向该目录。
5 个 mimic 元素与原始 O6 URDF 一致，拇指 1.86、其余四个末节 0.89，offset 全为 0。
所有 11 关节指令/反馈查找表、几何零位、实测范围证据与前三版逐项一致，旧产物哈希不变。
相对上一版 URDF，除 mimic 参数外仅 IP 下限改变：
`−0.0020103987491093 → −0.00320818651354352 rad`，约增加 **0.068628°** 的联动兼容范围。
SDK 指令 0 时，拇指 pitch 为 34.2391°，原始 mimic 预览 IP 为 63.6848°，JSON IP 为 66.3515°。
完整 JSON＋URDF 的拇指 IP 独立角度 P95 为 0.43466°、位置 P95 为 2.11692 mm；
小指 DIP 分别为 1.87341°、1.94512 mm，原精度门限未改。
独立图像验收的门限、样本数量和通过结论保持不变；重复数值优化的末位浮点值及诊断项可能变化。
读取保存的 JSON 重建得到逐字节相同 URDF；本次没有重新运动机械手。
复核材料位于原始会话目录：`verify_source_mimic_export.py`、
`source_mimic_export_verification.json`、`source_mimic_replay.log`。
当前 8 Tag 布局仍只独立测量 5 个关节，其余 6 个按既有声明迁移；在线采集曾有 OOM 的边界保持原记录。
src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md
+93 -10
View File
@@ -1,6 +1,80 @@
# 按关节零位与实测/复制 JSON v3 实施记录
日期：2026-09-11。范围：`linkerhand_calibration`。
更新：2026-09-15。范围：`linkerhand_calibration`。
## 当前收敛结果
- G20、L6、O6、O12 右手共用四轮连续扫描，随后执行独立的稳态指令映射训练与验证。原精度门限保留。
- 正式入口在启动硬件前检查运动覆盖与绝对零位来源。O12 的 `thumb_mcp`、`pinky_pip`、`ring_pip`
按用户确认沿用原始 URDF 零位，记录为 `assumed_source_cad_zero`；运动继续实测或按声明复制。
- 收尾先保存 JSON，再由 `runtime/artifacts/urdf_from_json.py` 读取它重建 URDF。
发布器检查修正元数据能否逐字节重建最终文件；可独立使用 `rebuild_calibrated_urdf` 命令。
- 四型号的合成原始观测均已通过实际拟合、文件生成、验收、发布及读取器加载。
合成夹具使用明确的几何真值与范围，这不代表四种实机均完成验证。
- O6 会话 `20260915_134211` 实采 36/36 单元一次通过，无重扫、无采集中断，采集耗时 412.35 秒。
同一原始日志已生成候选 JSON 与修正 URDF，独立 JSON 重建得到相同字节；整链几何验收仍失败，未发布。
最大 Tag 位置 P95 为 15.32 mm，小指末节姿态 P95 为 7.76°，不能称为整手精度合格。
- 随后用棋盘格重新测得 O6 三相机外参：两组各 15 个姿态，最大联合重投影 RMS 为 0.928 px，
最大重复平移差 0.341 mm、旋转差 0.043°，通过原门限。新配置为
`config/o6_three_camera_extrinsics_20260915.yaml`；旧数据保留原外参身份，不混用。
- 新外参会话 `20260915_144514` 完成拇指 24/36 单元后，小指准备求解有两个候选耗尽 150 次迭代。
同一批 120 帧离线复现确认是内部 LSMR 线性求解精度不足；调整 `atol/btol` 为 `1e-10` 后，
四候选均在原 150 次预算内收敛。外层收敛条件、分支统计和物理精度门限未变。
`test_recorded_pinky_alternatives_converge_within_original_budget` 保存该实采回归；相关 34 项测试通过。
- 恢复会话 `20260915_145437` 暴露旧日志在运动回调中校验导致的反馈超时报错：离线复现读取约
1.41 秒、完整凭据/单元校验约 13.88 秒。`PreparedResume` 将静态校验移到回调启动前；
当前固定基准与逐关节安装仍在现场独立验证。后续 `20260915_145748` 暴露一次性导入大日志
仍会阻塞；改为每个控制周期最多导入 64 条记录，全部持久化后才确认复用。
恢复相关 73 项测试通过，包含导入期间持续处理反馈、接收中止及落盘前不得跳过单元；原 1 秒反馈保护未变。
- 图像模型零位恢复还需同步导入该零位的 `pnp_candidate_frame`，不能只复制 `joint_zero_sample`。
已修复该来源闭合问题；真实中断恢复会话离线核对发现恰好漏掉 10 帧零位图像，按新导入逻辑保留后
原凭据检查通过。原始日志未修改。相关 74 项测试通过，包含连续两次恢复的零位图像回放。
- 新外参完整会话 `20260915_150015` 已完成 36/36 单元，采集中无暂停，两次自动重扫均通过。
生成的候选 JSON 可独立重建出字节相同的修正 URDF，并通过标准 ROS URDF 加载。
整链训练阶段的刚性 Tag 安装检查仍失败：拇指 IP 位置 P95 为 9.73 mm，小指 DIP 姿态 P95 为 9.40°。
这不是第四轮整链精度通过的结果，未生成发布 manifest，原始 URDF 未覆盖。
用户已确认 ID4/ID5 安装在平整硬片上，Tag 黑框为 16×16 mm，棋盘格单格为 27 mm。
尚不能据这些残差判定硬件故障。
- 随后完成三相机原始棋盘格内参核验：正面 34、侧面 25、顶部 24 个不同姿态；
每第 4 张预先留作独立验证。验证重投影 RMS 从旧内参的 0.880/0.955/0.484 px，
降至 0.276/0.465/0.317 px。新内参保存在 `config/o6_camera_intrinsics_20260915/`。
配套外参使用正面/侧面 25 组同步观测和补采的正面/顶部 15 组同步观测，均无剔除；
最大 RMS 0.575 px、旋转重复差 0.024°、平移重复差 0.269 mm，通过原门限。
O6 产品配置已指向 `config/o6_three_camera_extrinsics_intrinsics_20260915.yaml`，
并通过无运动的配置及受保护输入校验。旧相机文件与旧整手记录保留原身份。
原图、角点、训练/验证划分、拟合报告及导出哈希保存在
`calibration_output/O6_RIGHT_001/camera_intrinsics_audit_20260915/`。
相机核验通过后仍须新采整手数据，不能将旧整手会话改挂新相机参数发布。
- 新相机完整重采 `20260915_154743` 的拇指 24/36 单元全部完成，无重扫；小指准备的四候选
均在 26–36 次迭代内收敛，但候选区分证据不足，停在 `image_motion_families_not_distinguishable`。
随后发现统一 launch 将四型号受保护 Tag 配置中的 `detector.decimate=1.0` 强制覆盖为 `1.5`。
已移除这层未声明在产品配置内的覆盖，检测设置统一来自受保护 YAML；不修改候选区分或物理精度门限。
新回归实际展开 ROS 组件参数，修复前四型号均复现 `1.5 != 1.0`；修复后启动及 runner 的 55 项测试通过。
构建和 `git diff --check` 通过；旧降采样记录不混入新结果。
- 原分辨率完整重采 `20260915_160040` 已从三台运行节点读回 `detector.decimate=1.0`。
拇指 24/36 单元一次通过，无重扫；小指准备的 104 帧、四候选均在 21–35 次迭代内收敛，
仍因 `image_motion_families_not_distinguishable` 暂停，未生成本次完整候选 JSON/URDF。
离线回放中两个未分开的 ID4 姿态旋转差 P95 为 18.64°，不能当作同一解；
ID4 法线与相机光轴夹角在两解下分别约 6–8°、10–11°，运动中一直接近正视。
已请求保持相机、手掌和 ID3 不动，将 ID4 硬片垫斜后重新采集；这不是硬件故障结论。
`review/all_view_detections/` 保留本次后 200.7 秒的三机位检测与 SDK 消息。
其中侧摆期间同时看到 ID1/ID2 的匹配样本仅 9 帧、反馈范围 206–255，
不足以判断整个侧摆行程是否引起被动关节联动。所有精度门限保持原值。
- 上述修复后的四型号文件链、JSON 重建、恢复凭据和图像求解定向回归共 55 项通过（76.55 秒），
构建通过，`git diff --check` 通过；142 个原始 URDF/mesh 文件仍与 HEAD 相同。
本次生成测试见 `test_all_model_artifact_pipeline.py` 和 `test_json_urdf_rebuild.py`。
O6 最新采集及暂停证据在工作区 `calibration_output/O6_RIGHT_001/20260915_160040/review/`；
最近一次完成采集并生成候选文件的证据在 `calibration_output/O6_RIGHT_001/20260915_150015/review/`；
旧外参会话证据保留在 `calibration_output/O6_RIGHT_001/20260915_134211/review/`。
下方分批测试数字为各次改造的历史记录。
2026-09-15 最后一次全包回归覆盖 1424 项：1408 项直接通过、15 项旧夹具兼容问题、1 项缺少实测记录跳过。
这些失败项已修复，按失败清单重跑 15/15 通过，没有遗留失败；这是全量运行加失败项复测的结果。
O12 使用当前 CAD 假设的完整生成/验收测试另行通过。构建与四型号安装入口检查通过，
142 个原始 URDF/mesh 文件与 HEAD 内容一致。
完整日志和复测日志保存在上述 O6 `review/` 目录的 `linkerhand_final_full_tests.log`、
`linkerhand_final_rechecks.log`；`software_validation.json` 汇总了验证范围。
## 实现与坐标约定
@@ -38,16 +112,23 @@ Profile 必须声明完整零位指令和非零的最终到达动作；扫描起
JSON 关节来自自己的视觉样本，或 Profile 明确声明的对应实测来源；同一 SDK 通道允许绑定多个关节。
接收关节保留自己的 SDK 通道，复制来源的指令/反馈节点及角度，不再拟合。
接收关节没有独立零位帧和第四轮指标，报告标记 `independently_measured=false` 并引用来源证据。
正式单条指令曲线只用前三轮，约束 baseline=0；两条方向诊断曲线保留回差，共用同一参考。
第四轮分别检验两方向，不能用于重新拟合曲线、零位、Tag 安装或基座。
运动曲线只用前三轮，第四轮分别验证两方向；独立稳态分区用于指令映射训练与验证，约束 baseline=0。
O6 侧摆按已确认策略输出正反向指令曲线；其他关节沿用单值指令曲线。两方向共用同一参考。
验证数据不能用于重新拟合曲线、零位、Tag 安装或基座。
序列化只导出冻结结果；u8 必须实测支持 0–255 才能生成 256 项，rad 只在 `input_values` 范围内插值。
URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出 offset 精确为 0。
非线性 JSON 与 mimic 近似分别保存。近似误差不否决完整映射；机械范围、拓扑、授权字段及标准加载仍必须合法。
修正 URDF 保留各型号、本侧的原始 mimic，单独使用时提供近似联动；完整非线性角度由配套 JSON 提供。
`urdf_correction` v4 明确 `preserve_source_mimic`，正式标定不再拟合倍率或偏移，
不复制其他手指的 mimic 参数，也不因修正 origin 而重写原 mimic offset。准确 FK 显式使用 JSON 的各关节角。
`mimic_envelopes` 与实测 `motion_range_evidence` 分开：只在明确声明的实测范围策略下，
取实测范围与原始联动可达范围的最小包络；禁止突破独立安全界限及默认 CAD 硬界限。
多级 mimic 使用父关节真正的联动可达范围递推，不将其更宽的 JSON 运动范围继续放大。
JSON 重建与最终文件验收均核对原 mimic 元素、最小包络、通道一致性及非线性表。
旧 correction v1、移除 mimic 的 v2、拟合近似的 v3 保持原有重建/读取语义。
既无运动观测也无显式复制声明，或缺少绝对零位来源时，保留相对运动诊断并拒绝完整发布。
旧 mimic 及旧迁移字段不能自动授权 v3 复制，不用默认零偏补全未知几何。
接受原 CAD 末节零位的前提单独记录在 `cad_zero_assumptions`，不作为独立实测证据；
仅这些明确列出的末节取 δ=0，保持其自身 `origin`。O12 的被动 PIP 不在该假设范围。
仅明确列出的关节取 δ=0，保持其自身 `origin`；O12 的三个额外关节假设见上方当前结果。
## 验收与消费
@@ -55,7 +136,7 @@ URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出
1. 最终 JSON 对第四轮原始视觉角度；逐实测关节、逐方向检查；复制表另核验来源与一致性。
2. 最终 JSON＋修正 URDF 的完整 FK 对第四轮 Tag 位姿；显式使用所有被动角。
3. 相同主动映射配合标准 mimic 的损失；使用同一份训练冻结安装关系，只报告近似误差。
3. 相同主动映射配合标准 mimic 的独立近似损失，单独报告。
前两项精度门限仍为角度 MAE ≤1°、P95 ≤2°、最大 ≤3°，Tag 位置 P95 ≤3 mm。
三文件哈希、保护输入、观测覆盖和通道绑定一并检查；拟合失败不自动运动，取消后不能提交。
@@ -63,7 +144,8 @@ URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出
明确记录观测关节、复制关节和 CAD 零位假设。未观测接收关节不具有独立精度结论。
公共读取器和 `calibrated_joint_state_bridge` 支持 v3 的完整关节映射及历史 v1/v2 原语义。
完整 FK 使用 `independent_mimic_angles=True`。标准 mimic 加载器用于结构兼容检查，不能覆盖完整非线性关节状态。
完整 JSON 回放使用 `independent_mimic_angles=True`，防止线性近似覆盖实测被动角。
标准 `robot_state_publisher` 和普通 URDF 网页查看器提供线性联动预览，不据此宣称实测非线性精度。
本轮未修改 MuJoCo、相机时间同步和接触动力学；现有 MuJoCo 脚本尚不能视为支持 v3。
## 当前型号的发布边界
@@ -73,12 +155,13 @@ URDF mimic 使用前三轮成对实测角度拟合经过零点的直线，输出
| G20 | 21 / 21 个运动关节 | 无 | 五个末节的原 CAD 零位为明确假设 |
| L6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | MCP 零偏明确迁移；五个末节 CAD 零位为假设 |
| O6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | 同上 |
| O12 | 16 / 19 | 无名指 MCP/PIP/DIP ← 小指对应关节 | pinky_pip、ring_pip、thumb_mcp 零位仍须另行确定 |
| O12 | 16 / 19 | 无名指 MCP/PIP/DIP ← 小指对应关节 | pinky_pip、ring_pip、thumb_mcp 及末节沿用已确认的 CAD 零位假设 |
`--validate-only` 的 `independent_measurement_coverage` 返回具体关节列表。
配置检查通过表示合同可执行，不能解释为全关节精度通过。
上述运动策略已经覆盖各型号全部运动关节；完整发布仍要求实测来源验收通过、零位有来源且范围合法。
O12 当前仍被三项几何缺口阻止完整发布；L6/O12/G20 的现有 CAD mimic/限位冲突保留诊断，不能放宽限位掩盖。
四个产品配置的零位来源均已明确；L6/O12/G20 的原 CAD mimic/限位冲突保留诊断，
收尾保留限位约束内的线性近似；所有 JSON 查表角度和 URDF mimic 全行程都必须处于合法范围内。
现有 Tag 布局没有被补写成未经验证的新布局，新增参考动作也没有标为实机已验证。
## 原按关节零位改造的软件验证记录
src/linkerhand_calibration/README.md
+459 -26
View File
@@ -4,7 +4,7 @@ G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器
验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。
本包按观测关节冻结实机 baseline 零位，输出实测曲线及明确声明的复制曲线组成的 JSON v3，
以及线性 mimic 近似 URDF；报告区分独立实测、迁移和 CAD 零位假设。
以及保留线性 mimic 联动的修正 URDF；报告区分独立实测、迁移和 CAD 零位假设。
保留 CAD 连杆尺寸、轴位置、mesh、惯量。完整运动由 JSON 的全部关节角度配合修正 URDF 计算。
软件仿真通过不等于实机精度通过；当前原始 G20/L6/O12 存在需要确认的 mimic/限位冲突。
本轮实现与验证见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
@@ -27,10 +27,43 @@ ros2 run linkerhand_calibration calibrate_hand --config \
```
其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`。
AprilTag 检测参数直接读取产品指定的受保护 Tag YAML，统一启动文件不再覆盖其中的采样分辨率。
`calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。
`--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。
重新开始时先安全回基准，仍必须确保现场没有障碍物。
### JSON 到修正 URDF
统一收尾顺序为：拟合与独立角度验证 → 保存标定 JSON → 重新读取 JSON 修正原始 URDF → 文件回读与整链验证 → 发布。
JSON 的 `urdf_correction` 保存原始 URDF SHA256 和必要的关节字段修正，重建不依赖内存拟合对象。
原始 URDF 不被覆盖；关节拓扑、字段授权、完整 mimic 范围和 JSON 查表范围在文件安装前检查。
新 v3 使用 `urdf_correction.schema_version: 4`、`joint_angle_source: calibration_json`，
并记录 `mimic_policy: preserve_source_mimic`：所有型号保留本侧原始 `<mimic>` 元素，
不改来源、倍率、偏移或省略的默认属性；正式标定不再拟合 mimic 参数。JSON 保留各关节
非线性曲线及方向分支，精度验收使用 JSON 提供的全部关节角。父子链、轴和连杆不变，
被动关节与驱动关节仍绑定同一 SDK 通道，所有数值编辑仍按 Profile 授权。
`mimic_envelopes` 单独记录基础范围、原始联动可达范围和导出范围；在明确的实测范围策略下，
导出范围取二者的最小包络，实测范围证据与查找表不变。未声明该策略的 CAD 限位，以及
另行声明的硬件安全限位，仍是硬边界；冲突时报告具体关节和所需范围，不压低倍率或放宽容差。
关节 origin 按标定零位修正后，原始 mimic 作为输出坐标中的近似预览，不作为准确物理约束。
旧 correction v1、移除 mimic 的 v2、拟合线性近似的 v3 产物继续兼容读取与重建。
导出规则单独版本化，不改变采集配置指纹或旧原始记录，已发布产物不能原地覆盖。
已生成的 JSON 可单独重建对应 URDF，无需相机或 SDK：
```bash
ros2 run linkerhand_calibration rebuild_calibrated_urdf \
--config src/linkerhand_calibration/config/o6_right_product.yaml \
--calibration-json /path/to/calibration.json \
--output /path/to/new_corrected.urdf
```
输出路径必须尚不存在。没有 `urdf_correction` 的历史 JSON 不会被猜测或自动补全。
同一 JSON 和源 URDF 可逐字节重建相同修正文件；发布器会核对此项一致性，读取器要求相应验收记录。
历史无修正元数据的已发布文件继续沿用原有读取规则。
完整文件验收失败时保留候选 JSON／URDF 和诊断，便于复核；只有 `release_manifest.json`
和成功发布指针才代表通过，运行时读取器拒绝把未发布候选当作合格标定。
| Profile | SDK 位置通道 | 扫描任务 | Tag | 输出格式 |
| --- | ---: | ---: | ---: | --- |
| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v3，256 项指令查表 |
@@ -51,17 +84,179 @@ ros2 run linkerhand_calibration calibrate_hand --config \
复制参数不等于接收关节独立实测；接收关节仍须有明确的绝对零位来源及合法机械范围。
`zero.cad_zero_assumptions` 显式记录“实机 baseline 与原 CAD 末节零位一致”的使用前提，
相应 DIP（及拇指末节 IP）δ=0、自身 `origin` 不改。这是假设，不是视觉测得的绝对 CAD 朝向。
**O12 的 PIP 不采用此假设。** 当前 `pinky_pip`、`ring_pip` 和主动 `thumb_mcp` 仍缺绝对零位依据，
可以采集诊断数据，但这些缺口解决前不能发布完整 O12 v3 JSON/URDF。
按用户确认，O12 的 `pinky_pip`、`ring_pip` 和主动 `thumb_mcp` 也采用原始 URDF 零位作为 baseline 假设。
运动曲线仍按实际观测／明确复制关系生成，报告标记 `assumed_source_cad_zero`，不称为独立实测零偏。
正式入口在驱动硬件前检查运动覆盖及零位来源，缺失依据时直接指出缺项，不采完才发现无法发布。
启动检查分别列出 `unmeasured_joints`（未独立观测）、`unresolved_motion_joints`（也未声明复制）
及 `missing_baseline_geometry`。旧迁移字段单独存在不会自动授权新的 v3 曲线复制。
本次沿用现有 Tag 和避让布局声明延后零位动作，没有把新增动作或缺失 Tag 布局标为实机验证通过。
每任务四个往返周期：前三轮训练，第四轮独立验证。
每个方向先连续扫描，再单通道访问稳态点。训练每方向默认 9 点，并包含该通道 baseline，第四轮为交错点加端点；
每任务的运动模型采集四个往返，共八趟单程：前三轮训练，第四轮独立双向验证。
四个产品配置统一使用 `command_capture_mode: separate`：这八趟均为完整连续扫描，随后另做一组稳态映射训练往返和一组独立验证往返。
主扫描不逐点停顿；稳态点直接执行移动、稳定反馈、新图像采样，不重复等待两次到位。
稳态训练点由前三轮连续数据生成 `command_sampling_plan`：在原节点网格上寻找最少的
保留节点，各关节、各方向的预测曲线插值变化均不超过 0.25°；端点、基准、中间支撑点及
显式增加的非线性节点保留。缺少完整训练数据或曲线噪声过大时使用原网格。
这份计划只决定停在哪里，每个保留点的静态角度仍由新图像独立测量，不用连续运动角度
冒充静态角度。独立验证网格保持原样，不参与节点选择。计划及训练来源哈希随原始记录保存，
拟合、离线回放和断点恢复均重新核验；缺点、篡改或最终误差超限仍禁止发布。
上一段已经完成到位/稳定检查且下一准备目标完全相同时，可复用一秒内、反馈未变化的
完整保持姿态，省去重复准备等待；有实际位移、反馈变化或记录过期时执行原准备流程。
原训练网格每方向通常为 9 点，并保留行程内的 baseline；O6 小指额外包含 239，共 10 点。
新流程在这份网格上按上述规则精简训练停点。
验证使用交错点加端点（O6 小指包含 247），两类数据不混用。
日志中的运动轮编号为 0–3，独立指令映射训练和验证编号为 4、5；界面分别显示“稳态指令映射训练／独立验证”。
其他采用 `interleaved` 的配置仍在四轮各方向中穿插稳态点，并使用三轮稳态训练、第四轮稳态验证。
行进图像用于旋转/几何拟合，停稳后的新图像用于 SDK 指令映射；同一张图像不会复制成两类样本。
每点至少 3 个稳定同步图像，轨迹结束后最多等待 2 秒，不能用精确指令/反馈相等判断稳定。
关节零位冻结另需至少 10 张独立有效图像。`acquisition.joint_zero_timeout_seconds`
单独限定首张合格零位图像后的采集窗口，四个产品配置均为 5 秒；几何求解后的跟踪恢复仍最多等待 2 秒。
反馈不稳定会清空当前零位候选，但不会重置总采集期限；图像数量、1° 稳定性及分支证据要求不变。
稳定窗口使用每条新收到的 SDK 反馈，按消息时间戳去重，不再只取控制定时器看到的最新一条。
定时器延迟不能制造“独立反馈不足”；期间真实运动仍会使窗口失效。
候选清空时的 `steady_capture_reset` 保存反馈窗口和丢弃样本数量，便于区分抖动、断流和调度问题。
零位样本的 `capture_timing` 记录接收、历史快照和提交时间，用于检查处理延迟。
图像数量未齐时先返回缺样结果，不在控制定时器中反复校验全部姿态。
四轮及稳态采集完成、数据落盘后关闭图像采集；拟合只使用已保存的相机和参考证据。
之后的新图像和相机消息不再改变拟合输入，也不触发采集暂停；SDK 反馈、故障保护及操作者中止持续生效。
拟合或文件验收失败结束为 `FAILED`，保存 `failure_diagnostic.json` 中的失败阶段、原始数据路径和异常堆栈。
这类失败应先离线复算，不能仅因状态失败就重复四轮运动。运动采集期间的固定参考检查保持生效。
O6 的顶部 ID7 与正面 ID2 实际固定在同一末节，Profile 将 ID7 明确绑定到 `rh_thumb_distal`。
`thumb_yaw` 是它承担的侧摆观测角色，不是其安装连杆名称。整链回放包含该 Tag 上游的侧摆、弯曲和末节角度，
保持关节沿用记录的到达方向。新绑定改变了 Profile 哈希，旧会话不会被自动复用为新配置的通过结果。
固定 Tag 的 ID 解码成功不代表边框完整。独立的视觉边框检查进程按完全相同的图像时间戳配对整流图像与检测结果，
检查当前检测四边的黑白边界支持；部分遮挡产生的畸变四边形会被记为 `detection_quality_rejected`，
不能进入 PnP、关节样本或固定基准移动计数。配对缓存有界，不用邻近时间的图像替代。
检查使用当前检测四边形，清晰 Tag 发生真实位移仍进入原有 5 px／连续 10 帧保护。
控制进程只订阅 `detections_checked` 小体积观测；三路大图像的传输、配对和边框处理不占用 SDK 控制节点的回调队列。
ROS 宿主将准备几何求解和逐帧图像角度求解交给独立计算进程，避免与 SDK 反馈争用 Python 执行资源。
准备请求仍限时 120 秒；逐帧请求最多三个在途，超时或工作进程不可用的图像不生成有效样本。
工作进程只接收不可变图像/模型输入，没有 ROS 或 SDK 控制接口。返回后仍检查当前图像、跟踪版本和会话版本，
不能将迟到结果提交到下一运动段。`motion_solver_diagnostic.jsonl` 记录请求、计算耗时、线程 CPU 时间及提交结果。
当前任务需要的固定 Tag 无有效观测时，仍按原采样规则等待或暂停；其他机位的遮挡不误报为移动。
稳态点缺失与连续扫描不足统一在方向结束处理，最多同速重扫一次。
首次关节零位准备、避让及收尾属于独立的必要动作，不计入四个正式往返；不会每轮重新锁定零位。
分段加减速和停点仍有耗时，八趟行程不等于耗时恰好降为原来的三分之一。
新产品会话记录 `capture_schedule_version=unified_schedule_v3_separate_mapping`；旧 `interleaved` 配置仍记录 v2。
采集模式或保护配置不同的旧断点不与新流程拼接，
历史原始数据仍保留原语义供对应配置离线读取和分析。训练／验证身份与精度阈值保持不变。
字节拟合不再改写原始 command/feedback，也不将接近端点的读数伪装成 0/255。
## O6 右手拇指短程诊断
当前 O6 实物在 2026-09-16 复测的 Tag 黑色码区边长为 **16.5 mm**，棋盘格边长仍为
**27 mm**。O6 Profile、检测器配置和标定节点配置已同步采用 `0.0165 m`，产品配置记录
对应文件指纹。该数值属于这套实物标记，不是所有型号或所有打印批次的统一尺寸。
更换标记时应实测黑框，并同步维护上述配置；旧 16 mm 会话保留原尺寸记录，不作为
新尺寸的恢复断点。Tag 尺寸修正不改变 JSON 中非线性运动曲线与修正 URDF 的配合方式。
本套 O6 的 2026-09-16 实采数据已生成通过验收的产物，目录为
`calibration_output/O6_RIGHT_001/20260916_134154_source_mimic_165mm/`，
`latest_partial_passed` 已指向该目录。JSON 和修正 URDF 应配套使用；当前 8 Tag 布局
直接测量拇指、小指，其余三指沿用配置中的曲线迁移关系。采集完成后的在线收尾曾发生
系统 OOM，最终产物由完整原始数据的正式离线流程验收；日志读取内存优化另经独立进程
验证，不能将此记录描述为在线全过程无中断。详细证据见 `CALIBRATION_PIPELINE_REVIEW.md`。
此目录用同一份原始数据重新验收，5 组 mimic 的完整元素与原始 URDF 一致：
拇指倍率 **1.86**、其他四个末节 **0.89**，偏移均为 **0**。
11 个关节的指令/反馈查找表及几何零位保持不变；仅拇指 IP 的 URDF 下限从
−0.115187° 调整到 −0.183816°，覆盖原始联动所需范围，实测范围证据仍为原值。
拇指 SDK 指令 0 时，IP 的 URDF 预览为 **63.68°**，JSON 为 **66.35°**；
这是线性预览与实测非线性的区别。完整 JSON＋URDF 和独立原始图像继续满足原精度门限。
`20260916_123347_json_driven_165mm/`（移除 mimic）及
`20260916_124718_mimic_json_165mm/`（拟合倍率和偏移）为历史版本，已被本次发布替代。
旧目录均保留；始终使用同一 manifest 指定的 JSON、报告和 URDF。
定位 Tag 姿态跳变时，可沿用相同产品配置添加诊断选项：
```bash
ros2 run linkerhand_calibration calibrate_hand \
--config src/linkerhand_calibration/config/o6_right_product.yaml \
--diagnostic-thumb
```
诊断选择原 Profile 中的 `thumb_yaw_top` 和 `thumb_pitch_ip_front`，各采一次往返，
观测拇指 CMC yaw、CMC pitch、IP 三个关节。保留原速度、完整扫描行程、准备/避让、
固定基准锁定、按关节尝试零位确认和安全收尾；其余手指不执行扫描任务，但开始时仍会安全回到 baseline。
本次诊断只启动正面和顶部机位：开始时一次锁定正面 ID0、顶部 ID6；运动观测使用顶部 ID6/ID7、正面 ID0/ID1/ID2。
侧面不参与本次诊断，ID3 缺失或双解、侧面相机未连接均不会阻塞；因此无需为拇指诊断调整 ID3 或改动相机外参。
范围由本次全部任务的主/辅观测依赖统一确定，启动、ROS 订阅、设备检查、基准锁定、指纹和进度使用同一范围。
它不会随当前任务切换而重新锁定；正面和顶部的配置变化、固定基准漂移及所需 Tag 数据不足仍按原策略处理。
正式全手标定继续使用 Profile 声明的全部机位和固定基准，外参文件及保护输入的完整校验保持不变。
“短程”缩短的是任务和轮次数，不截断需要复现异常的运动行程。
本模式跳过稳态映射采样与三轮训练/第四轮验收，收尾后显示 `DIAGNOSTIC_COMPLETE`
并正常退出。诊断按当前运动 Tag 的原始图像覆盖推进，持续姿态歧义不阻断原始候选采集；
缺少当前运动 Tag 角点或同步反馈导致方向覆盖不足时，仍最多同速重扫一次，严重故障和中止行为不变。
归零准备证据为 `unresolved` 时，仅诊断模式允许继续收集原始观测：必须先到达 Profile 声明的零位指令，
SDK 反馈稳定，当前运动 Tag 具有足量、带会话和运动版本的图像，才保存独立的 `diagnostic_joint_zero_attempt`。
它表示访问并观察了声明姿态，不表示已建立关节零位；不写入 `zero_references`，方向切换及重扫不重新归零。
该组关节后续仅保存原始角点和候选，即使普通跟踪恢复，也不能生成 `joint_sample` 或伪造有效角度。
缺少零位原始图像、SDK 不稳定、固定基准移动及已冻结参考冲突仍按现有策略暂停；正式标定仍须先确认分支和冻结零位。
会话目录保存 `raw_samples.jsonl`（含角点和 IPPE 候选）、实际成功冻结的关节零位与 `diagnostic_capture.json`。
`diagnostic_observation_frame` 在零位尝试和扫描阶段单独记录各帧候选，包括未接受的姿态，不能当作关节角度样本。
报告 `unit_quality` 分别记录原始观测覆盖与 `accepted_pose_quality` 可信姿态覆盖；
`joint_zero_status`、`missing_joint_zero_references` 列出各关节真实零位状态，`motion_initialization_summary` 保留各候选的关节残差。
进度显示原始观测覆盖，并单列可信姿态数量和未确认关节；暂停时保持“已暂停”，不会被零位动作名称覆盖。
可信姿态不足会在退出时明确提示。相邻帧姿态跳变和图像身份用于定位问题，诊断完成不代表标定精度通过。
每方向的 `observation_timing` 分别统计原始/可见/同步图像数、缺命令与缺反馈图像数、最大图像及同步观测间隔；
缺命令与缺反馈可以发生在同一帧，不能相加当作总丢帧数。少于两帧的间隔记为 `null`。
方向失败时这些信息同时进入暂停诊断。反馈跨度 100% 仅表示两端都有观测，不代表中间没有缺口。
原始帧的 `timing` 保存回调接收、历史快照及最新命令/反馈时间戳，用于区分消息延迟与状态匹配问题；
接收到快照的时间包含线程调度和锁等待，不能单独解释为写盘耗时。未匹配反馈时同步误差为 `null`，不会显示为 0 ms。
可额外加 `--record-bag` 保存现有录制话题；仅检查入口与配置可加 `--validate-only`，不会启动硬件。
诊断始终完整新采，不复用断点，不拟合或发布标定 JSON/URDF，也不更新发布指针。
原始会话带有诊断标记，正式离线拟合和断点发现会拒绝将其用作正式标定数据。
当前选项只开放给 O6 右手；其他型号仍走原来的正式流程。原始 URDF、Profile 和设备配置无需修改。
诊断任务预设在 `profiles/diagnostics.py` 声明，运行时只选择既有 Profile 任务，复用公共调度与采集组件。
诊断采集完成后，runner 先关闭相机和 SDK 进程，再自动运行公共图像几何分析，生成独立的
`diagnostic_analysis.json` 和可直接阅读的 `diagnostic_analysis.md`。耗时优化不进入实时状态锁或相机回调。
分析按任务、机位、方向和尝试分别保留原始图像身份，比较所有候选族的三维 PnP 残差、深度误差比例、
图像域关节链拟合以及隔帧和前后区间验证。验证时几何参数冻结，只从当前图像反解几何角度，不使用 SDK 角度映射。
图像误差小不等于毫米精度通过，优化收敛也不代表分支唯一；分析结果不能写回跟踪器、零位或正式标定产物。
未采到的方向和旧会话缺少的归零前证据会明确列出，不拼接不同会话，不使用第四轮重新拟合。
图像分析失败或被取消时，已完成的采集及原始文件仍保留，不会改标成 SDK 故障。
已保存的数据可直接离线复算，无需启动相机或重复运动：
```bash
ros2 run linkerhand_calibration analyze_calibration_capture \
--config src/linkerhand_calibration/config/o6_right_product.yaml \
--raw calibration_output/O6_RIGHT_001/20260914_192637/raw_samples.jsonl \
--output calibration_output/O6_RIGHT_001/20260914_192637/image_review
```
这些修改解决诊断采集被未确认零位阻塞及报告不完整的问题。现有真实 IP 数据仍有分段几何不一致，
正式分支门槛、拟合器、URDF 修正和独立精度验收保持原要求；软件回归不能替代实机精度结论。
### O6 右手 yaw 方向差保持诊断
需要区分“到达指令后仍在缓慢稳定”和“保持后仍存在方向差”时，使用独立选项：
```bash
ros2 run linkerhand_calibration calibrate_hand \
--config src/linkerhand_calibration/config/o6_right_product.yaml \
--diagnostic-yaw-hold
```
此模式仅选择 `thumb_yaw_top`，只启动顶部机位，使用固定基准 ID6 和运动 Tag ID7。
每次 Start 都重新采集会话基准并尝试确认 yaw 零位，不复用旧会话；同一会话的两个方向共用首次冻结零位，
不会分别归零。未能确认零位时按诊断规则保留原始观测，并明确标记无法生成可信角度。
在原速度和行程下执行一次往返。每个方向扫描后先回到该方向的起点，再沿同一方向访问三个保持点：
递减方向为 `255 → 192 → 128 → 64`，递增方向为 `0 → 64 → 128 → 192`。
每个保持点在指令轨迹完成后至少保持 10 秒，两方向合计六次保持；反馈确认可能使实际等待稍长。
保持过程继续记录图像角点、候选、实际指令和同步反馈，用于比较到达后的角度随时间变化及双向最终差异。
保持阶段的原始观测单独标记，不计入连续扫描的覆盖统计；遮挡、设备故障、中止及安全收尾仍沿用公共策略。
`--diagnostic-yaw-hold` 与 `--diagnostic-thumb` 互斥，不能同时使用离线拟合、发布或禁用运动选项。
可加 `--validate-only` 只检查入口与配置，或加 `--record-bag` 留存现有录制话题。
结果保存在 `raw_samples.jsonl` 和 `diagnostic_capture.json`，记录保持指令及时间；
不拟合或发布标定 JSON/URDF，不形成正式断点，也不算精度验收。
本诊断用于查清实机稳定时间和方向差，不改变最终单值 `angle_rad` 的验收要求。
## 启动前相机检查
`--validate-only` 检查相机配置文件、序列号、内外参指纹和已有外参质量记录，不采集现场图像。
@@ -97,11 +292,26 @@ ros2 run linkerhand_calibration calibrate_hand --config \
- 相机相对位置改变时须重新确认/标定外参；仅重新锁定掌心 Tag 不能替代外参标定。
- 固定 Tag 被避让遮住时可用已锁定参考继续采集；软件不能保证检测完全不可见对象的移动。
运动 Tag 的刚性和几何一致性仍须通过最终独立验证。
- 断点版本为 `unified_engine_v5_joint_zero`，不复用旧策略数据。
- 采集断点版本为 `unified_engine_v8_all_view_images`；另以 `pose_tracking_policy_version=confirmed_branch_v8_training_model_selection`
记录姿态分支策略。缺少此字段或版本不同的旧断点不能自动或手工恢复到新会话，历史文件保持原样供离线诊断。
首先检查配置哈希与整场固定参考；各关节轮到时，在同一完整保持姿态核对原零位与父子 Tag 位姿
（5 px、2°、5 mm），通过后才导入对应已完成扫描单元，并保留原零位记录。
整场参考不匹配则放弃旧数据；逐关节安装验证失败则暂停，禁止在旧样本上新建零位拼接。
排除原因后用 `--no-resume` 开始新会话。旧会话文件不修改。
旧日志的读取、凭据回放与单元质量检查在实时回调启动前完成，不占用运动/反馈回调。
现场验证通过后，每个控制周期最多导入 64 条历史记录；全部落盘后才跳过对应单元。
导入期间持续处理反馈、固定基准监测和中止，原 1 秒反馈超时保护不变。
连续图像、候选和样本按图像批量追加并刷新到系统缓存，避免持有控制状态锁逐条 `fsync` 阻塞反馈和运动定时器。
固定参考、关节零位、方向完成和暂停记录仍强制同步到磁盘；拟合、报告哈希、发布和关闭前另有同步屏障。
断点只复用通过且持久化的完整方向，意外断电时未完成方向不能拼入旧数据；同步失败不得发布。
此存储策略不改变 50 ms 图像/状态配对门限、方向覆盖门限、拟合公式或精度验收条件。
O6/L6/G20 标定读取 SDK 的 `*_hand_state_timed` 反馈接口；位置与每个通道的 CAN 接收时间
一同保存。G20 五指的分帧反馈分别插值到图像时间，重复发布的缓存不能刷新反馈超时或增加
稳态独立样本。标准 `JointState` 仍可供其他程序使用，其时间戳保留测量时间。
`sdk_feedback_sample` 原始记录支持复查时间配对；旧版本只保存发布时间，不能事后补成真实
CAN 测量时间，也不能与新版断点混用。O12 继续使用独立 HCAN 同步读取接口。
共同坐标约定：`q_CAD = q_output + delta`；每个关节在自身冻结 baseline 时 `q_output=0`。
局部零位通过采样时的完整保持姿态和运动链进入同一掌部参考。固定 Tag 不提供未知末端连杆的 CAD 朝向。
@@ -113,6 +323,8 @@ O12 避让：小指和无名指到 Profile 的最大弯曲指令；标定食指
不要求反馈数值精确等于命令。SDK 指令最大值与实测 URDF 角度不是同一个量。
O12 使用 HCAN device 0/channel 0 的已锁定 vendor Python wheel，不检查 `can0`。
连接参数由本包的 `config/o12_sdk.yaml` 保存，并通过产品配置中的 SHA256 校验；
不依赖第三方 SDK 子仓库中未提交的示例 YAML 修改。
只读反馈桥可在第一次运动命令之前取得状态，不会发送零位命令来“激活回读”。
POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 错误位过热保护，不伪造温度。
字节 SDK 当前没有独立硬件故障遥测，状态会明确注明这一能力边界。
@@ -123,6 +335,9 @@ POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK
物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。
开始后相机投影参数变化也会拒绝继续使用混合坐标数据。
当前关节持续缺失零位观测会暂停并列出关节、机位和所需 Tag；尚未轮到的手指遮挡不阻塞当前任务。
零位和稳态点的反馈稳定窗口以最新独立反馈为终点，并保留跨越窗口起点的一条反馈。
控制定时器的调度抖动或重复读取同一反馈不会清空稳定观测；真实反馈波动或反馈过期仍会使当前窗口失效。
零位仍需至少 10 帧且姿态波动不超过 1°；拟合验收门限不变。
短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动
不会实时中断轨迹。无效视觉帧直接丢弃。
@@ -130,15 +345,150 @@ POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK
所有型号的独立 Tag 姿态筛选共用 `core/geometry/pnp.py` 中的近似同误差阈值，默认 `0.03 px`。
ROS 和离线采集使用相同默认值，型号 YAML 不重复声明该值。`1.5 px` 是图像质量拒绝阈值，
不能用它判断两种姿态是否同样可信；仅在误差差值不超过近似同误差阈值时使用时间连续性。
当另一分支的图像拟合明确更好时允许纠正旧分支。覆盖参数必须有限、非负且小于图像质量拒绝阈值。
这项修复不保证解决所有平面 Tag 双解，真实角点噪声、采集时序和最终运动精度仍需实测验证。
分支容差覆盖值必须有限、非负且小于图像质量拒绝阈值。
独立 Tag 首次接受姿态前，需要默认至少 5 帧、跨越至少 0.1 秒的图像证据确认。
近似同误差的候选只有旋转相差不超过 1°、平移相差不超过 1 mm 时才视为同一姿态；这些参数统一定义于
`core/geometry/tag_pose/parameters.py`，ROS 通过 `pnp_branch_confirmation_frames`、
`pnp_branch_confirmation_minimum_seconds`、`pnp_branch_equivalence_deg` 加载，离线采集使用相同默认值。
这只是图像分支的一致性确认，不能把多帧低重投影误差视为物理姿态真值。
以下独立图像跟踪规则保留用于预览和历史兼容；新会话的运动 Tag 首次零位还必须通过后述公共运动几何确认。
参考冻结前，明显更优但与历史姿态发生大跳变的新分支也必须经过确认，过渡帧不进入零位或拟合样本。
运动 Tag 参考冻结后，若原分支仍有连续、图像质量合格的候选，但另一候选的重投影误差明显更低，
该帧标为 `pose_branch_ambiguous` 并丢弃，内部保留原分支轨迹等待图像证据恢复；
不能仅凭另一候选连续数帧得分更高，就判定冻结参考损坏。不会切换分支、重建零位或把歧义帧送入拟合。
如果持续不存在符合原分支连续性约束的候选，仍在确认后暂停；长期失联后无法确认分支一致时也会明确拒绝。
正式标定方向结束仍检查可信样本、覆盖与稳态点，最多同速重扫一次；诊断模式另报原始观测与可信姿态覆盖。
需要更换分支或重新安装 Tag 时，必须重新开始新会话，
不能把旧冻结参考与新分支数据拼接。原有 0.03 px 容差、35° 跳变、75° 倾角及 1.5 px 重投影质量阈值保持不变。
真实角点噪声、采集时序和最终运动精度仍须通过独立实测验证。
### 首次零位前的公共运动几何确认
新生产会话复用 Profile 已声明的**最后一段归零准备运动**，在 `zero_approach` 中保存父子 Tag 的原始候选，
到 `joint_zero` 后先确认分支，再采集原有的稳定零位帧。没有增加扫描轮次、额外试探动作或型号专用求解器。
几何确认与 SDK 反馈稳定分别检查；几何通过不会提前开启零位采集窗口。
计算在状态锁外进行，期间持续接收 SDK 反馈并保持声明姿态；计算本身有 120 秒上限。
几何确认后，先在原有 2 秒观察窗口内等待恢复后的首个有效零位样本，再用
`joint_zero_timeout_seconds` 限定的独立窗口采齐稳定帧（字段默认 2 秒，四型号产品配置均为 5 秒）。
窗口不会无限延长。正式采集中，每组尚未冻结零位的关节最多自动恢复一次：可恢复的图像
准备失败重新执行该组原有的归零准备路径并采集全新图像；几何已经冻结但零位图像不足时，
保持原姿态和模型，只重新开启一次零位图像窗口。原始失败和恢复动作写入 `joint_zero_recovery`。
已冻结的零位、部分已经提交的多视角几何、保持姿态变化、设备故障不重新拟合；恢复后仍
不足时明确暂停，保留原有反馈、固定基准保护及用户中止能力。
中止、暂停或运动版本改变后，迟到的计算结果不能提交。
前面的避让段、预览、训练扫描和第四轮不参与首次确认。准备段同时核对其他通道的完整保持指令及反馈；
开始后不能改变相机、固定基准或 Tag 安装关系。
生产入口 `core/geometry/tag_pose/production_image_motion.py` 枚举 IPPE 候选家族，
使用父子转动副 `R = exp(theta × axis) R_ref`、`t = pivot + R × mount` 拟合原始角点的重投影残差。
稀疏线性子问题使用 `atol=btol=1e-10`，避免近似计算误差导致外层耗尽迭代预算；
外层收敛条件、默认 150 次迭代预算和各项精度门限保持不变，记录每个候选的求解状态及迭代次数。
父 Tag 本身运动时联合求解整条已声明的观测链；已冻结的上游几何不会重新拟合。
角度来自当前图像，不使用 SDK 到角度的先验、旧 mimic 或 CAD 零位假设选择分支。
单帧 PnP 深度噪声较大时，不再将其三维平移估计直接作为关节运动的真值。
当源 URDF 中两个转轴相邻且平行，并且观测 Tag 实际固定在对应连杆上时，
`profiles/observations.py` 提取零位修正后仍不变的平行关系和垂直轴距。
`CadImageHingeBundle` 消去三个多余几何变量，使准备模型遵守最终 URDF 保留的几何；
不修改原骨长/轴向、不增加权重或 CAD 零位假设。不满足条件的观测链使用原模型。
新冻结模型保存源 URDF 哈希和约束，最终处理从源文件重新推导核验；旧模型保持可读。
修改前的原始图像对照实验及适用边界见 [流程评估](CALIBRATION_PIPELINE_REVIEW.md)。
准备段至少需要 24 帧，训练与留出图像身份分离，默认各最多 72 帧。
训练帧取交错序列，未进入训练的图像才可留出；不因训练帧增加而复用验证图像。
局部几何可观测性检查计入逐帧角度的不确定性，轴线与转心的不确定度界分别不超过 3° 和 3 mm。
候选只按训练图像的平方误差选择；留出数据不用于选优，所选候选验证失败不能改选另一候选。
候选家族还须在准备段留出图像上区分：按运动角度分组，比较与拟合一致的平方误差损失，
差值为 `alternative_rms² - (candidate_rms + 0.03 px)²`，保留原像素容差。
同时通过单侧均值和符号秩检验，再对完整比较集采用 Holm 逐步校正，名义族错误水平保持 0.01。
`image_model_selection.py` 统一负责候选选择、同帧角色对齐、损失比较和多重校正；
诊断记录原始/校正后概率、选择策略和实际源 URDF 约束，失败时也保留这些证据。
缺失、未收敛或无法评估的替代家族不能被当作已排除；几何等价的家族才可合并。
这些检查是图像几何的条件性证据，不是独立毫米精度证明，不能替代第四轮 JSON／URDF 验收。
确认成功后冻结轴线、参考旋转、转心、安装关系及其来源图像。
`image_motion_projection.py` 后续每帧只反解当前图像角度，不重新拟合这些几何参数。
保留 1.5 px 重投影、75° 倾角及单帧角度不确定度检查；信息不足或角度仍有歧义时丢弃该帧，
按现有方向覆盖和最多一次同速重扫处理。35° 连续性和失联后的确认仍由公共 tracker 检查。
Tag 中心接近转轴本身不再是必然失败条件，是否可解取决于全部角点提供的几何信息。
准备段暂时缺帧或候选不确定时，每组尚未冻结零位的关节最多沿原归零路径补采一次；
已确认几何但零位图像短缺时，保持几何和姿态，只重新打开一次图像窗口。
两个恢复方式共享一次额度，保存 `joint_zero_recovery`，不延长成无限重试。
多机位已有部分几何冻结、保持姿态改变、设备故障和诊断采集不会触发重新拟合。
恢复后仍无法确认时不冻结零位、不发布；已有零位和旧数据不能被重新解释后拼接。
`motion_branch_observation` 保存准备段图像、角点、候选及 SDK 状态；`motion_branch_initialization` 保存各假设残差、
冻结模型、来源图像和内容哈希。新姿态来源为 `image_constrained_current_image`，保留原始 IPPE 候选，
同时记录 `frozen_image_model` 与 `image_model_sha256`。每个运动 Tag 的 `motion_evidence_id` 绑定首次证据，
`JointZeroReference.branch_references` 将它与零位、机位和分支版本关联。
正式读取与收尾校验这些关联，缺少证据、篡改来源或删除表头均不能降级为旧图像确认；
`motion_branch_provenance.json` 只证明证据链完整，不是精度证书。
恢复时共享来源只导入一次，关节零位及历史验证记录在该关节实际验证后导入，支持恢复期间再次中止。
正式回放用同一冻结模型复算同帧角点，核对父链、相机、Tag 尺寸及实际提交姿态，不重新拟合模型。
旧 `confirmed_branch_v1/v2/v3` 及 v4 三维候选约束记录仍按原语义读取，不能自动升级或恢复成新 v5 证据。
`motion_evidence.py` 的旧三维筛选入口保留供历史兼容和诊断。这里 v5 仅指姿态策略，最终标定 JSON 仍为 schema v3。
真实 O6 `20260914_175815` 的旧三维筛选只读回放中，yaw 与 pitch 分别存在唯一通过几何条件的家族；
但 pitch→IP 的四种父子组合全部超差，IP 转心 RMS 约 4.9–11.4 mm，完整拇指链仍为 `unresolved`。
这批图像来自旧零位冻结之后的单轮诊断扫描，只能用于反事实回归，不能代替新准备段证据、
覆盖旧零位或证明 JSON＋URDF 精度。
可复算摘要见[本次分支修复回放](../../calibration_output/O6_RIGHT_001/20260914_175815/motion_branch_fix_review/README.md)，
测试夹具同时保留“yaw/pitch 可分”和“IP 不得强行通过”两类断言。
后续 `20260914_200044` 的图像求解回放使用其真实归零准备段冻结模型，后续 838 帧可复算通过当前图像检查；
这仍是单轮诊断回放，不是完整四轮精度验收，也不会生成正式 JSON 或 URDF。
`20260915_104446` 正式实采中，拇指三个关节各冻结 10 帧零位并完成四轮，
pitch／IP 的旋转及指令单表子项均通过；yaw 旋转通过，但指令单表 MAE 为 1.0454°，超过 1°。
两场独立会话均观察到稳定方向差，该批观测即使允许每个指令任取一个输出角，MAE 下界仍超过 1°。
这不是通过重新拟合单表能够消除的误差；方向分支的诊断通过也不能冒充单表通过。
详见[拇指实测子项报告](../../calibration_output/O6_RIGHT_001/20260915_104446/motion_holdout_review/README.md)。
后续 `20260915_111401` yaw 保持实测在 64／128／192 各双向保持 10 秒；末 3 秒的方向差
分别为 2.2965°／2.7885°／2.1248°，每个窗口均有 90 帧可靠观测，同一目标的 SDK 反馈相同。
0.5–1 秒至末段的角度变化最多约 0.038°，延长等待没有消除方向差。
本轮仅启动顶部机位，不能据此确定具体机械原因或证明整手精度；继续保留单值 JSON 与原验收门槛。
复现脚本、时间曲线与来源哈希见[双向保持实测](../../calibration_output/O6_RIGHT_001/20260915_111401/yaw_hold_review/README.md)。
`20260915_104446` 会话在小指准备求解超时后停止，未完成整手空间／FK 验收、未发布 JSON／URDF。
小指同源准备段离线求解只需约 3.42 秒，但候选仍未达到统计区分要求；运行时超时来源尚待定位，
详见[小指准备段核查](../../calibration_output/O6_RIGHT_001/20260915_104446/side_preparation_review/README.md)。
后续会话另存 `motion_solver_diagnostic.jsonl`，记录请求来源哈希、版本、求解耗时和提交接受／拒绝，
以区分求解未返回与迟到结果被丢弃；该文件不参与测量授权，也不修改已冻结的原始记录。
此诊断接线在上述实采之后添加，不能补证该次超时原因。
固定基准另有明确的静止约束：锁定后始终使用首次冻结的相机局部姿态，将其投影到当前角点验证，
不因 IPPE 两支重投影误差排名反转而更换姿态或暂停。仍执行原有 1.5 px 重投影和 75° 倾角限制，
并保留当前 IPPE 候选与原有角点漂移监测。当前图像无法验证冻结姿态时丢弃依赖该基准的观测，
记录 `fixed_reference_pose_unverified`，不将几帧图像质量不合格判为不可恢复的姿态分支冲突。
恢复须默认至少 5 个合格帧且跨越至少 0.1 秒，始终核验同一冻结姿态，恢复完成前也禁止缓存兜底。
尚未使用该机位的动作可以继续；当前任务持续缺少有效数据时，仍按原有零位超时、稳态采样和最多一次同速重扫处理。
固定基准实际连续 10 帧漂移超过 5 px 仍使整场暂停，包括当前任务未使用的机位。
此约束仅用于 Profile 声明的 `fixed_reference`，
不用于随关节运动的 Tag，也不证明初始分支物理正确或当前外参已经实测验证。
方向结束才检查 ≥40 个有效同步样本、≥32 分箱、内部空白 ≤行程的 1/16、规定的有效行程覆盖，
以及 Profile 声明的端点/双视角观测。数据不足只同速重扫一次；第二次失败暂停。
拟合或最终文件验收失败不重新自动运动、不更新发布指针。
`independent_rotation_holdout_failed` 表示前三轮冻结旋转模型未通过第四轮检查，发生在空间求解前。
错误原因包含失败关节、机位和 MAE/P95/最大误差；`fit_diagnostics.json.rotation_holdout`
另存完整 SO(3)、轴向、偏轴统计及最坏图像身份。验收使用完整旋转误差，不能仅凭投影角度曲线重复判定通过。
新采样行的 `pnp_observation_evidence` 保存父子 Tag 的当前角点、尺寸、已选姿态和候选诊断，
相机矩阵引用同会话的 `rectified_camera_model`。缓存固定基准明确标注来源，不伪造当前帧的角点或候选。
可见且通过冻结姿态投影验证的基准标为 `verified_fixed_reference`，保存当前角点、IPPE 候选和实际投影误差；
完全未检测到时的缓存仍标为 `cached_fixed_reference`。候选回放保持同一冻结基准并复核前者的当前角点证据。
短程诊断报告以 `fixed_reference_summary` 单独统计基准的当前图像投影误差，
不会把固定输出姿态的零跳变误当作独立测量结果。
不合格姿态事件另外记录当时的动作阶段、指令与同步反馈，便于区分准备动作期间的异常与正式采样异常。
角点与候选记录用于诊断；其中分支状态、版本及实际选中姿态也用于核验冻结参考的一致性，
不使历史采集自动获得缺失的观测证据。O12 的轨迹几何评分仍只使用训练轮，
但正式收尾不得用评分更好的另一支替换已冻结的训练或第四轮姿态；超过同一公共等价界时明确拒绝发布。
历史记录仍可在离线诊断入口分析候选，不能借旧格式标记绕过正式产物的冻结分支检查。
所有型号显示相同进度：阶段、任务/轮次/方向、分机位 Tag ID、命令/反馈/单位/速度、
有效数据、基准、断点来源，以及中文原因、建议和原始诊断。耗时拟合在后台执行，不阻塞反馈与状态。
有效数据、基准、断点来源，以及中文原因、建议和原始诊断。
ROS 收尾拟合在独立进程执行：只读取采集结束时已同步的日志前缀，后续图像不进入拟合；
主进程继续接收反馈并检查固定基准，阶段事件按原顺序回传，最终发布仍由主进程持锁完成。
中止时终止计算子进程，不允许子进程更新发布指针。`finalization_timing.json` 保存实际耗时。
标题明确显示型号、左右手和序列号。开始前等待状态列出缺少或过期的相机内参、检测消息和 SDK 条件；
空 Tag 检测消息可以证明视觉链路工作，不要求开始前已经识别全部 Tag。
收到开始请求时重新检查设备状态；实时 CameraInfo 参数必须与受保护外参中的指纹一致。
@@ -155,7 +505,7 @@ linkerhand_calibration/
├── profiles/ 加载、静态验证、Tag/CAD 观测关系
├── core/domain/ Profile、样本、CalibrationResult、状态
├── core/geometry/ 相机、变换、PnP 兼容导出
│ └── tag_pose/ IPPE、连续跟踪、刚性组、轨迹分支选择
│ └── tag_pose/ IPPE、连续跟踪、冻结前运动证据、刚性组和轨迹选择
├── core/fitting/ 运动曲线、线性耦合、固定安装
│ └── spatial_solver/ 空间观测、基座、零位、统计与独立验收
├── core/urdf/ 授权修正、FK、范围与最终文件验收
@@ -169,6 +519,9 @@ linkerhand_calibration/
├── runtime/execution.py Profile 任务与运动效果调度
├── runtime/motion_execution.py 平滑轨迹与位移到位判定
├── runtime/capture.py 多机位视觉/反馈采集
├── runtime/branch_initialization.py 最后归零准备段证据与一次性几何确认
├── runtime/branch_tracking.py 冻结约束选候选，原子提交同帧姿态与证据
├── runtime/motion_provenance.py 读取和收尾核验来源内容、零位与样本关联
├── runtime/scan_quality.py 唯一方向数据门
├── runtime/reference_lock.py 开始后固定基准
├── runtime/resume.py 整场断点证据检查
@@ -193,7 +546,8 @@ linkerhand_calibration/
PnP 的原导入路径继续可用。`tag_pose/parameters.py` 集中定义跟踪默认值，
ROS 加载器将角度参数由 deg 转为 rad，离线采集直接使用同一套默认值。
公共分支容差仍为 `0.03 px`，重投影质量、倾角、过滤原因和连续跟踪恢复策略保持一致。
公共分支容差仍为 `0.03 px`，重投影质量和倾角限制保持一致；首次确认、参考冻结与失联恢复
使用同一公共分支策略，ROS 和离线链路均不能绕过它。
`CalibrationSession.phase` 是流程状态来源。协调器在状态锁内开始、中止、推进和提交观测；
耗时姿态计算在锁外进行，提交时核对会话版本、采集对象、运动对象及稳态采样边界。
@@ -242,25 +596,66 @@ v1/v2 按原语义读取，不会自动升级为独立实测；保留的 v2 数
有效发布由 `release_manifest.json` 及产品发布指针共同证明；单独的 `PASS` 摘要或存在 URDF 文件不等于已发布。
manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和独立 holdout 证据。任一验收失败都不会替换原发布指针。
`measurement_contract.json` 提前列出 Tag/link、零位观测关系、CAD 保留/迁移与原始 mimic 限位冲突。
主 JSON 以原始 URDF 关节名为键，每个关节仅包含 `sdk_channel` 和 `angle_rad`；
主 JSON 以原始 URDF 关节名为键，单值关节包含 `sdk_channel` 和 `angle_rad`；
O12 等 rad 输入还包含一一对应的 `input_values`（在有效节点间线性插值，不外推）。
字节型号固定 256 项：SDK 指令 `v` 对应 `angle_rad[v]`，只能在训练指令实际覆盖 0–255 时导出。
字节型号每条曲线固定 256 项：SDK 指令 `v` 对应所选曲线的第 `v` 项，只能在训练指令实际覆盖 0–255 时导出。
输出均是修正 URDF 的 q_output，不再加零偏。顶层记录身份、单位、完整 `baseline_command` 和共同零位约定。
独立观测的主动和被动关节直接导出各自视觉曲线，同一驱动通道可绑定多个关节。
显式复制的关节使用来源关节的同一组节点与角度，不重新拟合、归零或由 mimic 推导；接收通道保持不变。
最终单条曲线在前三轮拟合中约束 baseline=0，正反向诊断分支共用冻结参考、保留回差。
第四轮分别检验两个方向；超差时禁止发布，不隐藏切换分支。
URDF mimic 单独用前三轮成对视觉角度拟合 `q_child≈a*q_parent`，共同零位下 offset=0；
复制的被动关节使用对应来源的线性近似，证据仍归属于来源关节。
其近似误差不会否决已达标的完整映射，但结构、授权字段与机械限位始终强制检查。
默认单值曲线在指定的稳态训练数据中约束 baseline=0，正反向诊断分支共用冻结参考、保留回差。
独立稳态验证分别检验两个方向；超差时禁止发布，不隐藏切换分支。
验收分别记录：最终 JSON 对第四轮独立视觉角度；完整 JSON＋URDF FK 对第四轮 Tag 位姿；
同一主动映射配合标准 mimic 的近似损失。前两项决定精度通过，第三项仅报告。
O6 侧摆 `rh_thumb_cmc_yaw` 通过 `artifacts.directional_command_joints` 显式启用双向指令曲线。
v3 JSON 顶层保存同名声明，所列关节额外包含 `increasing_rad` / `decreasing_rad`；
方向指原始 SDK 数值增加/减少，与关节角度正负无关。其他关节继续使用单值表。
两条曲线分别只用所声明的稳态训练数据拟合，共用已冻结零位、各自满足 baseline=0；
独立稳态验证按实采方向读取最终序列化曲线，仍要求 MAE≤1°、P95≤2°、最大≤3°。
报告分别保存运动与指令映射的训练／验证分区及来源图像，限位推导仍不得使用任一验证分区。
`angle_rad` 保留为均值参考；双向关节在非 baseline 输入且方向未知时禁止用均值代替。
读取器须从 baseline 初始化，停住时保持到达方向，允许端点换向；行程中途反转尚无小回环标定证据，会明确拒绝。
字节指令的方向判断与查表共用 `floor(v+0.5)` 量化规则；连续小数步进不能绕过方向切换检查。
两条主回环曲线不构成任意反转、负载或多轴工况的精度证明。旧读取器会拒绝新增关节字段，须同步更新。
修正 URDF 保留原被动关节 `<mimic>`，单独拖动父关节滑条时，末节按
`q_child ≈ multiplier × q_parent + offset` 联动。标准 URDF 不能将查找表表达为 mimic。
前三轮成对视觉角度用于拟合线性近似，优化变量为父关节两端对应的子关节角度；
两端都限制在子关节原有输出范围内，因此整个联动行程合法，无需放大限位或裁剪小负角。
偏移是近似直线在 baseline 的误差，不能当作新的物理零位；报告明确保存
`baseline_error_rad`、`physical_zero_correction: false` 和独立留出误差。
JSON 指令/反馈曲线、几何零位不因近似拟合改变。需要实测非线性运动时使用完整 JSON，
消费程序显式设置全部关节角，并避免 URDF mimic 再次覆盖末节角度。
原始 CAD 限位默认仍是硬边界。Profile 可逐关节显式声明 `calibrated_motion_envelope`，
将该 CAD 限位视为本次要测定的运动范围；只有已授权的 `limit.lower/upper` 可据此修改。
O6 右手采用此策略，范围由前三轮运动反馈映射、独立稳态训练映射的实测包络及共同零位确定，
不增加安全余量，不用第四轮扩展范围，也不为容纳 mimic 越界而放大范围。
SDK 安全指令域不变，IP／DIP 的 CAD 零位不变。复制关节的范围证据仍属于来源关节。
报告和 manifest 的验收数据保存 `motion_range_evidence`，包含来源限位、零偏、训练轮次、
样本身份与新范围，并在收尾重新验证。O6 尚无独立机械安全角度界的实测证据，
该策略不代表对碰撞、过载或任意多轴姿态作出安全认证。
验收分别记录：最终 JSON 对独立稳态验证角度；完整 JSON＋URDF FK 对独立验证 Tag 位姿；
仅使用 URDF mimic 的近似损失。前两项决定实测精度通过，第三项如实报告，不冒充非线性精度。
manifest 同时保存 `urdf_joint_angle_source: calibration_json`、`urdf_mimic_policy` 和两种回放结果。
门限保持角度 MAE≤1°、P95≤2°、最大≤3°，Tag 位置 P95≤3 mm。
平行转轴的零位计算先消除轴上参考点的任意坐标，再比较相机平面中的轴间方向；
改变同一轴线的表示点不能改变标定零位。这是所有型号共用的几何规则。
独立精度结论只覆盖实际观测的关节和 Tag；复制关节另检查来源、查表一致性、通道与机械范围，
不把来源关节的第四轮误差称为接收关节精度。混合产物 manifest 使用
`measured_and_transferred_json_urdf_v3`，列出实测和复制覆盖。
两种 FK 使用相同的训练冻结安装与基座；第四轮不拟合零位、曲线、Tag 安装或基座。
FK 使用训练冻结的安装与基座；第四轮不拟合零位、曲线、Tag 安装或基座。
正式图像采集还要求核验第四轮及独立稳态验证的原始 Tag 四角，沿用冻结的安装、基座
和受哈希保护的相机参数。主观测的独立三维位姿必须解释同帧原始角点，四角重投影 RMS
不得超过原 1.5 px；最终落盘 JSON＋修正 URDF 与独立位姿的差异仍按上述角度和位置门限检查。
最终 FK 的总像素误差单独报告，不能将包含允许运动误差的总量等同于图像测量噪声。
额外机位没有独立三维测量时，仅检查是否存在满足原物理误差范围、且重投影误差不超过
1.5 px 的局部刚性位姿。这个数值见证不是真实位姿测量，不写回安装、零位、曲线或相机；
各运动任务分别检查，不能用其他任务中的静止图像稀释误差。它不能替代必要的独立三维验收。
缺少独立测量或原始图像、相机身份不一致、测量像素或物理误差超限均不能发布。
失败时保留 `final_image_diagnostics.json`；通过后在 manifest 记录 `final_file_image_holdout`
及规则版本 `independent_measurement_and_physical_consistency_v2`，分别列出独立精度与额外视角相容性。
训练阶段若 Tag 与整条运动链不满足刚性一致性，`tag_installation_diagnostics.json`
一次列出全部 Tag 的角度／位置 P95、门限和通过状态，不只显示遇到的第一个失败 Tag。
该失败不能直接归因于硬件松动，也不能作为自动重做整场采集的依据。
`calibration_report.json` 保存完整的 `command_to_rad` / `feedback_to_rad`、输入域、方向分支、
实测关节的零位原始参考帧、完整指令/反馈/到达方向、来源和训练/holdout 指标。
@@ -280,8 +675,11 @@ ros2 run linkerhand_calibration calibrated_joint_state_bridge --ros-args \
需要反馈角度显示时显式选择 `input_kind:=feedback`，桥读取配套报告中的反馈映射。
桥检查三文件哈希、拒绝越域输入，
v3 全部关节角由 JSON 显式计算，完整 FK 必须启用 `independent_mimic_angles=True`，不加第二次零偏。
标准 `robot_state_publisher` 的 mimic 更新会覆盖被动角；仅检查其加载兼容性，不用它验证完整非线性运动。
v3 全部关节角由 JSON 显式计算，不加第二次零偏。完整 FK 使用
`independent_mimic_angles=True`，以 JSON 给出的末节角度覆盖线性近似。
标准 `robot_state_publisher` 和普通 URDF 网页查看器会执行 mimic，适合检查近似联动；
仅把完整 JointState 发给标准 RSP，不能让它停止覆盖被动角。
消费实测非线性曲线的仿真程序必须明确采用 JSON 角度，关闭重复的线性被动更新。
仿真输出不能与实机反馈话题同名。
MuJoCo 是用户后续使用的外部验收项目，本轮不改动它、不增加 MuJoCo 依赖，自动标定不要求打开 GUI。
外部对比须确认加载的模型来自这份 URDF，指令转换使用配套 JSON；不能用实机反馈直接驱动仿真来证明指令映射。
@@ -301,6 +699,41 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \
旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出，且不会绕过验证。
当前坐标规则、生产职责与验收范围见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
当前代码审查、已修复问题与未完成的精度验收见
[2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)。相机图像目前采用主机取帧时间；
USB/驱动积压情况下，图像与反馈的时间戳接近不能证明曝光时刻同步，不能据此宣称实机精度通过。
早期需求审查见 [2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)；
当前修改依据、实机结果与未完成的精度验收见 [流程评估](CALIBRATION_PIPELINE_REVIEW.md)。
相机和反馈目前均采用下述测量时间；时间配对正确仍须经过独立空间验收。
### 实时调度边界
公共 ROS 节点顺序处理控制和反馈消息；耗时图像估计由每视角有界工作队列执行。
图像最多在原同步容差内等候真实反馈时间覆盖，队列不反向持锁查询会话。
增加执行器线程数不能提高共享状态的处理能力；反馈超时仍按真实测量判断。
暂停或验收失败的会话仅提供诊断/候选文件，不能当作已发布的标定结果。
调度对照依据与验证范围见 `CALIBRATION_PIPELINE_REVIEW.md` 第七轮评估。
### 相机与反馈时间
连续扫描将 CAN 实测反馈与相机曝光中点配对。MVS 取帧/图像发布时间不参与替代曝光时间；
设备时钟由独立锁存换算到 ROS 时钟，换算依据保存在会话的 `camera_timing_<view>.jsonl`。
四型号共用 `unified_engine_v8_all_view_images`，保留设备曝光时间策略；旧采集只能按其原策略诊断，不能混用。
报告中的 `feedback_u8` 表示来自字节通道的测量值：插值到曝光时间后可以是小数，
拟合、内存映射和报告读取均按声明的分段线性曲线计算，不再次取整。
`command_u8` 仍按实际发送的整数字节查表；旧版整数表保留原读取规则。
相机时钟修正不会将被动关节的非线性曲线转成 URDF mimic；仿真仍须读取 JSON 查找表。
### 同一运动中的所有机位原始角点
四轮扫描、必要稳态点和关节基准采样期间，所有机位均保存 `image_observation_frame`。
记录包含曝光时间、完整指令/反馈及方向、轮次与尝试编号、相机矩阵和可见 Tag 的二维角点。
它经过原检测质量与采集窗口检查，但不依赖 PnP 分支或任务的主机位。
缺少同步反馈时保留明确的空值，不能将该帧当成有效角度测量。
例如 O6 顶部 ID7 与正面 ID2 属于同一末节：弯曲和侧摆时两机位的可见角点都应保存，
以便检查整条运动链。其他机位的记录不会增加停点或运动次数，也不会替代主机位的采样覆盖要求。
同机位重复帧不重复入库；暂停、切换运动和结束采集时仍按同一会话身份撤销迟到数据。
最终 JSON/URDF 的图像验收使用通过采集检查的第四轮和独立稳态验证窗口内全部机位角点。
主观测必须能与原始图像记录逐项对应，同一图像不能重复计数；其他动作下的有效角点也直接
投影验收，不先拟合成三维位姿。缺少主观测的原始记录或二者 SDK 条件不一致时拒绝发布。
这不表示联合求解或外参自校准已经通过验证。
src/linkerhand_calibration/config/g20_right_product.yaml
+1 -1
View File
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: G20/right/g20_right_19/v1
profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml
profile_config_sha256: f73c8a391a8feb6cb3028fbd8278d3b18d6e2e875980235f7797fd8de2efbac1
profile_config_sha256: 958f74c476730636d2dadb58a30296a7cdf0b25ad00345252754f9c1b6b50c7f
model: G20
side: right
tag_layout: g20_right_19
src/linkerhand_calibration/config/l6_right_product.yaml
+1 -1
View File
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: L6/right/l6_right_8/v1
profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml
profile_config_sha256: 733094679e5477bbe97a633f4fb1ce25242804b667cd435ef9132f491e6ef247
profile_config_sha256: d241e87ded3a29d416a876b27f1a251f8daf2f98e3676ab277264f1bdfaf5c60
model: L6
side: right
tag_layout: l6_right_8
src/linkerhand_calibration/config/o12_right_product.yaml
+2 -2
View File
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: O12/right/o12_right_16/v1
profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml
profile_config_sha256: d0c1e2ac62f96162612f0ec45c25c6f467d045c1c1030b5be34ccc115dca4cf9
profile_config_sha256: 652c2a82dfc7532b4f62c42cebdad57b169435cf950c5f2ee910cc67c7972621
model: O12
side: right
tag_layout: o12_right_16
@@ -15,7 +15,7 @@ sdk:
package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170
transport: hcan
setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml
config: package://linkerhand_calibration/config/o12_sdk.yaml
config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c
cameras:
src/linkerhand_calibration/config/o12_sdk.yaml
+45
View File
@@ -0,0 +1,45 @@
# OmniHand Pro 2025 (O12) Configuration
# connection_type: "zlgcan" | "hcan" | "socketcan" | "rs485" | "usb"
# request_interval_ms: min gap between requests, SDK default 0 = no throttling, range 0..100
# frame_recv_timeout_ms: single-frame reply wait, SDK default 50, range 10..1000
/**:
ros__parameters:
# This workstation uses one O12 right hand through HCAN device 0/channel 0.
# Leave left_hand undefined so the node does not try to open a second hand.
right_hand:
hand_device_id: 1
connection_type: "hcan"
canfd_device_id: 0
canfd_channel_id: 0
# O12 POSITION streams set-points and has no G20-style firmware speed
# interpolation, so do not throttle the 50 Hz calibration command stream.
request_interval_ms: 0
frame_recv_timeout_ms: 100
show_data_details: false
# Alternative ZLG CANFD example:
# right_hand:
# hand_device_id: 1
# connection_type: "zlgcan"
# canfd_device_id: 0
# canfd_channel_id: 0
# request_interval_ms: 0
# frame_recv_timeout_ms: 50
# show_data_details: false
# socketcan example
# left_hand:
# hand_device_id: 1
# connection_type: "socketcan"
# can_interface: "can0"
# request_interval_ms: 0
# frame_recv_timeout_ms: 50
# show_data_details: true
# right_hand:
# hand_device_id: 1
# connection_type: "socketcan"
# can_interface: "can1"
# request_interval_ms: 0
# frame_recv_timeout_ms: 50
# show_data_details: true
src/linkerhand_calibration/config/o6_right_8_tags.yaml
+6 -6
View File
@@ -3,7 +3,7 @@
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
size: 0.0165
max_hamming: 0
detector:
threads: 4
@@ -16,14 +16,14 @@
tag:
ids: [0, 1, 2]
frames: [front_base, thumb_pitch, thumb_ip]
sizes: [0.016, 0.016, 0.016]
sizes: [0.0165, 0.0165, 0.0165]
/o6_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
size: 0.0165
max_hamming: 0
detector:
threads: 4
@@ -36,14 +36,14 @@
tag:
ids: [3, 4, 5]
frames: [side_base, pinky_pitch, pinky_dip]
sizes: [0.016, 0.016, 0.016]
sizes: [0.0165, 0.0165, 0.0165]
/o6_calibration/top/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
size: 0.0165
max_hamming: 0
detector:
threads: 4
@@ -56,4 +56,4 @@
tag:
ids: [6, 7]
frames: [top_base, thumb_yaw]
sizes: [0.016, 0.016]
sizes: [0.0165, 0.0165]
src/linkerhand_calibration/config/o6_right_product.yaml
+8 -8
View File
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: O6/right/o6_right_8/v1
profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml
profile_config_sha256: df6c4abb3c9d48b186c1bd6ab304ea9c0bbab54f9381ff222d5d8d3a3f65babe
profile_config_sha256: 72bf56b13eb2a0d4d14a5be42838b2ffc176722f470fe1a3ae5fd84604e1df4d
model: O6
side: right
tag_layout: o6_right_8
@@ -14,25 +14,25 @@ cameras:
front:
serial_number: DB2163742
camera_name: hikrobot_front_DB2163742
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163742.yaml
side:
serial_number: DB2163749
camera_name: hikrobot_side_DB2163749
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163749.yaml
top:
serial_number: DB2163739
camera_name: hikrobot_top_DB2163739
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
camera_info: config/o6_camera_intrinsics_20260915/hikrobot_DB2163739.yaml
artifacts:
source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf
source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
camera_extrinsics: config/o6_three_camera_extrinsics.yaml
camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
camera_extrinsics: config/o6_three_camera_extrinsics_20260915_192517.yaml
camera_extrinsics_sha256: d057183eaba592149ec4c63c4ceab8a70e1a0c9d7cf2460a1c8b7e86a6ca6638
calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
calibration_config_sha256: 15adaec462d0895dc68d19760e8f333c62e57786d124f22cfc481007a3d976a4
calibration_config_sha256: 49ce8a0d317695994c5b306e3a3a28d0470b8769c77f1173ca935178430587db
tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml
tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
tag_config_sha256: 219b8aa906fc9e2a13f3bbede46f542ff176ef2c7f60190f0dd2508993f04593
release:
required_independent_passes: 1
src/linkerhand_calibration/config/o6_three_camera_calibration.yaml
+2 -2
View File
@@ -21,9 +21,9 @@ o6_calibration:
torque_u8: 80
repetitions: 4
preflight_checkpoints_u8: [255, 127, 0]
tag_size_m: 0.016
tag_size_m: 0.0165
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
tag_size_overrides_m: [0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165]
minimum_detection_rate: 0.95
minimum_joint_frame_rate: 0.85
src/linkerhand_calibration/config/profiles/g20_right_19.yaml
+3 -1
View File
@@ -1503,7 +1503,9 @@ artifacts:
- urdf_zero_publication
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v5_joint_zero
command_capture_mode: separate
joint_zero_timeout_seconds: 5.0
policy_version: unified_engine_v8_all_view_images
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
src/linkerhand_calibration/config/profiles/l6_right_8.yaml
+3 -1
View File
@@ -347,7 +347,9 @@ artifacts:
- l6_partial_v1
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v5_joint_zero
command_capture_mode: separate
joint_zero_timeout_seconds: 5.0
policy_version: unified_engine_v8_all_view_images
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
src/linkerhand_calibration/config/profiles/o12_right_16.yaml
+8 -2
View File
@@ -744,6 +744,9 @@ measurement:
pose_axis_line_required: false
zero:
cad_zero_assumptions:
thumb_mcp: User accepts original CAD zero at SDK baseline; absolute zero is assumed, motion is measured.
pinky_pip: User accepts original CAD zero at SDK baseline; absolute zero is assumed, motion is measured.
ring_pip: User accepts recipient CAD zero at SDK baseline; motion is copied from pinky_pip, not independently measured.
index_dip: User accepts original CAD joint zero as the physical baseline; not independently
measured.
middle_dip: User accepts original CAD joint zero as the physical baseline; not independently
@@ -844,7 +847,6 @@ zero:
- pinky_mcp_pitch
depth_free_axis_projection: true
accept_validated_zero_in_confidence_interval: true
project_axis_gauge_before_image: true
axis_order:
- thumb_cmc_roll
- thumb_cmc_yaw
@@ -929,7 +931,9 @@ artifacts:
- full_sdk_range_v2
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v5_joint_zero
command_capture_mode: separate
joint_zero_timeout_seconds: 5.0
policy_version: unified_engine_v8_all_view_images
mapping_probe_maximum_rad: 0.05235987755982989
urdf:
authorized_fields:
@@ -997,8 +1001,10 @@ urdf:
- mimic.multiplier
- mimic.offset
ring_pip:
- mimic.multiplier
- mimic.offset
ring_dip:
- mimic.multiplier
- mimic.offset
joint_coverage:
thumb_cmc_roll: measured_static_dynamic
src/linkerhand_calibration/config/profiles/o6_right_8.yaml
+61 -1
View File
@@ -55,31 +55,40 @@ vision:
- role: front_base
fixed_reference: true
id: 0
size_m: 0.0165
- role: thumb_pitch
fixed_reference: false
id: 1
size_m: 0.0165
- role: thumb_ip
fixed_reference: false
id: 2
size_m: 0.0165
- name: side
tags:
- role: side_base
fixed_reference: true
id: 3
size_m: 0.0165
- role: pinky_pitch
fixed_reference: false
id: 4
size_m: 0.0165
- role: pinky_dip
fixed_reference: false
id: 5
size_m: 0.0165
- name: top
tags:
- role: top_base
fixed_reference: true
id: 6
size_m: 0.0165
- role: thumb_yaw
fixed_reference: false
id: 7
size_m: 0.0165
link: rh_thumb_distal
common_frame: calibration_common
extrinsic_reference_view: front
extrinsics_quality_limits:
@@ -329,6 +338,8 @@ scope:
default_scope: partial
artifacts:
output_schema_version: 3
directional_command_joints:
- rh_thumb_cmc_yaw
calibration_filename: o6_right_{serial_number}_partial_calibration.json
corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf
protected_input_fields:
@@ -343,7 +354,12 @@ artifacts:
- o6_partial_v1
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v5_joint_zero
joint_zero_timeout_seconds: 5.0
command_capture_mode: separate
steady_training_nodes: 9
steady_extra_training_nodes:
pinky_pitch_dip_side: [239.0]
policy_version: unified_engine_v8_all_view_images
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
@@ -358,6 +374,40 @@ acquisition:
fixed_reference_maximum_drift_px: 5.0
fixed_reference_confirmation_frames: 10
urdf:
limit_policies:
rh_pinky_mcp_pitch:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_thumb_cmc_pitch:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_middle_mcp_pitch:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_index_mcp_pitch:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_ring_mcp_pitch:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_thumb_cmc_yaw:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_pinky_dip:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_thumb_ip:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_middle_dip:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_ring_dip:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
rh_index_dip:
source_limits: calibrated_motion_envelope
evidence_source: User accepts original CAD zeros but requests measured SDK motion amplitudes; source CAD limits are nominal motion envelopes.
authorized_fields:
rh_pinky_mcp_pitch:
- limit.lower
@@ -386,18 +436,28 @@ urdf:
rh_ring_dip:
- mimic.multiplier
- mimic.offset
- limit.lower
- limit.upper
rh_index_dip:
- mimic.multiplier
- mimic.offset
- limit.lower
- limit.upper
rh_pinky_dip:
- mimic.multiplier
- mimic.offset
- limit.lower
- limit.upper
rh_thumb_ip:
- mimic.multiplier
- mimic.offset
- limit.lower
- limit.upper
rh_middle_dip:
- mimic.multiplier
- mimic.offset
- limit.lower
- limit.upper
joint_coverage:
rh_pinky_mcp_pitch: measured_static_dynamic
rh_thumb_cmc_pitch: measured_static_dynamic
src/linkerhand_calibration/launch/three_camera_calibration.launch.py
+23 -13
View File
@@ -29,6 +29,8 @@ def _launch_stack(context):
)
from linkerhand_calibration.profiles import load_hand_profile
from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics
from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture
from linkerhand_calibration.runtime.observation_scope import required_observation_views
model = LaunchConfiguration("model").perform(context).strip().upper()
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
@@ -48,7 +50,11 @@ def _launch_stack(context):
raise ValueError("launch identity differs from the protected Profile")
except ValueError as error:
raise RuntimeError(str(error)) from error
views = contract.typed_profile.vision.view_names
diagnostic = resolve_diagnostic_capture(
contract.typed_profile,
LaunchConfiguration("diagnostic_capture").perform(context).strip(),
)
views = required_observation_views(contract.typed_profile, diagnostic)
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
if not requested_tag_config:
raise RuntimeError("tag_config is required; use calibrate_hand --config")
@@ -110,9 +116,9 @@ def _launch_stack(context):
for view in views
}
if any(not serial for serial in camera_serials.values()):
raise RuntimeError("all Profile camera serial numbers are required")
raise RuntimeError("all required observation camera serial numbers are required")
if len(set(camera_serials.values())) != len(views):
raise RuntimeError("Profile camera serial numbers must be unique")
raise RuntimeError("required observation camera serial numbers must be unique")
cameras = []
components = []
@@ -153,6 +159,8 @@ def _launch_stack(context):
),
"image_width": 1624,
"image_height": 1240,
"timestamp_journal_path": str(Path(LaunchConfiguration("session_dir").perform(context))
/ f"camera_timing_{view}.jsonl"),
"frame_rate": ParameterValue(
LaunchConfiguration("camera_frame_rate"),
value_type=float,
@@ -194,15 +202,10 @@ def _launch_stack(context):
plugin="AprilTagNode",
name="apriltag",
namespace=detector_namespace,
parameters=[
str(tag_config),
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
value_type=float,
)
},
],
# Detector settings belong to the protected model YAML.
# A generic launch default must not silently replace its
# full-resolution setting for the small calibration Tags.
parameters=[str(tag_config)],
remappings=[
("image_rect", rect_topic),
("camera_info", info_topic),
@@ -318,6 +321,9 @@ def _launch_stack(context):
"commands_enabled": ParameterValue(
LaunchConfiguration("commands_enabled"), value_type=bool
),
"diagnostic_capture": ParameterValue(
LaunchConfiguration("diagnostic_capture"), value_type=str
),
},
],
)
@@ -364,6 +370,10 @@ def _launch_stack(context):
),
*cameras,
vision,
Node(package="linkerhand_calibration", executable="tag_border_filter_node",
name="tag_border_filter", output="screen",
arguments=["--profile-config", profile_path, "--profile-sha256", expected,
"--views", *views]),
sdk,
calibration,
bag,
@@ -400,7 +410,6 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
DeclareLaunchArgument("gain_db", default_value="0.0"),
DeclareLaunchArgument("auto_exposure", default_value="false"),
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
DeclareLaunchArgument("can_interface", default_value="can0"),
DeclareLaunchArgument("calibration_speed", default_value="15"),
DeclareLaunchArgument("camera_extrinsics_file", default_value=""),
@@ -430,6 +439,7 @@ def generate_launch_description() -> LaunchDescription:
),
DeclareLaunchArgument("profile_config", default_value=""),
DeclareLaunchArgument("commands_enabled", default_value="true"),
DeclareLaunchArgument("diagnostic_capture", default_value=""),
DeclareLaunchArgument("start_cameras", default_value="true"),
DeclareLaunchArgument("start_sdk", default_value="true"),
DeclareLaunchArgument("record_bag", default_value="false"),
src/linkerhand_calibration/linkerhand_calibration/acquisition.py
+24
View File
@@ -109,6 +109,7 @@ class Observation:
class StateSample:
stamp_ns: int
position_u8: tuple[float, ...]
channel_stamps_ns: tuple[int, ...] = ()
def interpolate_state_u8(
@@ -127,6 +128,29 @@ def interpolate_state_u8(
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))
src/linkerhand_calibration/linkerhand_calibration/camera_timing.py
+165
View File
@@ -0,0 +1,165 @@
"""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"
@dataclass(frozen=True)
class ClockLatch:
device_tick: int
before_ns: int
after_ns: int
@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
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+1_000_000
if elapsed < 0 or abs(elapsed-host_elapsed) > tolerance:
raise ValueError("camera/host clock discontinuity or counter unit mismatch")
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=""):
self.camera, self.mvs, self.clock_ns = camera, mvs, clock_ns
self.exposure_us = exposure_us
self.frame_clock = 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):
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")
return ClockLatch(tick, before, self.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
self.frame_clock.update(valid)
self._write(dict(kind="camera_clock_sync", policy=CAMERA_TIMING_POLICY,
ticks_per_second=self.frame_clock.ticks_per_second,
latches=[asdict(x) for x in latches], anchor=asdict(self.frame_clock.anchor)))
if self.journal is not None:
self.journal.flush()
self.next_refresh = time.monotonic()+.5
def refresh_if_due(self):
if time.monotonic() >= self.next_refresh:
self._update([self._latch() for _ in range(3)])
def timestamp(self, info, received_ns):
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)
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
src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py
+3
View File
@@ -39,6 +39,8 @@ def build_typed_profile():
right,
key=KEY,
namespace="/o6_left_transferred",
# This historical mirror has no measured left-hand range evidence.
urdf_limit_policies={},
command=replace(
right.command,
command_index_by_joint=command_index,
@@ -116,6 +118,7 @@ def build_typed_profile():
artifacts=replace(
right.artifacts,
output_schema_version=2,
directional_command_joints=frozenset(),
calibration_filename=(
"o6_left_{serial_number}_transferred_calibration.json"
),
src/linkerhand_calibration/linkerhand_calibration/core/__init__.py
+2
View File
@@ -7,6 +7,7 @@ from .domain import (
CalibrationProfile,
CalibrationResult,
JointMapping,
JointLimitPolicy,
CalibrationStatus,
CommandLayout,
MeasurementPolicy,
@@ -53,6 +54,7 @@ __all__ = [
"CalibrationProfile",
"CalibrationResult",
"JointMapping",
"JointLimitPolicy",
"CalibrationStatus",
"CommandLayout",
"DIRECTION_DECREASING",
src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py
+14 -2
View File
@@ -31,9 +31,14 @@ def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
def append_jsonl_many(
path: str | Path, payloads: Iterable[Mapping[str, Any]]
path: str | Path, payloads: Iterable[Mapping[str, Any]], *, durable: bool = True,
) -> None:
"""Durably append a batch while paying the fsync cost only once."""
"""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)
lines = [
@@ -45,6 +50,13 @@ def append_jsonl_many(
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."""
with Path(path).open("rb") as stream:
os.fsync(stream.fileno())
src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py
+2
View File
@@ -19,6 +19,7 @@ from .profile import (
ZeroSolvePolicy,
JointZeroSpec,
KnownBaselineGeometry,
JointLimitPolicy,
validate_profile,
)
from .result import CalibrationResult, JointMapping
@@ -42,6 +43,7 @@ __all__ = [
"JointZeroSample",
"JointZeroSpec",
"KnownBaselineGeometry",
"JointLimitPolicy",
"CalibrationStatus",
"CommandLayout",
"MeasurementPolicy",
src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py
+64 -1
View File
@@ -8,7 +8,7 @@ from pathlib import PurePath
from typing import Mapping
ACQUISITION_POLICY_VERSION = "unified_engine_v5_joint_zero"
ACQUISITION_POLICY_VERSION = "unified_engine_v8_all_view_images"
@dataclass(frozen=True, order=True)
@@ -337,9 +337,21 @@ class AcquisitionPolicy:
require_endpoint_observations: bool = False
endpoint_tolerance_01: float = 2.0 / 255.0
steady_training_nodes: int = 9
command_capture_mode: str = "interleaved"
steady_extra_training_nodes: Mapping[str, tuple[float, ...]] = field(default_factory=dict)
steady_minimum_samples: int = 3
steady_window_seconds: float = 0.2
steady_timeout_seconds: float = 2.0
joint_zero_timeout_seconds: float = 2.0
def observation_streams(profile, task):
"""Primary and declared secondary views of each measured joint."""
for name in task.joints:
yield "joint", name, profile.measurement.measurements[name]
secondary = profile.measurement.cross_view_sources.get(name)
if secondary is not None:
yield "observation_joint", name, profile.measurement.measurements[secondary]
@dataclass(frozen=True)
@@ -364,6 +376,22 @@ class ArtifactPolicy:
publication_pointer: str = "latest_passed"
session_compatibility_tokens: frozenset[str] = frozenset()
publish_corrected_urdf: bool = False
directional_command_joints: frozenset[str] = frozenset()
@dataclass(frozen=True)
class JointLimitPolicy:
"""An explicit interpretation of CAD limits, separate from SDK safety.
Independent safety bounds, when known, are in original CAD coordinates.
Their absence never means that a calibrated motion envelope is a hardware
safety certificate.
"""
source_limits: str
evidence_source: str
independent_safety_limits_cad_rad: tuple[float, float] | None = None
safety_evidence_source: str = ""
@dataclass(frozen=True)
@@ -388,6 +416,7 @@ class CalibrationProfile:
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
# and mimic_nominal.
joint_coverage: Mapping[str, str] = field(default_factory=dict)
urdf_limit_policies: Mapping[str, JointLimitPolicy] = field(default_factory=dict)
@property
def curve_input_domain(self) -> str:
@@ -723,6 +752,16 @@ def validate_profile(profile: CalibrationProfile) -> None:
artifacts = profile.artifacts
if artifacts.output_schema_version < 1:
errors.append("artifact schema version must be positive")
if artifacts.directional_command_joints and (artifacts.output_schema_version != 3
or not artifacts.directional_command_joints <= profile.measurement.measurements.keys()):
errors.append("directional command curves require measured v3 joints")
for target, source in profile.measurement.transferred_motion_sources.items():
if (target in artifacts.directional_command_joints) != (source in artifacts.directional_command_joints):
errors.append("transferred command curves must retain their source direction policy")
for task in profile.motion.tasks:
if artifacts.directional_command_joints.intersection(task.joints):
if profile.command.baseline_values[task.command_index] not in (task.start_value, task.end_value):
errors.append("directional command mapping requires an endpoint baseline")
for label, filename in (
("calibration", artifacts.calibration_filename),
("corrected URDF", artifacts.corrected_urdf_filename),
@@ -742,9 +781,33 @@ def validate_profile(profile: CalibrationProfile) -> None:
valid_urdf_fields
):
errors.append("URDF authorization contains an unsupported field")
for name, policy in profile.urdf_limit_policies.items():
if name not in all_joints or not isinstance(policy, JointLimitPolicy):
errors.append(f"URDF limit policy requires a known movable joint:{name}")
continue
if policy.source_limits != "calibrated_motion_envelope" or not policy.evidence_source.strip():
errors.append(f"URDF motion envelope requires explicit source-limit interpretation:{name}")
if not {"limit.lower", "limit.upper"} <= set(profile.urdf_authorized_fields.get(name, ())):
errors.append(f"URDF motion envelope lacks limit authorization:{name}")
bounds = policy.independent_safety_limits_cad_rad
if bounds is None:
if policy.safety_evidence_source:
errors.append(f"URDF safety evidence has no independent bounds:{name}")
elif (len(bounds) != 2 or not all(math.isfinite(v) for v in bounds)
or bounds[0] >= bounds[1] or not policy.safety_evidence_source.strip()):
errors.append(f"URDF independent safety bounds require finite limits and evidence:{name}")
acquisition = profile.acquisition
if acquisition.command_capture_mode not in {"interleaved", "separate"}:
errors.append("unsupported command capture mode")
task_by_key = {task.key: task for task in profile.motion.tasks}
for key, values in acquisition.steady_extra_training_nodes.items():
task = task_by_key.get(key)
if task is None or any(not math.isfinite(value) or not min(task.start_value, task.end_value) < value < max(task.start_value, task.end_value) for value in values):
errors.append(f"extra steady nodes must lie inside a declared task:{key}")
if acquisition.steady_training_nodes < 3 or acquisition.steady_minimum_samples < 3:
errors.append("steady mapping requires at least three nodes and three images per node")
if not acquisition.steady_timeout_seconds <= acquisition.joint_zero_timeout_seconds <= 10.0:
errors.append("joint zero capture timeout must be between the steady timeout and 10 seconds")
if not 0 < acquisition.steady_window_seconds < acquisition.steady_timeout_seconds <= 2.0:
errors.append("steady acquisition must have a finite stability window and <=2 second wait")
if acquisition.policy_version != ACQUISITION_POLICY_VERSION:
src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py
+84 -10
View File
@@ -2,8 +2,9 @@
from __future__ import annotations
from dataclasses import asdict, dataclass
from dataclasses import asdict, dataclass, replace
import math
import re
from typing import Mapping, Sequence
import numpy as np
@@ -46,6 +47,22 @@ class JointZeroSample:
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
@@ -55,6 +72,7 @@ class JointZeroReference:
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])
@@ -81,7 +99,57 @@ class JointZeroReference:
**{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)
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):
@@ -91,9 +159,19 @@ def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, sessi
secondary = profile.measurement.cross_view_sources.get(joint)
if secondary:
views.add(profile.measurement.measurements[secondary].view)
samples = tuple(JointZeroSample.from_row(row, profile.command.unit) for row in rows)
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}")
@@ -105,14 +183,8 @@ def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, sessi
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}")
minimum = profile.acquisition.fixed_reference_minimum_frames
for view in views:
selected = [s for s in samples if s.view == view]
if len({s.sample_id for s in selected}) < 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}:{len(selected)}/{minimum}")
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}")
@@ -132,7 +204,7 @@ def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, sessi
if sample.arrival_directions[task.command_index] != expected:
raise ValueError(f"joint_zero_arrival_changed:{joint}")
return JointZeroReference(joint, spec.task_key, profile.command.baseline_values[task.command_index],
task.command_index, session_epoch, motion_version, samples)
task.command_index, session_epoch, motion_version, samples, _branch_references(profile, joint, rows))
def read_joint_zero_references(profile, records):
@@ -164,6 +236,8 @@ def read_joint_zero_references(profile, records):
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
src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py
+3 -1
View File
@@ -82,7 +82,9 @@ class JointMapping:
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
if self.input_domain.endswith("_u8"):
# 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))
src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py
+9
View File
@@ -3,9 +3,18 @@
import numpy as np
def command_training_cycles(profile):
return (4,) if profile.acquisition.command_capture_mode == "separate" else (0, 1, 2)
def command_holdout_cycle(profile):
return 5 if profile.acquisition.command_capture_mode == "separate" else 3
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:
baseline = profile.command.baseline_values[task.command_index]
if not lo <= baseline <= hi:
src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py
+1
View File
@@ -13,6 +13,7 @@ class TaskStatus:
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
)
src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py
+24 -11
View File
@@ -12,11 +12,13 @@ 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
from ..domain.sampling import command_nodes, command_training_cycles, command_holdout_cycle
def fit_command_mappings(profile, fit, records_by_joint):
mappings, metrics, applicability = {}, {}, {}
training_cycles = command_training_cycles(profile)
holdout_cycle = command_holdout_cycle(profile)
for joint in sorted(fit.output_mappings):
donor = joint if profile.artifacts.output_schema_version >= 3 else profile.zero.transferred_zero_sources.get(joint, joint)
rows = records_by_joint.get(donor, ())
@@ -36,9 +38,11 @@ def fit_command_mappings(profile, fit, records_by_joint):
channel = fit.output_mappings[joint].motor_index
source_channel = fit.output_mappings[donor].motor_index
branches = {}
directional = joint in profile.artifacts.directional_command_joints
task = next(t for t in profile.motion.tasks if donor in t.joints)
lo, hi = sorted((task.start_value, task.end_value))
knots = command_nodes(profile, task)
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:
sign = 1 if curve.angle_rad[-1] > curve.angle_rad[0] else -1
@@ -46,7 +50,7 @@ def fit_command_mappings(profile, fit, records_by_joint):
values = []
for knot in knots:
medians = []
for cycle in (0, 1, 2):
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:
@@ -61,6 +65,11 @@ def fit_command_mappings(profile, fit, records_by_joint):
if np.max(np.abs(projected-values)) > 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,
fit.joint_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
@@ -68,7 +77,7 @@ def fit_command_mappings(profile, fit, records_by_joint):
fit.joint_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)
validation = [r for r in rows if r["cycle"] == 3]
validation = [r for r in rows if r["cycle"] == holdout_cycle]
expected = command_nodes(profile, task, holdout=True)
for direction in ("increasing", "decreasing"):
for knot in set(expected):
@@ -76,7 +85,7 @@ def fit_command_mappings(profile, fit, records_by_joint):
for r in validation) < profile.acquisition.steady_minimum_samples:
raise ValueError(f"steady_holdout_incomplete:{joint}:{direction}:{knot}")
errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]),
"" if profile.artifacts.output_schema_version >= 3 else r["direction"])
r["direction"] if directional or profile.artifacts.output_schema_version < 3 else "")
- observed[r["sample_id"]] for r in validation]
quality = angular_metrics(errors)
if not quality.passed:
@@ -84,14 +93,15 @@ def fit_command_mappings(profile, fit, records_by_joint):
mappings[joint] = mapping
from dataclasses import asdict
metrics[joint] = {**asdict(quality), "independently_measured": donor == joint,
"source_joint": donor, "training_cycles": [0, 1, 2], "holdout_cycle": 3}
"source_joint": donor, "training_cycles": list(training_cycles), "holdout_cycle": holdout_cycle,
"training_sample_ids": sorted(r["sample_id"] for r in rows if r["cycle"] in training_cycles)}
if profile.artifacts.output_schema_version >= 3:
directional = {direction: angular_metrics([error for error, row in zip(errors, validation)
branch_metrics = {direction: angular_metrics([error for error, row in zip(errors, validation)
if row["direction"] == direction]) for direction in ("increasing", "decreasing")}
if not all(value.passed for value in directional.values()):
raise ValueError(f"single_command_curve_direction_holdout_failed:{joint}")
metrics[joint]["by_direction"] = {key: asdict(value) for key, value in directional.items()}
training = [r for r in rows if r["cycle"] in {0, 1, 2}]
if not all(value.passed for value in branch_metrics.values()):
raise ValueError(f"command_curve_direction_holdout_failed:{joint}")
metrics[joint]["by_direction"] = {key: asdict(value) for key, value in branch_metrics.items()}
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 != source_channel}
@@ -101,5 +111,8 @@ def fit_command_mappings(profile, fit, records_by_joint):
applicability[joint] = {"task": task.key, "held_command_values": held,
"scope": "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False,
"branch_initialization": "full_range_endpoint_reset"}
if directional:
applicability[joint].update(command_mapping="directional", direction_source="raw_sdk_input",
interior_reversals_validated=False)
return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics,
command_applicability=applicability)
src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py
+141
View File
@@ -0,0 +1,141 @@
"""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
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):
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 {0, 1, 2} and row.get("direction"):
key = (row["cycle"], row["direction"])
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 {0, 1, 2}
or row.get("sample_phase", "sweep") != "sweep" or "relative_quaternion_xyzw" not in row):
continue
if int(row.get("attempt", 1)) != latest.get((row["cycle"], row["direction"])):
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"],
"input": float(row[f"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 = []
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"feedback_{profile.command.unit}",
sdk_to_joint_direction=profile.command.joint_directions[task.command_index])
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": [0, 1, 2],
"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
src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py
+42 -1
View File
@@ -106,6 +106,7 @@ def fit_standard_mimic(
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.
@@ -139,8 +140,16 @@ def fit_standard_mimic(
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)
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])
@@ -149,6 +158,38 @@ def fit_standard_mimic(
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
src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py
+69 -7
View File
@@ -1,9 +1,9 @@
"""A training-only URDF approximation, separate from measured joint curves."""
"""Training-only URDF approximations, separate from exact JSON lookup tables."""
from dataclasses import dataclass
from dataclasses import dataclass, replace
import math
from .coupling import LinearMimicFit, fit_standard_mimic
from .coupling import LinearMimicFit, fit_standard_mimic, fit_range_bounded_mimic
from .observed_motion import paired_visual_observations
@@ -14,16 +14,43 @@ class MimicApproximation:
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 fit_mimic_approximations(model, curves, records):
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
@@ -36,10 +63,45 @@ def fit_mimic_approximations(model, curves, records):
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]
fit = fit_standard_mimic(parent, name, [r.source_rad for r in training],
[r.target_rad for r in training], cycles=[r.cycle for r in training], offset_rad=0.0)
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-r.target_rad for r in holdout))
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))
src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py
+6 -3
View File
@@ -89,7 +89,7 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
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))
reference_xyzw=reference.quaternion(view), joint=name, view=view)
references[name] = reference.input_reference(view, feedback=True)
coordinates = observed
elif profile.command.unit == "u8":
@@ -105,7 +105,8 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
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))
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)])
@@ -124,7 +125,9 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
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))
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)
src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_transfer.py
+3 -1
View File
@@ -42,7 +42,9 @@ def expand_profile_transfers(profile: CalibrationProfile, fit: CalibrationResult
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"
if target in profile.zero.passive_joints:
# 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]
src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py
+7 -3
View File
@@ -10,6 +10,7 @@ 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:
@@ -44,7 +45,8 @@ def paired_visual_observations(source_rows, target_rows, source_fit, target_fit)
return output
def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction, reference_xyzw=None):
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.
@@ -60,10 +62,12 @@ def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_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)
errors = fitted.holdout_errors([row for row in observations if row.cycle == 3])
holdout = [row for row in observations if row.cycle == 3]
errors = fitted.holdout_errors(holdout)
metrics = angular_metrics(errors)
if not metrics.passed:
raise ValueError("independent_rotation_holdout_failed")
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,
src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_diagnostics.py
+78
View File
@@ -0,0 +1,78 @@
"""Explain a failed frozen rotation holdout without changing its acceptance."""
from __future__ import annotations
from dataclasses import dataclass
import math
from typing import Sequence
from ..urdf.acceptance import ErrorMetrics, angular_metrics
from .rotation_curve import RotationCurve, RotationObservation
@dataclass(frozen=True)
class RotationHoldoutSample:
sample_id: str
cycle: int
direction: str
input_value: float
full_so3_error_deg: float
axial_error_deg: float
off_axis_error_deg: float
@dataclass(frozen=True)
class RotationHoldoutDiagnostic:
joint: str | None
view: str | None
input_domain: str
full_so3: ErrorMetrics
axial: ErrorMetrics
off_axis: ErrorMetrics
worst_sample: RotationHoldoutSample
training_cycles: tuple[int, ...] = (0, 1, 2)
holdout_cycle: int = 3
class RotationHoldoutFailure(ValueError):
"""A rejected rotation fit with serializable, read-only sample evidence."""
def __init__(self, diagnostics: RotationHoldoutDiagnostic):
self.diagnostics = diagnostics
metrics = diagnostics.full_so3
super().__init__(
"independent_rotation_holdout_failed:"
f"joint={diagnostics.joint or 'unknown'}:view={diagnostics.view or 'unknown'}:"
f"mae_deg={metrics.mae_deg:.6f}:p95_deg={metrics.p95_deg:.6f}:"
f"max_deg={metrics.maximum_deg:.6f}")
def rotation_holdout_diagnostic(
fitted: RotationCurve,
observations: Sequence[RotationObservation],
errors_rad: Sequence[float],
metrics: ErrorMetrics,
*,
joint: str | None,
view: str | None,
) -> RotationHoldoutDiagnostic:
"""Describe the existing failed fit; never fit, re-zero or filter samples.
Axial error compares projected log angles. Off-axis error measures the
observed rotation's departure from the frozen hinge axis. These explain
different effects; their magnitudes are not additive SO(3) components.
Only the original full SO(3) metrics determine acceptance.
"""
measurements = [fitted.measure(row.quaternion_xyzw) for row in observations]
axial = [angle - fitted.predict(row.input_value, row.direction)
for row, (angle, _) in zip(observations, measurements)]
off_axis = [error for _, error in measurements]
worst_index = max(range(len(errors_rad)), key=errors_rad.__getitem__)
worst = observations[worst_index]
return RotationHoldoutDiagnostic(
joint, view, fitted.input_domain, metrics,
angular_metrics(axial), angular_metrics(off_axis),
RotationHoldoutSample(
worst.sample_id, worst.cycle, worst.direction, worst.input_value,
math.degrees(errors_rad[worst_index]), math.degrees(axial[worst_index]),
math.degrees(off_axis[worst_index])))
src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py
+8 -5
View File
@@ -32,7 +32,7 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi
"directed_base_axis_joints", "depth_free_axis_projection", "axis_order",
"same_view_axis_pair_by_offset",
"parallel_root_pattern",
"accept_validated_zero_in_confidence_interval", "project_axis_gauge_before_image"}
"accept_validated_zero_in_confidence_interval"}
if not data or set(data) - allowed:
raise ValueError("spatial observation graph is missing or has unsupported fields")
direct = tuple(profile.zero.direct_zero_joints)
@@ -42,6 +42,12 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi
axes = tuple(data.get("axis_order", profile.zero.axis_joints))
if set(axes) != set(profile.zero.axis_joints) or len(set(axes)) != len(axes):
raise ValueError("spatial axis order differs from declared observations")
# Coordinate descent must solve an upstream zero before the downstream
# phase that depends on it. The declaration of required zeros is a set of
# requirements, not an optimization order (O6 used pitch before yaw).
if not set(direct) <= set(axes):
raise ValueError("spatial zero has no position in the declared axis order")
direct = tuple(name for name in axes if name in direct)
return ZeroCalibrationProfile(
hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints),
stable_cross_view_cone_bias=profile.measurement.stable_cross_view_cone_bias),
@@ -60,7 +66,6 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi
orientation_anchor_joint=data.get("orientation_anchor_joint"),
directed_base_axis_joints=frozenset(data.get("directed_base_axis_joints", ())),
accept_validated_zero_in_confidence_interval=bool(data.get("accept_validated_zero_in_confidence_interval", False)),
project_axis_gauge_before_image=bool(data.get("project_axis_gauge_before_image", False)),
parallel_root_pattern=bool(data.get("parallel_root_pattern", False)),
independent_observed_motion=profile.artifacts.output_schema_version >= 3)
@@ -136,7 +141,6 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
offsets = {name: float(zero.direct_offsets_rad[name]) for name in profile.zero.direct_zero_joints}
methods = {name: "urdf_serial_axis_geometry" for name in offsets}
if measured:
from .mimic_approximation import fit_mimic_approximations
for name, datum in profile.zero.known_baseline_geometry.items():
offsets[name], methods[name] = datum.cad_angle_rad, "independent_known_baseline_geometry"
for name in profile.zero.cad_zero_assumptions:
@@ -145,8 +149,7 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
tuple(curve.circle["input_knots"]), curve.angle_rad, curve.increasing_rad, curve.decreasing_rad)
for name, curve in curves.items()}
return CalibrationResult(curves, mappings, offsets, methods, {}, motion.holdout_errors_rad, zero,
motion.cross_view_metrics, motion.reference_inputs, joint_zero_references=dict(zero_references),
mimic_approximations=fit_mimic_approximations(model, curves, records_by_joint))
motion.cross_view_metrics, motion.reference_inputs, joint_zero_references=dict(zero_references))
for name in profile.zero.active_joints - offsets.keys() - profile.zero.transferred_zero_sources.keys():
if name not in profile.zero.cad_frozen_joints:
raise ValueError(f"active static zero has no measured or CAD-retention policy:{name}")
src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py
+4 -13
View File
@@ -93,7 +93,6 @@ class ObservationGeometry:
observed_child_point: np.ndarray,
view_normal_common: Sequence[float] | None = None,
) -> float:
profile = self.profile
parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis)
observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis)
if float(parent_axis @ observed_axis) < 0.0:
@@ -129,19 +128,11 @@ class ObservationGeometry:
# mounts therefore choose different along-axis gauges.
# Reusing delta here would reintroduce that arbitrary
# coordinate and turn it into a phase at oblique views.
predicted_image_input = (
predicted_radial if profile.project_axis_gauge_before_image
else predicted_delta
predicted_radial = predicted_radial - view_normal * float(
predicted_radial @ view_normal
)
observed_image_input = (
observed_radial if profile.project_axis_gauge_before_image
else observed_delta
)
predicted_radial = predicted_image_input - view_normal * float(
predicted_image_input @ view_normal
)
observed_radial = observed_image_input - view_normal * float(
observed_image_input @ view_normal
observed_radial = observed_radial - view_normal * float(
observed_radial @ view_normal
)
angle_axis = view_normal
if float(angle_axis @ parent_axis) < 0.0:
src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py
-5
View File
@@ -58,11 +58,6 @@ class ZeroCalibrationProfile:
# contains it. The frozen zero still faces every geometry/cycle/holdout
# check below. Default False preserves existing G20/L6/O6 decisions.
accept_validated_zero_in_confidence_interval: bool = False
# Axis-line points have no unique coordinate along the axis. Remove that
# gauge before projecting a parallel-axis phase into the camera plane.
# Opt in explicitly while legacy profiles retain their reviewed policy.
project_axis_gauge_before_image: bool = False
parallel_root_pattern: bool = False
common_mode_zero_joints: tuple[str, ...] = ()
small_offset_joints: frozenset[str] = frozenset()
src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py
+18 -4
View File
@@ -61,6 +61,17 @@ class FrozenTagInstallation:
training_translation_p95_m: float
class TagInstallationFailure(ValueError):
"""All role residuals from one failed rigid-chain validation."""
def __init__(self, diagnostics):
self.diagnostics = diagnostics
failed = [name for name, row in diagnostics.items() if not row["passed"]]
super().__init__("tag_installation_not_rigid:" + ";".join(
f"{name}:rotation_p95_deg={diagnostics[name]['rotation_p95_deg']:.6f}:"
f"translation_p95_m={diagnostics[name]['translation_p95_m']:.6f}" for name in failed))
def fit_tag_installations(
*, source_model: UrdfKinematicModel, common_from_base,
link_by_role: Mapping[str, str], observations: Sequence[TagTrainingPose],
@@ -84,8 +95,8 @@ def fit_tag_installations(
if previous is not None and (previous[0] != row.cycle or not np.allclose(previous[1], candidate, atol=1e-8, rtol=0)):
raise ValueError(f"conflicting duplicate Tag image:{row.role}:{row.sample_id}")
grouped[row.role][row.sample_id] = (row.cycle, candidate)
output = {}
for role, unique in grouped.items():
output, diagnostics = {}, {}
for role, unique in sorted(grouped.items()):
if len(unique) < minimum_samples or {value[0] for value in unique.values()} != {0, 1, 2}:
raise ValueError(f"Tag installation lacks three-cycle training evidence:{role}")
matrices = np.asarray([value[1] for value in unique.values()])
@@ -96,12 +107,15 @@ def fit_tag_installations(
distances = np.linalg.norm(matrices[:, :3, 3] - translation, axis=1)
rotation_p95 = float(np.rad2deg(np.percentile(errors, 95)))
translation_p95 = float(np.percentile(distances, 95))
if rotation_p95 > 2.0 or translation_p95 > 0.003:
raise ValueError(f"Tag installation is not rigid across training tasks:{role}:rotation_p95_deg={rotation_p95}:translation_p95_m={translation_p95}")
diagnostics[role] = {"count": len(unique), "rotation_p95_deg": rotation_p95,
"translation_p95_m": translation_p95,
"passed": rotation_p95 <= 2.0 and translation_p95 <= 0.003}
transform = np.eye(4)
transform[:3, :3], transform[:3, 3] = rotation.as_matrix(), translation
output[role] = FrozenTagInstallation(role, link_by_role[role], matrix_tuple(transform),
tuple(sorted(unique)), rotation_p95, translation_p95)
if any(not row["passed"] for row in diagnostics.values()):
raise TagInstallationFailure(diagnostics)
return output
src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py
+123 -2
View File
@@ -5,11 +5,17 @@ poses, substitute SDK angles, or relax the spatial solver's acceptance gates.
"""
from itertools import product
import math
from collections.abc import Mapping
import numpy as np
from scipy.spatial.transform import Rotation as R
from .pnp import solve_square_tag_ippe
from .tag_pose.ippe import fixed_pose_reprojection_error
from .tag_pose.branch_confirmation import BranchConfirmation
from .tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS, FROZEN_BRANCH_POLICY_VERSIONS
from .tag_pose.types import SquareTagPose
from .image_motion_replay import IMAGE_POSE_SOURCE, has_image_model, replay_image_models, validate_image_sample
POLICY = 'training_frozen_articulated_candidates_v1'
MATRIX_SOURCE = 'CameraInfo.P[:3,:3]'
@@ -41,15 +47,107 @@ def _trajectory_score(parent, child):
'score': rms/.0015 + off_axis/math.radians(1)}
def _recorded_pose(payload):
if not isinstance(payload, Mapping):
raise ValueError('frozen_branch_evidence_missing:selected_pose')
quaternion = tuple(payload.get('quaternion_xyzw', ()))
translation = tuple(payload.get('translation_xyz_m', ()))
if (len(quaternion) != 4 or len(translation) != 3
or not np.all(np.isfinite(quaternion + translation))
or not np.isclose(np.linalg.norm(quaternion), 1, atol=1e-5)):
raise ValueError('frozen_branch_evidence_invalid:selected_pose')
return SquareTagPose(quaternion, translation, 0.)
def _check_frozen_paths(rows, evidence, selected_paths, role_pairs, fixed_roles):
"""A geometry preference cannot replace any already-frozen image branch.
Revisions identify a role within its capture, not a physical installation
across sessions. JointResume verifies the latter before importing rows.
Compare each path pose with that image's original confirmed pose instead
of assuming numerically equal revisions authorize cross-session reuse.
"""
equivalence = BranchConfirmation(DEFAULT_POSE_TRACKING_PARAMETERS)
for row in rows:
stamp, view = int(row['image_stamp_ns']), row.get('view')
frame = evidence[stamp]
identity = f"joint={row['joint']}:view={view}:image={stamp}:cycle={row['cycle']}"
if (not view or frame.get('view') != view or any(
frame.get(key) != row.get(key) for key in ('cycle', 'direction', 'attempt'))):
raise ValueError(f'frozen_branch_frame_identity_changed:{identity}')
observed = row.get('pnp_observation_evidence', {})
if not isinstance(observed, Mapping):
raise ValueError(f'frozen_branch_evidence_missing:{identity}')
for field, role in zip(('parent', 'child'), role_pairs[row['joint']]):
item = observed.get(field, {})
if not isinstance(item, Mapping) or item.get('tag_role') != role:
raise ValueError(f'frozen_branch_evidence_missing:{identity}:role={role}')
source = item.get('pose_source')
fixed_source = source in {'cached_fixed_reference', 'verified_fixed_reference'}
if (fixed_source
!= bool(frame['roles'][role].get('locked_reference'))):
raise ValueError(f'frozen_branch_pose_source_changed:{identity}:role={role}')
if fixed_source and role not in fixed_roles:
raise ValueError(f'frozen_branch_fixed_role_invalid:{identity}:role={role}')
if source == 'cached_fixed_reference' and role in fixed_roles:
revision = item.get('reference_branch_revision')
else:
diagnostics = item.get('candidate_diagnostics', {})
if (source not in {'current_image', 'verified_fixed_reference', IMAGE_POSE_SOURCE} or not isinstance(diagnostics, Mapping)
or diagnostics.get('observation_stamp_ns') != stamp
or diagnostics.get('branch_status') != 'tracking'
or diagnostics.get('branch_frozen') is not True):
raise ValueError(f'frozen_branch_evidence_unconfirmed:{identity}:role={role}')
revision = diagnostics.get('branch_revision')
if source == 'verified_fixed_reference' and (
diagnostics.get('fixed_reference_verified') is not True
or item.get('reference_branch_revision') != revision):
raise ValueError(f'frozen_branch_fixed_reference_unverified:{identity}:role={role}')
if isinstance(revision, bool) or not isinstance(revision, int) or revision < 1:
raise ValueError(f'frozen_branch_revision_invalid:{identity}:role={role}')
original = _recorded_pose(item.get('selected_pose'))
if source == 'verified_fixed_reference':
_check_fixed_projection(original, item, frame, role, identity)
recorded = _recorded_pose(frame['roles'][role].get('selected'))
if not equivalence.equivalent(original, recorded):
raise ValueError(f'frozen_branch_recorded_pose_changed:{identity}:role={role}')
proposed = _recorded_pose(selected_paths[stamp][role])
if not equivalence.equivalent(original, proposed):
angle, translation = equivalence.distance(original, proposed)
raise ValueError(f'frozen_branch_reselection_forbidden:{identity}:role={role}:'
f'rotation_deg={math.degrees(angle):.6f}:translation_mm={translation*1000:.6f}')
def _check_fixed_projection(pose, item, frame, role, identity):
"""A fixed-pose marker cannot bypass current-image geometric evidence."""
data = frame['roles'][role]
try:
if not np.array_equal(np.asarray(item['corners_xy']), np.asarray(data['corners_xy'])):
raise ValueError('corners changed')
error = fixed_pose_reprojection_error(pose, data['corners_xy'],
tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=frame['camera_matrix'])
limit = float(data.get('maximum_reprojection_error_px', 1.5))
if not math.isfinite(limit) or limit <= 0 or not math.isfinite(error) or error > limit:
raise ValueError('fixed pose no longer explains this image')
except (KeyError, TypeError, ValueError) as exc:
raise ValueError(f'frozen_branch_fixed_projection_invalid:{identity}:role={role}') from exc
def resolve_chain_observations(records, *, task_key, role_pairs,
projection_override=None, solve_pose=solve_square_tag_ippe, fixed_roles=()):
projection_override=None, solve_pose=solve_square_tag_ippe, fixed_roles=(),
require_frozen_branches=False):
"""Return copied observations and auditable candidate-selection evidence.
Older captures did not save P. They remain on the legacy path; K is never
guessed to be P. An explicit override is for isolated offline diagnosis
and must be reported as such, not advertised as a verified whole session.
A current capture automatically preserves its frozen branches; production
callers enforce this even when a journal's policy header was removed.
"""
rows = [dict(r) for r in records]
preserve_frozen = require_frozen_branches or any(
row.get('kind') == 'session_start'
and row.get('pose_tracking_policy_version') in FROZEN_BRANCH_POLICY_VERSIONS for row in rows)
roles = tuple(dict.fromkeys(role for pair in role_pairs.values() for role in pair))
if not role_pairs or len(roles) > 8 or any(len(pair) != 2 or pair[0] == pair[1] for pair in role_pairs.values()):
raise ValueError('candidate selection requires a bounded articulated observation graph')
@@ -62,6 +160,8 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
'projection_override_used': projection_override is not None,
'is_accuracy_certificate': False}
if not evidence:
if preserve_frozen:
raise ValueError(f'frozen_branch_evidence_missing:task={task_key}:candidate_frames')
return rows, {**report, 'status': 'legacy_no_corners'}
if projection_override is None and any(
r.get('camera_matrix_source') != MATRIX_SOURCE for r in evidence.values()):
@@ -81,6 +181,23 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
raise ValueError('corner replay requires training and independent holdout')
if np.flatnonzero(training)[-1] > np.flatnonzero(~training)[0]:
raise ValueError('holdout must follow the complete training trajectory')
if any(has_image_model(item) for stamp in stamps for item in evidence[stamp]['roles'].values()):
if projection_override is not None:
raise ValueError('frozen image model cannot use a different camera projection')
lookup, verified = {}, {}
for stamp in stamps:
verified[stamp] = replay_image_models(evidence[stamp])
if not verified[stamp]:
raise ValueError('image_motion_provenance_mixed_pose_policies')
lookup[stamp] = {role: evidence[stamp]['roles'][role]['selected'] for role in roles}
for row in chosen_rows:
stamp = int(row['image_stamp_ns'])
validate_image_sample(row, evidence[stamp], verified[stamp])
_check_frozen_paths(chosen_rows, evidence, lookup, role_pairs, fixed_roles)
return rows, {**report, 'policy': 'frozen_image_motion_replay_v1', 'status': 'resolved',
'frozen_branches_preserved': True, 'geometry_refitted': False,
'training_frames': int(sum(training)), 'holdout_frames': int(sum(~training)),
'changed_joint_rows': 0}
paths = {role: [[], []] for role in roles}
for stamp in stamps:
frame = evidence[stamp]
@@ -90,7 +207,8 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
if data.get('locked_reference'):
if role not in fixed_roles or not data.get('selected'):
raise ValueError('cached fixed reference has no frozen pose')
cs = [dict(data['selected'], reprojection_error_px=0.0)]
cs = [dict(data['selected'],
reprojection_error_px=float(data['selected'].get('reprojection_error_px', 0.0)))]
else:
poses = solve_pose(data['corners_xy'], tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=matrix)
limit = float(data.get('maximum_reprojection_error_px', 1.5))
@@ -120,6 +238,9 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
selected = min(scores, key=lambda s: s['score'])
lookup = {stamp: {role: paths[role][branch][i] for role, branch in zip(roles, selected['branches'])}
for i, stamp in enumerate(stamps)}
if preserve_frozen:
_check_frozen_paths(chosen_rows, evidence, lookup, role_pairs, fixed_roles)
report['frozen_branches_preserved'] = True
changed = 0
for row in chosen_rows:
stamp = int(row['image_stamp_ns'])
src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py
+167
View File
@@ -0,0 +1,167 @@
"""Read back image-constrained poses against their frozen model and raw corners.
This never trains geometry or chooses a new trajectory. The image solver may
estimate only this image's joint angles, including on the independent holdout.
Raw IPPE candidates remain separate from the selected constrained poses.
"""
from collections.abc import Mapping
import hashlib
import json
import numpy as np
from scipy.spatial.transform import Rotation
from .tag_pose.motion_evidence import MotionEvidenceFrame, RoleCandidates
from .tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation
from .tag_pose.types import SquareTagPose
IMAGE_POSE_SOURCE = "image_constrained_current_image"
def image_model_sha256(payload):
encoded = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False)
return hashlib.sha256(encoded.encode()).hexdigest()
def frozen_image_model(item):
"""Read a named, content-bound model; a marker alone grants no permission."""
from .tag_pose.production_image_motion import image_motion_model_from_dict
payload, identity = item.get("frozen_image_model"), item.get("image_model_sha256")
if not isinstance(payload, Mapping) or image_model_sha256(payload) != identity:
raise ValueError("image_motion_provenance_model_hash_invalid")
return image_motion_model_from_dict(payload)
def has_image_model(item):
return (item.get("pose_source") == IMAGE_POSE_SOURCE
or "frozen_image_model" in item or "image_model_sha256" in item)
def _pose(payload):
try:
q, t = tuple(payload["quaternion_xyzw"]), tuple(payload["translation_xyz_m"])
error = float(payload["reprojection_error_px"])
if (len(q) != 4 or len(t) != 3 or not np.all(np.isfinite((*q, *t, error)))
or not np.isclose(np.linalg.norm(q), 1., atol=1e-5) or error < 0):
raise ValueError("invalid pose")
return SquareTagPose(q, t, error)
except (KeyError, TypeError, ValueError) as exc:
raise ValueError("image_motion_provenance_pose_invalid") from exc
def _same_pose(actual, expected):
return (float((Rotation.from_quat(actual.quaternion_xyzw).inv()
* Rotation.from_quat(expected.quaternion_xyzw)).magnitude()) <= 1e-6
and np.linalg.norm(np.subtract(actual.translation_xyz_m, expected.translation_xyz_m)) <= 1e-8
and abs(actual.reprojection_error_px - expected.reprojection_error_px) <= 1e-6)
def replay_image_models(frame):
"""Recompute every constrained role from the original frame, without fitting."""
from .tag_pose.production_image_motion import select_image_motion_frame
if (frame.get("camera_matrix_source") != "CameraInfo.P[:3,:3]"
or frame.get("input_is_rectified") is not True):
raise ValueError("image_motion_provenance_projection_unverified")
models, selected = {}, {}
for role, item in frame["roles"].items():
if not has_image_model(item):
continue
if item.get("pose_source") != IMAGE_POSE_SOURCE:
raise ValueError("image_motion_provenance_pose_source_changed")
identity = item.get("image_model_sha256")
model = frozen_image_model(item)
models[identity] = model
if role not in {geometry.relation.child_role for geometry in model.geometry}:
raise ValueError("image_motion_provenance_role_not_in_model")
for identity, model in models.items():
if (max(model.source_stamps_ns) >= frame["image_stamp_ns"]
or not np.array_equal(np.asarray(frame["camera_matrix"]), model.camera_matrix)):
raise ValueError("image_motion_provenance_model_not_frozen_before_image")
children = {geometry.relation.child_role for geometry in model.geometry}
roles, observations = [], []
for role, size in model.tag_sizes:
item = frame["roles"].get(role)
if item is None or float(item.get("tag_size_m", -1)) != size:
raise ValueError("image_motion_provenance_role_or_size_changed")
if role in children:
if (item.get("pose_source") != IMAGE_POSE_SOURCE
or item.get("image_model_sha256") != identity):
raise ValueError("image_motion_provenance_chain_model_changed")
candidates = tuple(_pose(pose) for pose in item.get("reprojection_valid_candidates", ()))
if not candidates:
raise ValueError("image_motion_provenance_raw_candidates_missing")
else:
# A root may itself be an earlier frozen moving parent. Its
# current selected pose is used, never its previous image.
diagnostics = item.get("candidate_diagnostics", {})
if (item.get("pose_source") not in {"verified_fixed_reference", IMAGE_POSE_SOURCE}
or diagnostics.get("branch_frozen") is not True
or diagnostics.get("branch_status") != "tracking"
or diagnostics.get("observation_stamp_ns") != frame["image_stamp_ns"]):
raise ValueError("image_motion_provenance_root_not_frozen_in_current_image")
candidates = (_pose(item["selected"]),)
points = item.get("corners_xy")
if points is not None:
array = np.asarray(points, dtype=float)
if array.shape != (4, 2) or not np.all(np.isfinite(array)):
raise ValueError("image_motion_provenance_corners_invalid")
observations.append(ImageRoleObservation(role, size, tuple(map(tuple, array))))
elif role in children:
raise ValueError("image_motion_provenance_corners_missing")
roles.append(RoleCandidates(role, candidates, role not in children))
result = select_image_motion_frame(model, ImageMotionFrame(
MotionEvidenceFrame(frame["image_stamp_ns"], tuple(roles)),
tuple(map(tuple, frame["camera_matrix"])), tuple(observations)))
if not result.resolved:
raise ValueError(f"image_motion_provenance_frame_unresolved:{result.reason}")
by_role = {item.role: item.pose for item in result.selections}
for role in children:
if role not in by_role or not _same_pose(_pose(frame["roles"][role]["selected"]), by_role[role]):
raise ValueError(f"image_motion_provenance_selected_pose_changed:{role}")
selected[role] = by_role[role]
return selected
def validate_image_sample(row, frame, replayed):
"""Bind sample pose, corners, model identity and relative geometry to a frame."""
if (row.get("view") != frame.get("view") or row.get("image_stamp_ns") != frame.get("image_stamp_ns")
or any(row.get(key) != frame.get(key) for key in ("task_name", "cycle", "direction", "attempt"))):
raise ValueError("image_motion_provenance_sample_identity_changed")
evidence = row.get("pnp_observation_evidence", {})
if set(evidence) != {"parent", "child"}:
raise ValueError("image_motion_provenance_sample_roles_missing")
for item in evidence.values():
role = item.get("tag_role")
data = frame["roles"].get(role)
if data is None:
raise ValueError("image_motion_provenance_sample_role_changed")
if not _same_pose(_pose(item["selected_pose"]), _pose(data["selected"])):
raise ValueError("image_motion_provenance_sample_pose_changed")
if has_image_model(item):
if (item.get("pose_source") != IMAGE_POSE_SOURCE or role not in replayed
or item.get("image_model_sha256") != data.get("image_model_sha256")
or image_model_sha256(item["frozen_image_model"]) != data.get("image_model_sha256")
or not np.array_equal(item.get("corners_xy"), data.get("corners_xy"))):
raise ValueError("image_motion_provenance_sample_model_or_corners_changed")
elif has_image_model(data):
raise ValueError("image_motion_provenance_sample_solver_marker_removed")
parent = _pose(evidence["parent"]["selected_pose"])
child = _pose(evidence["child"]["selected_pose"])
rp, rc = Rotation.from_quat(parent.quaternion_xyzw), Rotation.from_quat(child.quaternion_xyzw)
relative = Rotation.from_quat(row["relative_quaternion_xyzw"])
translation = rp.inv().apply(np.subtract(child.translation_xyz_m, parent.translation_xyz_m))
if ((relative.inv()*rp.inv()*rc).magnitude() > 1e-7
or not np.allclose(translation, row["relative_translation_xyz_m"], atol=1e-8, rtol=0)):
raise ValueError("image_motion_provenance_relative_pose_changed")
common_parent, common_child = row["parent_pose_common"], row["child_pose_common"]
rp = Rotation.from_quat(common_parent["quaternion_xyzw"])
rc = Rotation.from_quat(common_child["quaternion_xyzw"])
common_translation = rp.inv().apply(np.subtract(common_child["translation_xyz_m"],
common_parent["translation_xyz_m"]))
if ((relative.inv()*rp.inv()*rc).magnitude() > 1e-7
or not np.allclose(translation, common_translation, atol=1e-8, rtol=0)):
raise ValueError("image_motion_provenance_common_pose_changed")
src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py
+1
View File
@@ -16,3 +16,4 @@ from .tag_pose.rigid_group import SquareTagGroupPoseTracker as SquareTagGroupPos
from .tag_pose.parameters import PoseTrackingParameters as PoseTrackingParameters
from .tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS as DEFAULT_POSE_TRACKING_PARAMETERS
from .tag_pose.parameters import DEFAULT_REPROJECTION_TIE_PX as DEFAULT_REPROJECTION_TIE_PX
from .tag_pose.parameters import POSE_TRACKING_POLICY_VERSION as POSE_TRACKING_POLICY_VERSION
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_border.py
+44
View File
@@ -0,0 +1,44 @@
"""Independent pixel support for a decoded square's four outer edges.
A partially covered AprilTag can still decode without a bit error, while its
quad follows the occluder. Such a quad is not evidence of reference motion.
This check uses the current detected quad, never the locked reference: a fully
visible translated/rotated marker remains eligible for the movement guard.
"""
import cv2
import numpy as np
def inspect_tag_border(gray, corners):
points = np.asarray(corners, dtype=np.float32)
if (gray.ndim != 2 or points.shape != (4, 2) or not np.all(np.isfinite(points))):
return {"passed": False, "reason": "border_image_or_quad_invalid"}
edge = float(np.min(np.linalg.norm(points-np.roll(points, 1, axis=0), axis=1)))
if edge < 12:
return {"passed": False, "reason": "border_image_underresolved"}
# Sample opposite sides of the printed black/white boundary, including
# near-corner portions. A wider band would miss curved occluder edges.
inset = max(.025, 1.25/edge)
u = np.linspace(.05, .95, 25, dtype=np.float32)
def bands(offset):
return np.asarray([np.c_[u, np.full(25, offset)],
np.c_[np.full(25, 1-offset), u], np.c_[u, np.full(25, 1-offset)],
np.c_[np.full(25, offset), u]], dtype=np.float32)
transform = cv2.getPerspectiveTransform(np.float32([[0, 0], [1, 0], [1, 1], [0, 1]]), points)
def sample(normalized):
xy = cv2.perspectiveTransform(normalized.reshape(-1, 1, 2), transform).reshape(4, 25, 2)
if (np.min(xy) < 1 or np.max(xy[:, :, 0]) > gray.shape[1]-2
or np.max(xy[:, :, 1]) > gray.shape[0]-2):
return None
return cv2.remap(gray, xy[:, :, 0], xy[:, :, 1], cv2.INTER_LINEAR).astype(float)
inside, outside = sample(bands(inset)), sample(bands(-inset))
if inside is None or outside is None:
return {"passed": False, "reason": "border_image_out_of_bounds"}
black, white = float(np.percentile(inside, 25)), float(np.percentile(outside, 75))
threshold = (black+white)/2
support = np.mean((inside < threshold) & (outside > threshold), axis=1)
passed = white-black >= 30 and bool(np.all(support >= .9))
return {"passed": passed, "reason": "" if passed else "tag_border_incomplete",
"policy": "current_quad_border_v1", "edge_support": support.tolist(),
"contrast_gray": white-black, "minimum_edge_support": .9, "minimum_contrast_gray": 30}
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/branch_confirmation.py
+299
View File
@@ -0,0 +1,299 @@
"""Image-evidence confirmation and frozen-reference branch consistency.
Confirmation is repeatable image evidence, not a claim of physical truth.
After a reference is frozen, conflicting evidence must invalidate capture;
it cannot authorize a different planar solution for the same reference.
"""
from dataclasses import dataclass
import math
from typing import Sequence
import numpy as np
from .ippe import rotation_distance_rad
from .fixed_reference import FixedTagReference
from .parameters import BRANCH_TRANSLATION_EQUIVALENCE_M, PoseTrackingParameters
from .types import SquareTagPose
@dataclass
class PendingBranch:
pose: SquareTagPose
started_ns: int
count: int = 1
@dataclass
class BranchState:
observed_ns: int = 0
pose: SquareTagPose | None = None
revision: int = 0
pending: PendingBranch | None = None
frozen: bool = False
recovery_required: bool = False
conflict: bool = False
status: str = "initializing"
reason: str = "pose_branch_unconfirmed"
motion_evidence_id: str | None = None
class BranchConfirmation:
"""Mutable metadata, accessed only under the owning tracker's short lock."""
def __init__(self, parameters: PoseTrackingParameters):
self.parameters = parameters
count = parameters.branch_confirmation_frames
if isinstance(count, bool) or not isinstance(count, int) or count < 2:
raise ValueError("branch_confirmation_frames must be an integer >= 2")
for name in ("branch_confirmation_minimum_seconds", "branch_equivalence_rad"):
value = getattr(parameters, name)
if not math.isfinite(value) or value <= 0:
raise ValueError(f"{name} must be finite and positive")
if parameters.branch_equivalence_rad > math.radians(1.0):
raise ValueError("branch_equivalence_rad must not exceed 1 degree")
self.states: dict[str, BranchState] = {}
self.corrections: dict[str, int] = {}
def reset(self):
self.states.clear()
self.corrections.clear()
def distance(self, first: SquareTagPose, second: SquareTagPose) -> tuple[float, float]:
return (
rotation_distance_rad(first.quaternion_xyzw, second.quaternion_xyzw),
float(np.linalg.norm(np.asarray(first.translation_xyz_m) - second.translation_xyz_m)),
)
def continuous(self, first: SquareTagPose, second: SquareTagPose) -> bool:
angle, translation = self.distance(first, second)
return (angle <= self.parameters.maximum_pose_jump_rad
and translation <= self.parameters.maximum_translation_jump_m)
def equivalent(self, first: SquareTagPose, second: SquareTagPose) -> bool:
angle, translation = self.distance(first, second)
# A 4 cm image acceptance limit is not equivalence of two solutions.
return (angle <= self.parameters.branch_equivalence_rad
and translation <= BRANCH_TRANSLATION_EQUIVALENCE_M)
def nearest(self, reference: SquareTagPose, candidates: Sequence[SquareTagPose]) -> SquareTagPose:
def cost(candidate):
angle, translation = self.distance(reference, candidate)
return (angle / self.parameters.maximum_pose_jump_rad
+ translation / self.parameters.maximum_translation_jump_m)
return min(candidates, key=cost)
@staticmethod
def reject(state: BranchState, reason: str, status: str) -> tuple[None, str]:
state.status, state.reason = status, reason
return None, reason
@staticmethod
def accept(state: BranchState, selected: SquareTagPose) -> tuple[SquareTagPose, str]:
state.pose = selected
state.pending = None
state.status, state.reason = "tracking", ""
return selected, ""
def confirmed(self, state: BranchState, selected: SquareTagPose, stamp: int,
candidates: Sequence[SquareTagPose]) -> bool:
pending = state.pending
if (pending is None or not self.continuous(pending.pose, selected)
or not self.equivalent(self.nearest(pending.pose, candidates), selected)):
state.pending = PendingBranch(selected, stamp)
return False
pending.pose = selected
pending.count += 1
return (pending.count >= self.parameters.branch_confirmation_frames
and stamp - pending.started_ns >= round(
self.parameters.branch_confirmation_minimum_seconds * 1e9))
def _observe(self, role: str, stamp: int) -> tuple[BranchState | None, bool]:
state = self.states.setdefault(role, BranchState())
if stamp <= state.observed_ns:
return None, False
gap = state.observed_ns > 0 and stamp - state.observed_ns > round(
self.parameters.reset_after_seconds * 1e9)
state.observed_ns = stamp # Rejections are observations, not a lost Tag.
return state, gap
def update_fixed(self, role: str, reference: FixedTagReference,
checked_pose: SquareTagPose | None, stamp: int) -> tuple[SquareTagPose | None, str]:
"""Validate stationarity in the image; never track a new base pose.
The fixed pose was confirmed and frozen before this call. IPPE image
ranking alone cannot contradict the explicit stationary constraint.
A failed image invalidates observations using this reference, not the
frozen pose itself. Actual reference movement is monitored separately;
prolonged missing data is handled by the task's normal quality gates.
"""
state, gap = self._observe(role, stamp)
if state is None:
return None, "pose_observation_not_new"
if (type(reference.branch_revision) is not int or not state.frozen
or state.revision != reference.branch_revision):
return self.reject(state, "pose_branch_reference_invalid", "unavailable")
if state.conflict:
return self.reject(state, "pose_branch_conflict", "frozen_conflict")
if gap:
state.pending = None
state.recovery_required = True
if checked_pose is None:
state.pending = None
state.recovery_required = True
return self.reject(state, "fixed_reference_pose_unverified", "fixed_reference_unavailable")
if state.recovery_required:
if state.status != "recovering":
state.pending = None
if not self.confirmed(state, reference.pose, stamp, (reference.pose,)):
return self.reject(state, "pose_branch_unconfirmed", "recovering")
state.recovery_required = False
return self.accept(state, checked_pose)
def observe_motion(self, role: str, candidates: Sequence[SquareTagPose],
stamp: int) -> tuple[None, str]:
"""Register a frame whose branch will be selected jointly, outside locks.
Recording candidates is not confirmation. In particular, repeated image
winners cannot change a motion-authorized identity while the geometric
selector is evaluating this frame.
"""
state, gap = self._observe(role, stamp)
if state is None:
return None, "pose_observation_not_new"
if state.conflict:
return self.reject(state, "pose_branch_conflict", "frozen_conflict")
if gap:
state.pending = None
if state.frozen or state.motion_evidence_id is not None:
state.recovery_required = True
else:
state.pose = None
if not candidates:
state.pending = None
return self.reject(state, "no_pose_within_reprojection_or_tilt_limit", "unavailable")
# Preserve recovery confirmation between frames; merely receiving a
# frame must not clear PendingBranch before the joint selection commits.
state.reason = "pose_motion_evidence_required"
if not state.recovery_required:
state.status = "motion_pending"
return None, state.reason
def select_motion(self, state: BranchState, selected: SquareTagPose,
evidence_id: str, *, establish: bool) -> tuple[SquareTagPose | None, str]:
"""Apply an externally verified geometric selection to one metadata copy.
The tracker checks image membership and atomically commits all copies.
This method never fits a model or changes an existing evidence identity.
"""
if state.conflict:
return self.reject(state, "pose_branch_conflict", "frozen_conflict")
if state.motion_evidence_id is None:
if state.frozen or not establish:
return self.reject(state, "pose_motion_evidence_required", "motion_unconfirmed")
state.motion_evidence_id = evidence_id
state.revision += 1
state.recovery_required = False
return self.accept(state, selected)
if evidence_id != state.motion_evidence_id:
return self.reject(state, "pose_motion_evidence_changed", "motion_unconfirmed")
if state.pose is None:
return self.reject(state, "pose_branch_reference_invalid", "unavailable")
if state.recovery_required:
if not self.equivalent(state.pose, selected):
state.pending = None
return self.reject(state, "pose_branch_recovery_required", "recovery_required")
if not self.confirmed(state, selected, state.observed_ns, (selected,)):
return self.reject(state, "pose_branch_unconfirmed", "recovering")
state.recovery_required = False
elif not self.continuous(state.pose, selected):
state.pending = None
return self.reject(state, "pose_motion_branch_discontinuous", "motion_unconfirmed")
return self.accept(state, selected)
def update(self, role: str, candidates: Sequence[SquareTagPose], stamp: int,
tie: float) -> tuple[SquareTagPose | None, str]:
state, gap = self._observe(role, stamp)
if state is None:
return None, "pose_observation_not_new"
if state.conflict:
return self.reject(state, "pose_branch_conflict", "frozen_conflict")
if gap:
state.pending = None
if state.frozen:
state.recovery_required = True
else:
state.pose = None
if not candidates:
state.pending = None
return self.reject(state, "no_pose_within_reprojection_or_tilt_limit", "unavailable")
candidates = sorted(candidates, key=lambda c: c.reprojection_error_px)
best = candidates[0]
distinct = [c for c in candidates[1:] if not self.equivalent(best, c)]
decisive = not distinct or all(c.reprojection_error_px > best.reprojection_error_px + tie for c in distinct)
if state.recovery_required:
# Without a temporal path, only a return to the last known pose
# supplies a conservative reference for independent tracking.
near = self.nearest(state.pose, candidates)
if (not self.equivalent(state.pose, near)
or best.reprojection_error_px + tie < near.reprojection_error_px):
state.pending = None
return self.reject(state, "pose_branch_recovery_required", "recovery_required")
if not self.confirmed(state, near, stamp, candidates):
return self.reject(state, "pose_branch_unconfirmed", "recovering")
state.recovery_required = False
return self.accept(state, near)
if state.pose is None:
if not decisive:
state.pending = None
return self.reject(state, "pose_branch_unconfirmed", "initializing")
if not self.confirmed(state, best, stamp, candidates):
return self.reject(state, "pose_branch_unconfirmed", "initializing")
state.revision += 1
return self.accept(state, best)
incumbent = self.nearest(state.pose, candidates)
continuous = self.continuous(state.pose, incumbent)
advantage = best.reprojection_error_px + tie < incumbent.reprojection_error_px
if continuous and (not advantage or self.equivalent(incumbent, best)):
return self.accept(state, incumbent)
# Propagate the incumbent's identity while evidence is rejected. This
# does not emit a sample, and cannot age into a five-second reset.
if continuous:
state.pose = incumbent
if state.frozen and continuous:
# Both image-valid branches still exist and the incumbent has a
# continuous path. A different image-error winner is ambiguity,
# not evidence that the reference moved. Quarantine the image;
# retain this path so later observations can recover without a
# reset, branch switch, or new joint zero.
state.pending = None
return self.reject(state, "pose_branch_ambiguous", "frozen_ambiguous")
if not decisive:
state.pending = None
return self.reject(state, "pose_jump", "unconfirmed")
if not self.confirmed(state, best, stamp, candidates):
return self.reject(state, "pose_branch_unconfirmed", "conflict_pending" if state.frozen else "correcting")
if state.frozen:
state.conflict = True
return self.reject(state, "pose_branch_conflict", "frozen_conflict")
state.revision += 1
self.corrections[role] = self.corrections.get(role, 0) + 1
self.accept(state, best)
# A corrected tracker can establish NEW references, but the transition
# itself must never be interpreted as a measured joint displacement.
return self.reject(state, "pose_branch_corrected", "corrected")
def diagnostics(self, role: str) -> dict[str, int | str | bool]:
state = self.states[role]
return {
"observation_stamp_ns": state.observed_ns,
"branch_revision": state.revision,
"branch_status": state.status,
"branch_reason": state.reason,
"branch_confirmation_count": 0 if state.pending is None else state.pending.count,
"branch_frozen": state.frozen,
"motion_evidence_verified": state.motion_evidence_id is not None,
"motion_evidence_id": state.motion_evidence_id or "",
}
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py
+111
View File
@@ -0,0 +1,111 @@
"""Image hinges constrained by geometry retained in the output URDF.
Only adjacent parallel axes are represented here. Their signed parallelism
and perpendicular separation are invariant under joint-zero corrections.
Camera poses, Tag installations and image angles remain measured quantities.
"""
from dataclasses import dataclass
import math
import numpy as np
from scipy.spatial.transform import Rotation
from .motion_image_solver import ImageHingeBundle
@dataclass(frozen=True)
class ParallelAxisGeometry:
parent_joint: str
child_joint: str
axis_sign: int
distance_m: float
def __post_init__(self):
if (not self.parent_joint or not self.child_joint or self.parent_joint == self.child_joint
or self.axis_sign not in {-1, 1}
or not math.isfinite(self.distance_m) or self.distance_m <= 1e-6):
raise ValueError("invalid_parallel_axis_geometry")
class CadImageHingeBundle(ImageHingeBundle):
"""Eliminate three variables per constrained hinge, without penalty weights.
The child axis is the parent axis expressed in the parent Tag frame. Its
pivot is on the known-radius circle about the parent axis; one phase is
estimated. This keeps the model's axial-pivot gauge fixed and leaves no
redundant optimizer columns. The usual full Jacobian includes image-angle
uncertainty and dependencies on the parent's installation parameters.
"""
def __init__(self, frames, relations, poses, *, constraints, source_urdf_sha256):
super().__init__(frames, relations, poses)
self.constraints = tuple(constraints)
self.source_urdf_sha256 = source_urdf_sha256
indices = {relation.joint: i for i, relation in enumerate(relations)}
self._constraints = {}
for constraint in self.constraints:
parent = indices.get(constraint.parent_joint)
child = indices.get(constraint.child_joint)
if (parent is None or child is None or parent >= child
or relations[child].parent_role != relations[parent].child_role
or child in self._constraints):
raise ValueError("parallel_axis_constraint_outside_observed_chain")
self._constraints[child] = (parent, constraint)
self._full_initial = self.initial.copy()
removed = {10*i + local for i in self._constraints for local in (0, 1, 6)}
self._kept = np.asarray([i for i in range(len(self.initial)) if i not in removed])
self._columns = {original: column for column, original in enumerate(self._kept)}
for index, (parent, _) in sorted(self._constraints.items()):
parameters = self._full_initial[self._kept]
_, _, _, parent_mount = self.geometry_values(parameters, parent)
_, _, old_pivot, _ = super().geometry_values(self._full_initial, index)
tangent = self.axis_basis(parameters, index)
vector = parent_mount + old_pivot
self._full_initial[10*index + 5] = math.atan2(tangent[:, 1] @ vector, tangent[:, 0] @ vector)
self.scale[10*index + 5] = 1.
self.initial = self._full_initial[self._kept]
self.scale = self.scale[self._kept]
@property
def static_parameter_count(self):
return 10 * self.joints - 3 * len(self._constraints)
def geometry_parameter_indices(self, index):
return tuple(self._columns[original] for original in range(10*index, 10*(index+1))
if original in self._columns)
def geometry_dependency_indices(self, index):
result = self.geometry_parameter_indices(index)
if index in self._constraints:
result += self.geometry_dependency_indices(self._constraints[index][0])
return result
def _expand(self, parameters):
full = self._full_initial.copy()
full[self._kept] = parameters
return full
def axis_basis(self, parameters, index):
if index not in self._constraints:
values = self._expand(parameters)[10*index:10*index+2]
return Rotation.from_rotvec(self.bases[index] @ values).as_matrix() @ self.bases[index]
parent, constraint = self._constraints[index]
_, reference, _, _ = self.geometry_values(parameters, parent)
basis = reference.inv().as_matrix() @ self.axis_basis(parameters, parent)
basis[:, 1] *= constraint.axis_sign
return basis
def geometry_values(self, parameters, index):
full = self._expand(parameters)
if index not in self._constraints:
return super().geometry_values(full, index)
parent, constraint = self._constraints[index]
parent_axis, parent_reference, _, parent_mount = self.geometry_values(parameters, parent)
axis = constraint.axis_sign * parent_reference.inv().apply(parent_axis)
values = full[10*index:10*(index+1)]
reference = Rotation.from_rotvec(values[2:5]) * self.references[index]
phase = values[5]
pivot = constraint.distance_m * (self.axis_basis(parameters, index) @ [math.cos(phase), math.sin(phase)])
pivot -= parent_mount - axis * (axis @ parent_mount)
return axis, reference, pivot, values[7:10]
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py
+44
View File
@@ -0,0 +1,44 @@
"""A frozen image model with explicit, source-bound URDF constraints."""
from dataclasses import dataclass
from typing import ClassVar
import numpy as np
from scipy.spatial.transform import Rotation
from .cad_hinge import ParallelAxisGeometry
from .image_motion_model import ImageMotionModel
CAD_IMAGE_MOTION_MODEL_POLICY = "image_hinge_v2_source_parallel_geometry"
@dataclass(frozen=True)
class CadImageMotionModel(ImageMotionModel):
POLICY: ClassVar[str] = CAD_IMAGE_MOTION_MODEL_POLICY
policy: str = CAD_IMAGE_MOTION_MODEL_POLICY
constraints: tuple[ParallelAxisGeometry, ...] = ()
source_urdf_sha256: str = ""
def __post_init__(self):
super().__post_init__()
if (not isinstance(self.constraints, tuple) or not self.constraints
or len(self.source_urdf_sha256) != 64
or any(c not in "0123456789abcdef" for c in self.source_urdf_sha256)):
raise ValueError("image_model_source_geometry_missing")
geometry = {item.relation.joint: item for item in self.geometry}
children = set()
for constraint in self.constraints:
if not isinstance(constraint, ParallelAxisGeometry):
raise ValueError("image_model_source_geometry_invalid")
parent, child = geometry.get(constraint.parent_joint), geometry.get(constraint.child_joint)
if (parent is None or child is None or parent.relation.child_role != child.relation.parent_role
or constraint.child_joint in children):
raise ValueError("image_model_source_geometry_graph_changed")
children.add(constraint.child_joint)
axis = Rotation.from_quat(parent.reference_quaternion_xyzw).inv().apply(parent.axis_parent_xyz)
if not np.allclose(child.axis_parent_xyz, constraint.axis_sign * axis, atol=1e-8, rtol=0):
raise ValueError("image_model_source_axis_changed")
vector = np.asarray(parent.mount_child_xyz_m) + child.pivot_parent_xyz_m
distance = np.linalg.norm(vector - axis * (axis @ vector))
if not np.isclose(distance, constraint.distance_m, atol=1e-8, rtol=0):
raise ValueError("image_model_source_axis_distance_changed")
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/fixed_reference.py
+11
View File
@@ -0,0 +1,11 @@
"""An immutable camera-local pose for a declared stationary reference Tag."""
from dataclasses import dataclass
from .types import SquareTagPose
@dataclass(frozen=True)
class FixedTagReference:
pose: SquareTagPose
branch_revision: int
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_model_selection.py
+90
View File
@@ -0,0 +1,90 @@
"""Training-only model choice and held-out, motion-stratified comparisons.
All comparisons use squared image loss, matching the geometry optimizer.
Holm step-down controls the nominal familywise level for the complete set of
comparisons. These are conditional image tests, not a calibration certificate.
"""
import math
import numpy as np
from scipy.stats import ttest_1samp, wilcoxon
from .motion_evidence import MotionEvidenceParameters, _equivalent
IMAGE_MODEL_SELECTION_POLICY = "training_squared_loss_holm_v1"
def training_candidate_index(hypotheses):
"""Never replace a training-selected model because of validation scores."""
eligible = [i for i, hypothesis in enumerate(hypotheses) if hypothesis.training_accepted]
if not eligible:
return None
return min(eligible, key=lambda i: float(np.mean(
[quality.rms_px ** 2 for quality in hypotheses[i].training_quality])))
def equivalent_frame_selections(first, second):
if not first.resolved or not second.resolved:
return False
left = {selection.role: selection.pose for selection in first.selections}
right = {selection.role: selection.pose for selection in second.selections}
return bool(left and len(left) == len(first.selections) and len(right) == len(second.selections)
and left.keys() == right.keys()
and all(_equivalent(pose, right[role]) for role, pose in left.items()))
def compare_image_losses(best, alternative, angles, tolerance):
"""Compare paired squared errors with the existing pixel-resolution margin.
One observation per angular stratum prevents repeated images inflating
evidence. Medians and both mean/rank tests prevent an isolated large error
from deciding a branch. The candidate is selected on training images only.
"""
angles = np.asarray(angles, dtype=float)
best, alternative = np.asarray(best, dtype=float), np.asarray(alternative, dtype=float)
if (angles.ndim != 2 or not angles.shape[1] or best.ndim != 1
or len(best) != len(angles) or alternative.shape != best.shape or not len(best)
or not np.all(np.isfinite(angles)) or not np.all(np.isfinite(best))
or not np.all(np.isfinite(alternative)) or np.any(best < 0) or np.any(alternative < 0)
or not math.isfinite(tolerance) or tolerance < 0):
return 1.
moving = int(np.argmax(np.ptp(angles, axis=0)))
values = angles[:, moving]
span = float(np.ptp(values))
parameters = MotionEvidenceParameters()
count = min(24, int(np.floor(span / parameters.angular_bin_rad)))
if count < parameters.minimum_motion_bins:
return 1.
indices = np.minimum(count - 1, np.floor((values - min(values)) / span * count).astype(int))
losses = alternative ** 2 - (best + tolerance) ** 2
differences = np.asarray([float(np.median(losses[indices == index]))
for index in range(count) if np.any(indices == index)])
if (len(differences) < parameters.minimum_motion_bins
or not np.all(np.isfinite(differences)) or np.mean(differences) <= 0):
return 1.
if np.ptp(differences) < 1e-12:
return 2. ** -len(differences) if differences[0] > 0 else 1.
mean_probability = float(ttest_1samp(differences, 0., alternative="greater").pvalue)
rank_probability = float(wilcoxon(differences, alternative="greater", zero_method="wilcox").pvalue)
if not math.isfinite(mean_probability) or not math.isfinite(rank_probability):
return 1.
return max(mean_probability, rank_probability)
def holm_adjusted_probabilities(comparisons):
"""Order-independent Holm probabilities, returned in input identity order.
A geometric equivalence check may supply zero. It is still included in the
family; no candidate is omitted merely because of its initialization label.
Nonfinite or invalid probabilities fail closed.
"""
comparisons = tuple(comparisons)
if len({index for index, _ in comparisons}) != len(comparisons):
raise ValueError("duplicate_image_model_comparison")
values = [(index, probability if math.isfinite(probability) and 0 <= probability <= 1 else 1.)
for index, probability in comparisons]
adjusted, previous = {}, 0.
for rank, (index, probability) in enumerate(sorted(values, key=lambda item: item[1])):
previous = max(previous, min(1., probability * (len(values) - rank)))
adjusted[index] = previous
return tuple((index, adjusted[index]) for index, _ in comparisons)
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py
+157
View File
@@ -0,0 +1,157 @@
"""Bind one image solve to versioned tracker inputs, then commit it once.
The numerical solve runs without tracker locks. Immutable tracker-owned pixels
and frozen root snapshots prevent callers substituting observations; a weak
identity registry prevents copied or manually assembled decisions being used as
commit authority. All tracker locks are acquired only for metadata checks.
"""
from __future__ import annotations
from contextlib import ExitStack
from collections.abc import Mapping, Sequence
from dataclasses import asdict, dataclass
import hashlib
import json
from threading import RLock
from weakref import WeakValueDictionary
from typing import TYPE_CHECKING
from .image_motion_model import ImageMotionModel
from .image_motion_projection import select_image_motion_frame
from .image_projection_worker import ImageProjectionWorker
from .motion_image_types import ImageMotionFrame, ImageRoleQuality
if TYPE_CHECKING:
from .tracking import MotionFrameCommit, MotionPoseSelection, MotionTrackerSnapshot
@dataclass(frozen=True)
class VerifiedImageMotionDecision:
accepted: bool
reason: str
selections: tuple[MotionPoseSelection, ...] = ()
root_snapshots: tuple[MotionTrackerSnapshot, ...] = ()
model: ImageMotionModel | None = None
model_sha256: str = ""
quality: tuple[ImageRoleQuality, ...] = ()
_verified = WeakValueDictionary()
_registry_lock = RLock()
def _root_current(snapshot):
tracker, role = snapshot.tracker, snapshot.role
state = tracker._branches.states.get(role)
diagnostics = tracker.last_candidate_diagnostics_by_role.get(role, {})
evidence_verified = (diagnostics.get("motion_selection_committed") is True and snapshot.evidence_id is not None
if snapshot.motion_managed else diagnostics.get("fixed_reference_verified") is True)
return (tracker._epoch == snapshot.epoch and state is not None
and state.observed_ns == snapshot.stamp_ns and state.revision == snapshot.revision
and state.frozen and snapshot.frozen and state.status == "tracking"
and state.pose == snapshot.previous_pose and evidence_verified
and not state.conflict and not state.recovery_required
and tracker._image_observations_by_role.get(role) is snapshot.image_observation)
def _inputs_match(snapshots, roots, model, frame):
items = {r.role: r for r in frame.evidence.roles}
observations = {o.role: o for o in frame.observations}
if len(items) != len(frame.evidence.roles) or len(observations) != len(frame.observations):
return False
for snapshot in (*snapshots, *roots):
image = snapshot.image_observation
item, observation = items.get(snapshot.role), observations.get(snapshot.role)
if (image is None or item is None or observation is None
or snapshot.stamp_ns != frame.evidence.stamp_ns or image.stamp_ns != snapshot.stamp_ns
or image.camera_matrix != model.camera_matrix or image.camera_matrix != frame.camera_matrix
or image.tag_size_m != observation.tag_size_m or image.corners_xy != observation.corners_xy):
return False
if snapshot in roots:
if not item.frozen or item.candidates != (snapshot.previous_pose,):
return False
elif item.frozen or item.candidates != snapshot.candidates:
return False
return True
def verify_image_motion_frame(snapshots: Sequence[MotionTrackerSnapshot], model: ImageMotionModel,
frame: ImageMotionFrame, evidence_ids: Mapping[str, str], *,
root_snapshots: Sequence[MotionTrackerSnapshot] = (),
projection_worker=None) -> VerifiedImageMotionDecision:
"""Calculate a decision once; never accept caller-proposed output poses."""
from .tracking import MotionPoseSelection, SquareTagPoseTracker
if projection_worker is not None and type(projection_worker) is not ImageProjectionWorker:
return VerifiedImageMotionDecision(False, "pose_image_motion_worker_invalid")
snapshots, roots = tuple(snapshots), tuple(root_snapshots)
if not isinstance(model, ImageMotionModel) or not snapshots:
return VerifiedImageMotionDecision(False, "pose_image_motion_inputs_invalid")
child_roles = {g.relation.child_role for g in model.geometry}
root_roles = set(dict(model.tag_sizes)) - child_roles
if (set(evidence_ids) != child_roles or {s.role for s in snapshots} != child_roles
or len(snapshots) != len(child_roles)
or any(not isinstance(value, str) or not value for value in evidence_ids.values())):
return VerifiedImageMotionDecision(False, "pose_image_motion_roles_invalid")
if not roots:
collected = []
for role in sorted(root_roles):
found = [snapshot for tracker in {s.tracker for s in snapshots}
if (snapshot := tracker.motion_snapshot(role, frame.evidence.stamp_ns)) is not None]
if len(found) != 1:
return VerifiedImageMotionDecision(False, "pose_image_motion_root_missing")
collected.append(found[0])
roots = tuple(collected)
if {s.role for s in roots} != root_roles or len(roots) != len(root_roles):
return VerifiedImageMotionDecision(False, "pose_image_motion_root_missing")
if not _inputs_match(snapshots, roots, model, frame):
return VerifiedImageMotionDecision(False, "pose_image_motion_observation_mismatch")
with ExitStack() as stack:
for tracker in sorted({s.tracker for s in (*snapshots, *roots)}, key=id):
stack.enter_context(tracker._lock)
if not all(SquareTagPoseTracker._motion_snapshot_is_current(s) for s in snapshots) or not all(
_root_current(s) for s in roots):
return VerifiedImageMotionDecision(False, "pose_observation_superseded")
# This is the only expensive operation. Commit below never repeats it.
result = (select_image_motion_frame(model, frame) if projection_worker is None
else ImageProjectionWorker.solve(projection_worker, model, frame))
if not result.resolved:
return VerifiedImageMotionDecision(False, result.reason, quality=result.quality)
poses = {item.role: item.pose for item in result.selections}
identity = hashlib.sha256(json.dumps(asdict(model), sort_keys=True, separators=(",", ":"),
allow_nan=False).encode()).hexdigest()
selections = tuple(MotionPoseSelection(s, poses[s.role], evidence_ids[s.role]) for s in snapshots)
decision = VerifiedImageMotionDecision(True, "", selections, roots, model, identity, result.quality)
with _registry_lock:
_verified[id(decision)] = decision
return decision
def commit_image_motion_frame(decision: VerifiedImageMotionDecision, *,
establish: bool = False) -> MotionFrameCommit:
"""Consume a verifier-created decision and atomically update all children."""
from .tracking import MotionFrameCommit, SquareTagPoseTracker
if not isinstance(decision, VerifiedImageMotionDecision):
return MotionFrameCommit(False, "pose_image_motion_decision_invalid")
if not decision.accepted:
return MotionFrameCommit(False, decision.reason)
with _registry_lock:
if _verified.pop(id(decision), None) is not decision:
return MotionFrameCommit(False, "pose_image_motion_decision_invalid")
snapshots = tuple(item.snapshot for item in decision.selections)
with ExitStack() as stack:
for tracker in sorted({s.tracker for s in (*snapshots, *decision.root_snapshots)}, key=id):
stack.enter_context(tracker._lock)
if not all(SquareTagPoseTracker._motion_snapshot_is_current(s) for s in snapshots) or not all(
_root_current(s) for s in decision.root_snapshots):
return MotionFrameCommit(False, "pose_observation_superseded")
for snapshot in snapshots:
tracker, role = snapshot.tracker, snapshot.role
if tracker._image_observations_by_role.get(role) is not snapshot.image_observation:
return MotionFrameCommit(False, "pose_image_motion_observation_mismatch")
previous = tracker._image_model_sha256_by_role.get(role)
if previous is not None and previous != decision.model_sha256:
return MotionFrameCommit(False, "pose_image_motion_model_changed")
if previous is None and snapshot.evidence_id is not None:
return MotionFrameCommit(False, "pose_motion_solver_changed")
return SquareTagPoseTracker._commit_motion_selections_locked(decision.selections,
establish=establish, image_decision=decision)
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py
+136
View File
@@ -0,0 +1,136 @@
"""Frozen image-derived hinge models and their auditable authorization evidence."""
from __future__ import annotations
from dataclasses import dataclass
import math
from typing import ClassVar, Mapping
import numpy as np
from .motion_evidence import MotionRelation, MotionRoleSelection
from .motion_image_types import ImageHingeGeometry, ImageRoleQuality
from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS
IMAGE_MOTION_MODEL_POLICY = "image_hinge_v1_pre_zero_frozen"
@dataclass(frozen=True)
class ImageMotionModel:
POLICY: ClassVar[str] = IMAGE_MOTION_MODEL_POLICY
geometry: tuple[ImageHingeGeometry, ...]
camera_matrix: tuple[tuple[float, float, float], ...]
tag_sizes: tuple[tuple[str, float], ...]
source_stamps_ns: tuple[int, ...]
training_stamps_ns: tuple[int, ...]
validation_stamps_ns: tuple[int, ...]
branches: tuple[tuple[str, int], ...]
maximum_reprojection_error_px: float = 1.5
reprojection_tie_px: float = .03
policy: str = IMAGE_MOTION_MODEL_POLICY
@property
def relations(self):
return tuple(g.relation for g in self.geometry)
def __post_init__(self):
from .motion_image_diagnostics import _ordered_relations
if self.policy != self.POLICY:
raise ValueError("image_motion_model_policy_mismatch")
sequences = (self.geometry, self.camera_matrix, self.tag_sizes, self.source_stamps_ns,
self.training_stamps_ns, self.validation_stamps_ns, self.branches)
if any(not isinstance(value, tuple) for value in sequences) or any(
not isinstance(value, tuple) for values in (self.camera_matrix, self.tag_sizes, self.branches)
for value in values):
raise ValueError("image_motion_model_requires_immutable_sequences")
relations = self.relations
if tuple(_ordered_relations(relations)) != relations:
raise ValueError("image_motion_model_graph_not_ordered")
p = DEFAULT_POSE_TRACKING_PARAMETERS
if (self.maximum_reprojection_error_px != p.maximum_reprojection_error_px
or self.reprojection_tie_px != p.reprojection_tie_px):
raise ValueError("image_motion_model_quality_policy_changed")
matrix = np.asarray(self.camera_matrix)
if (matrix.shape != (3, 3) or not np.all(np.isfinite(matrix))
or matrix[0, 0] <= 0 or matrix[1, 1] <= 0
or not np.array_equal(matrix[2], [0, 0, 1])):
raise ValueError("image_motion_model_camera_invalid")
roles = {r for g in self.geometry for r in (g.relation.parent_role, g.relation.child_role)}
if (len(dict(self.tag_sizes)) != len(self.tag_sizes) or set(dict(self.tag_sizes)) != roles
or any(not math.isfinite(s) or s <= 0 for _, s in self.tag_sizes)):
raise ValueError("image_motion_model_tag_sizes_invalid")
if len(dict(self.branches)) != len(self.branches) or set(dict(self.branches)) != roles or any(
type(v) is not int or v not in (0, 1) for _, v in self.branches):
raise ValueError("image_motion_model_branches_invalid")
for stamps in (self.source_stamps_ns, self.training_stamps_ns, self.validation_stamps_ns):
if not stamps or any(type(v) is not int or v <= 0 for v in stamps) or any(
b <= a for a, b in zip(stamps, stamps[1:])):
raise ValueError("image_motion_model_sources_invalid")
source, train, validation = map(set, (self.source_stamps_ns, self.training_stamps_ns,
self.validation_stamps_ns))
if not train.isdisjoint(validation) or not train | validation <= source:
raise ValueError("image_motion_model_source_overlap")
for geometry in self.geometry:
vectors = (geometry.axis_parent_xyz, geometry.reference_quaternion_xyzw,
geometry.pivot_parent_xyz_m, geometry.mount_child_xyz_m)
if any(not isinstance(v, tuple) or len(v) != n for v, n in zip(vectors, (3, 4, 3, 3))) or not all(
math.isfinite(x) for v in vectors for x in v):
raise ValueError("image_motion_model_geometry_invalid")
if not np.isclose(np.linalg.norm(vectors[0]), 1., atol=1e-7) or not np.isclose(
np.linalg.norm(vectors[1]), 1., atol=1e-7):
raise ValueError("image_motion_model_rotation_invalid")
@dataclass(frozen=True)
class ImageMotionFrameSelection:
resolved: bool
reason: str
selections: tuple[MotionRoleSelection, ...] = ()
quality: tuple[ImageRoleQuality, ...] = ()
angles_rad: tuple[tuple[str, float], ...] = ()
@dataclass(frozen=True)
class ImageMotionHypothesis:
branches: tuple[tuple[str, int], ...]
converged: bool
reason: str
training_quality: tuple[ImageRoleQuality, ...] = ()
validation_quality: tuple[ImageRoleQuality, ...] = ()
motion_span_rad: tuple[tuple[str, float], ...] = ()
axis_uncertainty_95_rad: tuple[tuple[str, float], ...] = ()
pivot_uncertainty_95_m: tuple[tuple[str, float], ...] = ()
comparison_p_values: tuple[tuple[int, float], ...] = ()
training_solver_status: int | None = None
training_solver_evaluations: int = 0
training_accepted: bool = False
comparison_adjusted_p_values: tuple[tuple[int, float], ...] = ()
@dataclass(frozen=True)
class ImageMotionResolution:
resolved: bool
reason: str
model: ImageMotionModel | None = None
hypotheses: tuple[ImageMotionHypothesis, ...] = ()
selections: tuple[MotionRoleSelection, ...] = ()
def image_motion_model_from_dict(raw: Mapping) -> ImageMotionModel:
"""Read an immutable model; hashes/provenance are enforced by the caller."""
geometry = tuple(ImageHingeGeometry(MotionRelation(**item["relation"]),
tuple(item["axis_parent_xyz"]), tuple(item["reference_quaternion_xyzw"]),
tuple(item["pivot_parent_xyz_m"]), tuple(item["mount_child_xyz_m"])) for item in raw["geometry"])
model_type, extra = ImageMotionModel, {}
from .cad_image_model import CAD_IMAGE_MOTION_MODEL_POLICY, CadImageMotionModel
if raw["policy"] == CAD_IMAGE_MOTION_MODEL_POLICY:
from .cad_hinge import ParallelAxisGeometry
model_type = CadImageMotionModel
extra = {"constraints": tuple(ParallelAxisGeometry(**value) for value in raw["constraints"]),
"source_urdf_sha256": raw["source_urdf_sha256"]}
return model_type(geometry=geometry, camera_matrix=tuple(tuple(v) for v in raw["camera_matrix"]),
tag_sizes=tuple((str(r), float(s)) for r, s in raw["tag_sizes"]),
source_stamps_ns=tuple(raw["source_stamps_ns"]), training_stamps_ns=tuple(raw["training_stamps_ns"]),
validation_stamps_ns=tuple(raw["validation_stamps_ns"]),
branches=tuple((str(r), i) for r, i in raw["branches"]),
maximum_reprojection_error_px=raw["maximum_reprojection_error_px"],
reprojection_tie_px=raw["reprojection_tie_px"], policy=raw["policy"], **extra)
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py
+131
View File
@@ -0,0 +1,131 @@
"""Current-image angle estimates with immutable, pre-zero hinge geometry."""
from itertools import product
import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation
from .image_motion_model import ImageMotionFrameSelection
from .ippe import square_object_points
from .motion_evidence import MotionEvidenceParameters, MotionRoleSelection, _eligible, _equivalent
from .motion_image_diagnostics import _quality
from .relative import _relative_pose
from .types import SquareTagPose
def project_image_model(model, angles, roots):
transforms = {role: (Rotation.from_quat(pose.quaternion_xyzw),
np.array(pose.translation_xyz_m)) for role, pose in roots.items()}
pixels = []
positive = True
matrix, sizes = np.array(model.camera_matrix), dict(model.tag_sizes)
for geometry, angle in zip(model.geometry, angles):
relative = Rotation.from_rotvec(np.array(geometry.axis_parent_xyz) * angle) * Rotation.from_quat(
geometry.reference_quaternion_xyzw)
parent_rotation, parent_translation = transforms[geometry.relation.parent_role]
rotation = parent_rotation * relative
translation = parent_translation + parent_rotation.apply(np.array(geometry.pivot_parent_xyz_m)
+ relative.apply(geometry.mount_child_xyz_m))
role = geometry.relation.child_role
transforms[role] = (rotation, translation)
points = rotation.apply(square_object_points(sizes[role])) + translation
positive = positive and bool(np.all(points[:, 2] > 1e-6))
homogeneous = points @ matrix.T
pixels.append(homogeneous[:, :2] / np.maximum(homogeneous[:, 2:], 1e-6))
return np.asarray(pixels), transforms, positive
def _frame_inputs(model, frame):
if tuple(tuple(v) for v in frame.camera_matrix) != model.camera_matrix:
raise ValueError("image_motion_camera_changed")
observations = {o.role: o for o in frame.observations}
candidates = {r.role: r for r in frame.evidence.roles}
roles, sizes = tuple(dict(model.tag_sizes)), dict(model.tag_sizes)
if (len(observations) != len(frame.observations) or len(candidates) != len(frame.evidence.roles)
or not set(roles) <= observations.keys() or not set(roles) <= candidates.keys()):
raise ValueError("image_motion_frame_roles_missing")
for role in roles:
observation = observations[role]
if observation.tag_size_m != sizes[role] or np.shape(observation.corners_xy) != (4, 2) or not np.all(
np.isfinite(observation.corners_xy)):
raise ValueError("image_motion_frame_corners_invalid")
children = tuple(g.relation.child_role for g in model.geometry)
roots = set(roles) - set(children)
poses = {}
for role in roles:
selected = tuple(p for p in candidates[role].candidates if _eligible(p))
if not selected or (role in roots and (not candidates[role].frozen or len(selected) != 1)):
raise ValueError("image_motion_current_root_or_candidates_unavailable")
poses[role] = selected
frozen = {role: poses[role][0] for role in roots}
# A frozen root is still verified against this image, never silently moved.
for role, pose in frozen.items():
xyz = Rotation.from_quat(pose.quaternion_xyzw).apply(square_object_points(sizes[role]))
xyz += pose.translation_xyz_m
if np.any(xyz[:, 2] <= 1e-6):
raise ValueError("image_motion_root_behind_camera")
image = xyz @ np.array(model.camera_matrix).T
delta = image[:, :2] / image[:, 2:] - observations[role].corners_xy
if np.sqrt(np.mean(np.sum(delta ** 2, axis=1))) > model.maximum_reprojection_error_px:
raise ValueError("image_motion_current_root_reprojection_failed")
return poses, frozen, np.asarray([observations[r].corners_xy for r in children]), children
def select_image_motion_frame(model, frame):
"""Fit only current geometric angles, retaining all candidate initializations.
No model parameter, baseline or previous pose is changed. Distinct solutions
within the public 0.03 pixel tolerance are rejected rather than ranked.
"""
try:
poses, roots, observed, children = _frame_inputs(model, frame)
roles = tuple(poses)
if np.prod([len(poses[r]) for r in roles]) > 256:
raise ValueError("image_motion_frame_hypothesis_limit")
fits = []
for values in product(*(poses[role] for role in roles)):
selected = dict(zip(roles, values))
initial = []
for geometry in model.geometry:
rotation, _ = _relative_pose(selected[geometry.relation.parent_role],
selected[geometry.relation.child_role])
initial.append(float((rotation * Rotation.from_quat(
geometry.reference_quaternion_xyzw).inv()).as_rotvec() @ geometry.axis_parent_xyz))
def residual(angles):
pixels, _, _ = project_image_model(model, angles, roots)
return (pixels - observed).ravel()
fit = least_squares(residual, initial, max_nfev=40, ftol=1e-9, xtol=1e-9, gtol=1e-7)
_, transforms, positive = project_image_model(model, fit.x, roots)
if not fit.success or not positive or not np.all(np.isfinite(fit.fun)):
raise ValueError("image_motion_frame_optimizer_unresolved")
quality = _quality(fit.fun, children)
output = dict(roots)
for role, checked in zip(children, quality):
rotation, translation = transforms[role]
output[role] = SquareTagPose(tuple(rotation.as_quat()), tuple(translation), checked.rms_px)
score = float(np.sqrt(np.mean(np.square(fit.fun.reshape(-1, 2)).sum(axis=1))))
fits.append((score, output, quality, fit.x, fit.jac, fit.fun))
if not fits:
raise ValueError("image_motion_frame_reprojection_or_tilt_failed")
fits.sort(key=lambda item: item[0])
best = fits[0]
for other in fits[1:]:
if other[0] <= best[0] + model.reprojection_tie_px and not all(
_equivalent(best[1][r], other[1][r]) for r in roles):
raise ValueError("image_motion_frame_angles_ambiguous")
jacobian = np.asarray(best[4])
_, singular, vh = np.linalg.svd(jacobian, full_matrices=False)
if singular[-1] <= singular[0] * np.finfo(float).eps * max(jacobian.shape):
raise ValueError("image_motion_frame_angles_unobservable")
variance = max(model.reprojection_tie_px ** 2,
float(np.sum(best[5] ** 2) / max(1, len(best[5]) - len(best[3]))))
covariance = (vh.T / singular ** 2) @ vh * variance
if np.any(3 * np.sqrt(np.maximum(0., np.diag(covariance))) >
MotionEvidenceParameters().maximum_frame_off_axis_rad):
raise ValueError("image_motion_frame_angles_uncertain")
accepted = all(_eligible(p) for p in best[1].values())
return ImageMotionFrameSelection(accepted, "" if accepted else "image_motion_frame_reprojection_or_tilt_failed",
tuple(MotionRoleSelection(r, best[1][r]) for r in roles), best[2],
tuple((g.relation.joint, float(a)) for g, a in zip(model.geometry, best[3])))
except (ValueError, TypeError, KeyError, FloatingPointError, np.linalg.LinAlgError) as error:
return ImageMotionFrameSelection(False, str(error))
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_projection_worker.py
+38
View File
@@ -0,0 +1,38 @@
"""Bounded process execution of the unchanged, pure current-image solver."""
from concurrent.futures import ProcessPoolExecutor, TimeoutError
from concurrent.futures.process import BrokenProcessPool
import multiprocessing
import threading
from .image_motion_model import ImageMotionFrameSelection
from .image_motion_projection import select_image_motion_frame
class ImageProjectionWorker:
def __init__(self, workers=3):
self._pool = ProcessPoolExecutor(max_workers=workers,
mp_context=multiprocessing.get_context("spawn"))
self._slots = threading.BoundedSemaphore(workers)
self._closed = False
def solve(self, model, frame):
if self._closed or not self._slots.acquire(blocking=False):
return ImageMotionFrameSelection(False, "image_motion_frame_worker_busy")
try:
future = self._pool.submit(select_image_motion_frame, model, frame)
except (RuntimeError, BrokenProcessPool):
self._slots.release()
return ImageMotionFrameSelection(False, "image_motion_frame_worker_unavailable")
future.add_done_callback(lambda _: self._slots.release())
try:
return future.result(timeout=2.)
except TimeoutError:
future.cancel()
return ImageMotionFrameSelection(False, "image_motion_frame_worker_timeout")
except BrokenProcessPool:
return ImageMotionFrameSelection(False, "image_motion_frame_worker_unavailable")
def close(self):
self._closed = True
self._pool.shutdown(wait=False, cancel_futures=True)
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py
+27
View File
@@ -111,6 +111,33 @@ def solve_square_tag_ippe(
return candidates
def fixed_pose_reprojection_error(
pose: SquareTagPose,
corners_xy: Sequence[Sequence[float]],
*,
tag_size_m: float,
camera_matrix: Sequence[Sequence[float]],
) -> float:
"""Check a frozen pose against rectified corners without solving a new pose."""
corners = np.asarray(corners_xy, dtype=np.float64)
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
raise ValueError("corners_xy must contain four finite image points")
intrinsic = _as_camera_matrix(camera_matrix)
points = square_object_points(tag_size_m)
rotation = Rotation.from_quat(pose.quaternion_xyzw)
translation = np.asarray(pose.translation_xyz_m, dtype=np.float64)
camera_points = rotation.apply(points) + translation
if not np.all(np.isfinite(camera_points)) or np.min(camera_points[:, 2]) <= 0:
raise ValueError("fixed pose must have finite, positive-depth corners")
projected, _ = cv2.projectPoints(points, rotation.as_rotvec(), translation,
intrinsic, np.zeros(4, dtype=np.float64))
residual = projected.reshape(4, 2) - corners
error = float(np.sqrt(np.mean(np.sum(residual * residual, axis=1))))
if not math.isfinite(error):
raise ValueError("fixed pose reprojection error must be finite")
return error
def rotation_distance_rad(
first_xyzw: Sequence[float],
second_xyzw: Sequence[float],
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py
+413
View File
@@ -0,0 +1,413 @@
"""Pre-freeze planar-branch evidence from a single articulated motion.
The measurements are actual image candidates, never SDK-derived angles. A
hinge has one rotation axis and one fixed pivot in its parent Tag coordinates:
``R = exp(theta * axis) R_ref`` and ``t = pivot + R * mount``. These are the
same fixed-axis/fixed-pivot relations used by candidate_selection's trajectory
score. Unlike that diagnostic ranking, this module requires a unique hypothesis
to pass absolute quality and observability checks before authorizing a branch.
The 1.5 mm / 1 degree RMS scales come from that existing trajectory score.
Motion span, cross-validation and conditioning gates are additional evidence
requirements, not changes to final calibration accuracy limits. Calibration
holdout and previously frozen references must never enter this fit API.
"""
from __future__ import annotations
from dataclasses import dataclass
from itertools import permutations, product
import math
from typing import Sequence
import numpy as np
from scipy.spatial.transform import Rotation
from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS
from .relative import _relative_pose
from .types import SquareTagPose
@dataclass(frozen=True)
class MotionRelation:
joint: str
parent_role: str
child_role: str
@dataclass(frozen=True)
class RoleCandidates:
"""Actual candidates; ``frozen`` means already selected by prior evidence.
A previously frozen moving parent must be selected in this image using its
existing constraint before a new relation is trained. Its last image's pose
is not a substitute. Raw current-frame roots passed to select_motion_frame
remain a candidate set; the tracker's separate frozen flag is not this flag.
"""
role: str
candidates: tuple[SquareTagPose, ...]
frozen: bool = False
@dataclass(frozen=True)
class MotionEvidenceFrame:
stamp_ns: int
roles: tuple[RoleCandidates, ...]
@dataclass(frozen=True)
class MotionRoleSelection:
role: str
pose: SquareTagPose
@dataclass(frozen=True)
class MotionEvidenceParameters:
minimum_motion_rad: float = math.radians(15.0)
angular_bin_rad: float = math.radians(1.0)
minimum_motion_bins: int = 12
minimum_translation_span_m: float = 0.003
maximum_pivot_condition: float = 100.0
maximum_pivot_rms_m: float = 0.0015
maximum_pivot_p95_m: float = 0.003
maximum_off_axis_rms_rad: float = math.radians(1.0)
maximum_off_axis_p95_rad: float = math.radians(2.0)
maximum_split_axis_difference_rad: float = math.radians(3.0)
maximum_frame_off_axis_rad: float = math.radians(3.0)
maximum_frame_pivot_error_m: float = 0.003
def __post_init__(self):
for name, value in vars(self).items():
if not math.isfinite(value) or value <= 0:
raise ValueError(f"{name} must be finite and positive")
if type(self.minimum_motion_bins) is not int or self.minimum_motion_bins < 12:
raise ValueError("minimum_motion_bins must be an integer >= 12")
@dataclass(frozen=True)
class MotionConstraintResidual:
off_axis_rad: float
pivot_error_m: float
accepted: bool
@dataclass(frozen=True)
class FrozenMotionConstraint:
relation: MotionRelation
reference_quaternion_xyzw: tuple[float, float, float, float]
axis_parent_xyz: tuple[float, float, float]
pivot_parent_xyz_m: tuple[float, float, float]
mount_child_xyz_m: tuple[float, float, float]
source_stamps_ns: tuple[int, ...]
maximum_off_axis_rad: float
maximum_pivot_error_m: float
def evaluate(self, parent: SquareTagPose, child: SquareTagPose) -> MotionConstraintResidual:
rotation, translation = _relative_pose(parent, child)
off_axis, pivot = _residuals(self, Rotation.from_quat([rotation.as_quat()]),
np.asarray([translation]))
a, p = float(off_axis[0]), float(pivot[0])
return MotionConstraintResidual(a, p, bool(
_eligible(parent) and _eligible(child)
and a <= self.maximum_off_axis_rad and p <= self.maximum_pivot_error_m))
@dataclass(frozen=True)
class MotionRelationQuality:
joint: str
reason: str
motion_rad: float = 0.0
motion_bins: int = 0
translation_span_m: float = 0.0
pivot_condition: float = 0.0
pivot_rms_m: float = 0.0
pivot_p95_m: float = 0.0
off_axis_rms_rad: float = 0.0
off_axis_p95_rad: float = 0.0
split_axis_difference_rad: float = 0.0
@dataclass(frozen=True)
class MotionHypothesis:
branches: tuple[tuple[str, int], ...]
relations: tuple[MotionRelationQuality, ...]
accepted: bool
@dataclass(frozen=True)
class MotionBranchResolution:
resolved: bool
reason: str
selections: tuple[MotionRoleSelection, ...] = ()
constraints: tuple[FrozenMotionConstraint, ...] = ()
hypotheses: tuple[MotionHypothesis, ...] = ()
@dataclass(frozen=True)
class MotionFrameSelection:
resolved: bool
reason: str
selections: tuple[MotionRoleSelection, ...] = ()
def _eligible(pose: SquareTagPose) -> bool:
values = (*pose.quaternion_xyzw, *pose.translation_xyz_m, pose.reprojection_error_px)
if (len(pose.quaternion_xyzw) != 4 or len(pose.translation_xyz_m) != 3
or not np.all(np.isfinite(values))
or not np.isclose(np.linalg.norm(pose.quaternion_xyzw), 1.0, atol=1e-5)
or pose.translation_xyz_m[2] <= 0
or not 0 <= pose.reprojection_error_px <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px):
return False
normal = Rotation.from_quat(pose.quaternion_xyzw).apply([0, 0, 1])
tilt = math.acos(float(np.clip(abs(normal[2]), 0, 1)))
return tilt <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_tag_tilt_rad
def _distance(first: SquareTagPose, second: SquareTagPose) -> tuple[float, float]:
return (float((Rotation.from_quat(first.quaternion_xyzw).inv()
* Rotation.from_quat(second.quaternion_xyzw)).magnitude()),
float(np.linalg.norm(np.subtract(first.translation_xyz_m, second.translation_xyz_m))))
def _equivalent(first: SquareTagPose, second: SquareTagPose) -> bool:
angle, distance = _distance(first, second)
return angle <= math.radians(1.0) and distance <= 0.001
def _validate_relations(relations: Sequence[MotionRelation]) -> tuple[str, ...]:
if not relations or len({r.joint for r in relations}) != len(relations):
raise ValueError("motion relations need non-empty unique joints")
roles = tuple(dict.fromkeys(role for r in relations for role in (r.parent_role, r.child_role)))
if len(roles) > 8 or any(not r.joint or not r.parent_role or not r.child_role
or r.parent_role == r.child_role for r in relations):
raise ValueError("motion relations need a bounded articulated graph")
return roles
def _candidate_map(items: Sequence[RoleCandidates], roles: Sequence[str]):
result = {item.role: item for item in items}
if len(result) != len(items) or any(role not in result for role in roles):
return None
for role in roles:
item = result[role]
if not 1 <= len(item.candidates) <= 2 or (item.frozen and len(item.candidates) != 1):
return None
valid = tuple(candidate for candidate in item.candidates if _eligible(candidate))
if not valid:
return None
result[role] = RoleCandidates(role, valid, item.frozen)
return result
def _connect_paths(frames: Sequence[MotionEvidenceFrame], roles: Sequence[str]):
"""Preserve both continuous families without treating continuity as truth."""
candidates = [_candidate_map(frame.roles, roles) for frame in frames]
if any(item is None for item in candidates):
return None, "motion_candidates_missing_or_invalid"
paths = {}
for role in roles:
items = [frame[role] for frame in candidates]
if any(item.frozen != items[0].frozen for item in items):
return None, "motion_candidate_family_interrupted"
# Anchor where both eligible roots are available. If an alternative
# temporarily fails the image-quality gate it stays missing; never copy
# the surviving root into both families to invent evidence.
anchor = max(range(len(items)), key=lambda i: len(items[i].candidates))
count = len(items[anchor].candidates)
families = [[None] * len(frames) for _ in range(count)]
for index, candidate in enumerate(items[anchor].candidates):
families[index][anchor] = candidate
for sequence in (range(anchor + 1, len(frames)), range(anchor - 1, -1, -1)):
previous = list(items[anchor].candidates)
for frame_index in sequence:
current = items[frame_index].candidates
def cost(order):
return sum(_distance(previous[family], candidate)[0]
for family, candidate in zip(order, current))
orders = sorted(permutations(range(count), len(current)), key=cost)
if (len(orders) == 2 and abs(cost(orders[0]) - cost(orders[1])) < 1e-9
and not _equivalent(previous[0], previous[1])):
return None, "motion_candidate_assignment_ambiguous"
for family, candidate in zip(orders[0], current):
angle, translation = _distance(previous[family], candidate)
p = DEFAULT_POSE_TRACKING_PARAMETERS
if angle > p.maximum_pose_jump_rad or translation > p.maximum_translation_jump_m:
return None, "motion_candidate_family_discontinuous"
families[family][frame_index] = candidate
previous[family] = candidate
paths[role] = families
# Near-coincident IPPE roots represent one physical hypothesis, not two.
for role, families in paths.items():
if len(families) == 2 and all(a is not None and b is not None and _equivalent(a, b)
for a, b in zip(*families)):
paths[role] = families[:1]
return paths, ""
def _axis(rotations: Rotation) -> tuple[np.ndarray, np.ndarray]:
vectors = (rotations * rotations[0].inv()).as_rotvec()
axis = np.linalg.svd(vectors, full_matrices=False)[2][0]
if np.dot(axis, vectors[-1]) < 0:
axis = -axis
return axis, vectors @ axis
def _fit_constraint(relation, rotations, points, stamps, parameters):
axis, angles = _axis(rotations)
projected = Rotation.from_rotvec(angles[:, None] * axis) * rotations[0]
# One axial pivot gauge is unobservable for any revolute joint. Projecting
# onto the measured rotation axis gives rank five instead of fitting that
# gauge to tiny orientation noise with an arbitrary sixth parameter.
design = np.concatenate((np.tile(np.eye(3), (len(points), 1, 1)),
projected.as_matrix()), axis=2).reshape(-1, 6)
u, singular, vh = np.linalg.svd(design, full_matrices=False)
condition = float(singular[0] / max(singular[4], np.finfo(float).tiny))
solution = vh[:5].T @ ((u[:, :5].T @ points.ravel()) / singular[:5])
model = FrozenMotionConstraint(relation, tuple(rotations[0].as_quat()), tuple(axis),
tuple(solution[:3]), tuple(solution[3:]), tuple(stamps),
parameters.maximum_frame_off_axis_rad, parameters.maximum_frame_pivot_error_m)
return model, condition
def _residuals(model: FrozenMotionConstraint, rotations: Rotation, points: np.ndarray):
delta = rotations * Rotation.from_quat(model.reference_quaternion_xyzw).inv()
axis = np.asarray(model.axis_parent_xyz)
projected = Rotation.from_rotvec((delta.as_rotvec() @ axis)[:, None] * axis)
angular = (projected.inv() * delta).magnitude()
predicted = np.asarray(model.pivot_parent_xyz_m) + rotations.apply(model.mount_child_xyz_m)
return angular, np.linalg.norm(points - predicted, axis=1)
def _metrics(angular, pivot):
return (float(np.sqrt(np.mean(pivot**2))), float(np.percentile(pivot, 95)),
float(np.sqrt(np.mean(angular**2))), float(np.percentile(angular, 95)))
def _passes(values, parameters):
return all(value <= limit for value, limit in zip(values, (
parameters.maximum_pivot_rms_m, parameters.maximum_pivot_p95_m,
parameters.maximum_off_axis_rms_rad, parameters.maximum_off_axis_p95_rad)))
def _relation_evidence(relation, parent, child, stamps, parameters):
supported = [index for index, (p, c) in enumerate(zip(parent, child)) if p is not None and c is not None]
if len(supported) < parameters.minimum_motion_bins:
return None, MotionRelationQuality(joint=relation.joint, reason="insufficient_candidate_support")
pairs = [_relative_pose(parent[index], child[index]) for index in supported]
stamps = tuple(stamps[index] for index in supported)
rotations = Rotation.from_quat([rotation.as_quat() for rotation, _ in pairs])
points = np.asarray([point for _, point in pairs])
_, angles = _axis(rotations)
motion = float(np.ptp(angles))
# Use one actual image per angular bin; stationary endpoints and high frame
# rates cannot inflate the evidence count or overwhelm the moving arc.
bins: dict[int, list[int]] = {}
for index, angle in enumerate(angles):
key = int(np.floor((angle - float(np.min(angles))) / parameters.angular_bin_rad + 1e-9))
bins.setdefault(key, []).append(index)
indices = sorted(values[len(values) // 2] for values in bins.values())
span = float(np.linalg.norm(np.ptp(points[indices], axis=0)))
common = dict(joint=relation.joint, motion_rad=motion, motion_bins=len(indices), translation_span_m=span)
if motion < parameters.minimum_motion_rad or len(indices) < parameters.minimum_motion_bins:
return None, MotionRelationQuality(reason="insufficient_rotation_evidence", **common)
if motion >= math.radians(170):
return None, MotionRelationQuality(reason="rotation_log_wrap_unsupported", **common)
if span < parameters.minimum_translation_span_m:
return None, MotionRelationQuality(reason="insufficient_pivot_translation", **common)
rotations, points = rotations[indices], points[indices]
selected_stamps = tuple(stamps[index] for index in indices)
model, condition = _fit_constraint(relation, rotations, points, selected_stamps, parameters)
values = _metrics(*_residuals(model, rotations, points))
quality = dict(common, pivot_condition=condition, pivot_rms_m=values[0], pivot_p95_m=values[1],
off_axis_rms_rad=values[2], off_axis_p95_rad=values[3])
if condition > parameters.maximum_pivot_condition:
return None, MotionRelationQuality(reason="ill_conditioned_pivot", **quality)
if not _passes(values, parameters):
return None, MotionRelationQuality(reason="motion_geometry_residual", **quality)
# Check interleaved bins and separate early/late intervals in both
# directions. Dense or temporally correlated samples cannot independently
# confirm each other just by being adjacent. This is pre-freeze evidence
# only; no fourth calibration round is consumed here.
count = len(indices)
splits = ((np.arange(0, count, 2), np.arange(1, count, 2)),
(np.arange(count // 2), np.arange(count // 2, count)))
maximum_difference = 0.0
for first, second in splits:
models = []
for train, validate in ((first, second), (second, first)):
split, split_condition = _fit_constraint(relation, rotations[train], points[train],
tuple(selected_stamps[i] for i in train), parameters)
models.append(split)
if (split_condition > parameters.maximum_pivot_condition
or not _passes(_metrics(*_residuals(split, rotations[validate], points[validate])), parameters)):
return None, MotionRelationQuality(reason="motion_split_validation_failed", **quality)
difference = math.acos(float(np.clip(abs(np.dot(models[0].axis_parent_xyz,
models[1].axis_parent_xyz)), 0, 1)))
maximum_difference = max(maximum_difference, difference)
quality["split_axis_difference_rad"] = maximum_difference
if maximum_difference > parameters.maximum_split_axis_difference_rad:
return None, MotionRelationQuality(reason="motion_split_axis_inconsistent", **quality)
return model, MotionRelationQuality(reason="", **quality)
def resolve_motion_branches(
frames: Sequence[MotionEvidenceFrame], relations: Sequence[MotionRelation], *,
parameters: MotionEvidenceParameters = MotionEvidenceParameters(),
) -> MotionBranchResolution:
"""Resolve only a unique geometrically admissible pre-freeze hypothesis.
The caller authorizes this input as the current session's declared zero
approach, before any affected joint is frozen. A previously frozen role is
represented by its sole already-verified candidate, never both IPPE roots.
"""
roles = _validate_relations(relations)
if len(frames) < parameters.minimum_motion_bins:
return MotionBranchResolution(False, "motion_evidence_insufficient_frames")
stamps = tuple(frame.stamp_ns for frame in frames)
if any(type(stamp) is not int or stamp < 0 for stamp in stamps) or any(
a >= b for a, b in zip(stamps, stamps[1:])):
return MotionBranchResolution(False, "motion_evidence_image_identity_invalid")
paths, reason = _connect_paths(frames, roles)
if paths is None:
return MotionBranchResolution(False, reason)
hypotheses, accepted = [], []
for branches in product(*(range(len(paths[role])) for role in roles)):
selected = {role: paths[role][branch] for role, branch in zip(roles, branches)}
models, qualities = [], []
for relation in relations:
model, quality = _relation_evidence(relation, selected[relation.parent_role],
selected[relation.child_role], stamps, parameters)
qualities.append(quality)
if model is not None:
models.append(model)
valid = len(models) == len(relations) and all(selected[role][-1] is not None for role in roles)
hypotheses.append(MotionHypothesis(tuple(zip(roles, branches)), tuple(qualities), valid))
if valid:
accepted.append((selected, tuple(models)))
if len(accepted) != 1:
reason = "motion_geometry_ambiguous" if accepted else "motion_geometry_unresolved"
return MotionBranchResolution(False, reason, hypotheses=tuple(hypotheses))
selected, models = accepted[0]
return MotionBranchResolution(True, "", tuple(MotionRoleSelection(role, selected[role][-1])
for role in roles), models, tuple(hypotheses))
def select_motion_frame(
constraints: Sequence[FrozenMotionConstraint], current_candidates: Sequence[RoleCandidates],
) -> MotionFrameSelection:
"""Apply immutable evidence to this frame; never refit or synthesize poses."""
roles = _validate_relations(tuple(model.relation for model in constraints))
items = _candidate_map(current_candidates, roles)
if items is None:
return MotionFrameSelection(False, "motion_candidates_missing_or_invalid")
accepted = []
for combination in product(*(items[role].candidates for role in roles)):
poses = dict(zip(roles, combination))
if not all(model.evaluate(poses[model.relation.parent_role], poses[model.relation.child_role]).accepted
for model in constraints):
continue
if not any(all(_equivalent(poses[role], previous[role]) for role in roles) for previous in accepted):
accepted.append(poses)
if len(accepted) != 1:
return MotionFrameSelection(False, "motion_geometry_ambiguous" if accepted else "motion_geometry_inconsistent")
return MotionFrameSelection(True, "", tuple(MotionRoleSelection(role, accepted[0][role]) for role in roles))
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py
+223
View File
@@ -0,0 +1,223 @@
"""Image-domain articulated motion diagnosis, never branch authorization.
Each candidate family keeps its own fit. Frozen geometry is evaluated on held
out images by solving image angles only. No SDK values, calibration zero or
fourth-round fitting data enter this API. Callers must retain source provenance
and run this bounded but expensive analysis outside runtime state locks.
"""
from __future__ import annotations
from itertools import product
from typing import Sequence
import numpy as np
from scipy.spatial.transform import Rotation
from .motion_evidence import (
MotionEvidenceParameters, MotionRelation, _axis, _connect_paths, _fit_constraint, _residuals,
)
from .motion_image_solver import ImageHingeBundle
from .motion_image_types import (
ImageAxisRepeatability, ImageHypothesisDiagnostic, ImageIntervalDiagnostic,
ImageMotionFrame, ImageRoleObservation, ImageRoleQuality, MotionImageDiagnosticParameters,
MotionImageDiagnosticReport, PnpHingeDiagnostic,
)
from .relative import _relative_pose
__all__ = ["ImageMotionFrame", "ImageRoleObservation", "MotionImageDiagnosticParameters",
"MotionImageDiagnosticReport", "analyze_motion_images"]
def _ordered_relations(relations):
if not relations or len({r.joint for r in relations}) != len(relations):
raise ValueError("relations_missing_or_duplicate_joint")
children = {r.child_role for r in relations}
if len(children) != len(relations) or any(r.parent_role == r.child_role for r in relations):
raise ValueError("multiple_parent_or_self_relation")
known = {r.parent_role for r in relations} - children
pending, ordered = list(relations), []
while pending:
available = [r for r in pending if r.parent_role in known]
if not available:
raise ValueError("cyclic_motion_graph")
for relation in available:
ordered.append(relation)
known.add(relation.child_role)
pending.remove(relation)
return tuple(ordered)
def _validate_frames(frames, relations, limits):
if len(frames) < limits.minimum_frames:
raise ValueError("insufficient_image_frames")
if len(frames) > 512:
raise ValueError("source_frame_limit_exceeded")
stamps = [f.evidence.stamp_ns for f in frames]
if any(type(stamp) is not int or stamp <= 0 for stamp in stamps) or any(
newer <= older for older, newer in zip(stamps, stamps[1:])):
raise ValueError("image_identity_invalid")
roles = tuple(dict.fromkeys(role for r in relations for role in (r.parent_role, r.child_role)))
roots = set(roles) - {r.child_role for r in relations}
sizes = {}
first_matrix = np.asarray(frames[0].camera_matrix)
for frame in frames:
matrix = np.asarray(frame.camera_matrix)
if (matrix.shape != (3, 3) or not np.all(np.isfinite(matrix))
or matrix[0, 0] <= 0 or matrix[1, 1] <= 0
or not np.array_equal(matrix[2], [0, 0, 1])):
raise ValueError("camera_matrix_invalid")
if not np.array_equal(matrix, first_matrix):
raise ValueError("camera_model_changed")
observations = {o.role: o for o in frame.observations}
candidates = {r.role: r for r in frame.evidence.roles}
if len(observations) != len(frame.observations) or any(role not in observations for role in roles):
raise ValueError("image_observation_missing_or_duplicate")
if any(role not in candidates or not candidates[role].frozen for role in roots):
raise ValueError("root_pose_not_frozen")
for role in roles:
observation = observations[role]
corners = np.asarray(observation.corners_xy)
size = observation.tag_size_m
if (corners.shape != (4, 2) or not np.all(np.isfinite(corners))
or not np.isfinite(size) or size <= 0):
raise ValueError("tag_geometry_invalid")
if role in sizes and size != sizes[role]:
raise ValueError("tag_size_changed")
sizes[role] = size
paths, reason = _connect_paths([f.evidence for f in frames], roles)
if paths is None:
raise ValueError(reason)
return paths, roles
def _sample(indices, maximum):
if len(indices) <= maximum:
return list(indices)
return [indices[i] for i in np.linspace(0, len(indices) - 1, maximum, dtype=int)]
def _quality(residuals, roles):
norms = np.linalg.norm(np.asarray(residuals).reshape(-1, len(roles), 4, 2), axis=-1)
result = []
for index, role in enumerate(roles):
rms = np.sqrt(np.mean(norms[:, index] ** 2, axis=1))
result.append(ImageRoleQuality(role, len(rms), float(np.sqrt(np.mean(rms ** 2))),
float(np.percentile(rms, 95)), float(np.max(rms))))
return tuple(result)
def _subset(poses, indices):
return {role: tuple(items[i] for i in indices) for role, items in poses.items()}
def _interval(name, frames, relations, poses, train, validation, limits):
train = _sample(train, limits.maximum_training_frames)
validation = _sample(validation, limits.maximum_validation_frames)
stamps = tuple(frames[i].evidence.stamp_ns for i in train)
heldout = tuple(frames[i].evidence.stamp_ns for i in validation)
if len(train) < limits.minimum_frames or not heldout:
return ImageIntervalDiagnostic(name, False, "insufficient_interval_frames", 0, stamps, heldout)
try:
bundle = ImageHingeBundle(tuple(frames[i] for i in train), relations, _subset(poses, train))
fit = bundle.solve(limits)
residuals, valid = bundle.validate(fit.x, tuple(frames[i] for i in validation),
_subset(poses, validation), limits)
training = _quality(fit.fun, bundle.child_roles)
checked = _quality(residuals, bundle.child_roles)
converged = bool(fit.success and valid and np.all(np.isfinite(fit.fun))
and np.all(np.isfinite(residuals)))
return ImageIntervalDiagnostic(name, converged, "" if converged else "optimizer_or_projection_unresolved",
int(fit.nfev), stamps, heldout, training, checked, bundle.geometry(fit.x),
tuple(bundle.motion_spans))
except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error:
return ImageIntervalDiagnostic(name, False, str(error), 0, stamps, heldout)
def _pnp_diagnostics(frames, relations, poses):
result = []
for relation in relations:
parents, children = poses[relation.parent_role], poses[relation.child_role]
pairs = [_relative_pose(p, c) for p, c in zip(parents, children)]
rotations = Rotation.from_quat([r.as_quat() for r, _ in pairs])
points = np.asarray([t for _, t in pairs])
_, angles = _axis(rotations)
span = float(np.ptp(angles))
if span < MotionEvidenceParameters().minimum_motion_rad:
raise ValueError("insufficient_rotation_evidence")
if span >= np.deg2rad(170):
raise ValueError("rotation_log_wrap_unsupported")
model, condition = _fit_constraint(relation, rotations, points,
tuple(f.evidence.stamp_ns for f in frames), MotionEvidenceParameters())
if not np.isfinite(condition) or condition > MotionEvidenceParameters().maximum_pivot_condition:
raise ValueError("ill_conditioned_pivot")
angular, errors = _residuals(model, rotations, points)
difference = points - np.asarray(model.pivot_parent_xyz_m) - rotations.apply(model.mount_child_xyz_m)
camera_difference = Rotation.from_quat([p.quaternion_xyzw for p in parents]).apply(difference)
total_squared = float(np.sum(camera_difference ** 2))
depth_fraction = float(np.sum(camera_difference[:, 2] ** 2) / total_squared) if total_squared > 1e-24 else 0.0
result.append(PnpHingeDiagnostic(relation.joint, span,
float(np.sqrt(np.mean(errors ** 2))), float(np.percentile(errors, 95)),
depth_fraction, float(np.sqrt(np.mean(angular ** 2)))))
return tuple(result)
def _hypothesis(frames, relations, poses, branches, limits):
supported = [i for i in range(len(frames)) if all(items[i] is not None for items in poses.values())]
frames, poses = tuple(frames[i] for i in supported), _subset(poses, supported)
try:
pnp = _pnp_diagnostics(frames, relations, poses) if len(frames) >= limits.minimum_frames else ()
if not pnp:
return ImageHypothesisDiagnostic(branches, "insufficient_candidate_support")
except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error:
return ImageHypothesisDiagnostic(branches, str(error))
full = list(range(len(frames)))
train = _sample(full[::2], limits.maximum_training_frames)
heldout = [i for i in full if i not in set(train)]
midpoint = len(frames) // 2
intervals = (
_interval("interleaved", frames, relations, poses, train, heldout, limits),
_interval("early_to_late", frames, relations, poses, full[:midpoint], full[midpoint:], limits),
_interval("late_to_early", frames, relations, poses, full[midpoint:], full[:midpoint], limits),
)
repeatability = []
early, late = intervals[1:]
if early.converged and late.converged:
for first, second in zip(early.geometry, late.geometry):
dot = abs(float(np.dot(first.axis_parent_xyz, second.axis_parent_xyz)))
repeatability.append(ImageAxisRepeatability(first.relation.joint,
float(np.arccos(np.clip(dot, -1, 1))),
float(np.linalg.norm(np.subtract(first.pivot_parent_xyz_m, second.pivot_parent_xyz_m)))))
reason = "" if all(interval.converged for interval in intervals) else "incomplete_interval_analysis"
return ImageHypothesisDiagnostic(branches, reason, pnp, intervals, tuple(repeatability))
def analyze_motion_images(frames: Sequence[ImageMotionFrame], relations: Sequence[MotionRelation], *,
parameters: MotionImageDiagnosticParameters | None = None) -> MotionImageDiagnosticReport:
"""Return diagnostic evidence only; never select a winning branch or pose.
Maximum work is bounded by source frames, graph size, candidate families,
sampled image counts and optimizer evaluation limits. Roots must already be
frozen; calibration holdout images must never be passed as training frames.
"""
limits = parameters or MotionImageDiagnosticParameters()
frames, relations = tuple(frames), tuple(relations)
stamps = tuple(frame.evidence.stamp_ns for frame in frames)
try:
if len(relations) > limits.maximum_relations:
raise ValueError("motion_graph_complexity_limit")
relations = _ordered_relations(relations)
paths, roles = _validate_frames(frames, relations, limits)
family_count = int(np.prod([len(paths[role]) for role in roles]))
if family_count > limits.maximum_hypotheses:
raise ValueError("candidate_hypothesis_limit")
except (ValueError, TypeError) as error:
return MotionImageDiagnosticReport("not_available", str(error), stamps)
hypotheses = []
for indices in product(*(range(len(paths[role])) for role in roles)):
branches = tuple(zip(roles, indices))
poses = {role: paths[role][index] for role, index in branches}
hypotheses.append(_hypothesis(frames, relations, poses, branches, limits))
complete = all(not h.reason for h in hypotheses)
status = "available" if complete else "partial" if any(h.intervals for h in hypotheses) else "not_available"
return MotionImageDiagnosticReport(status, "" if complete else "some_hypotheses_not_evaluable",
stamps, tuple(hypotheses))
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py
+174
View File
@@ -0,0 +1,174 @@
"""Bounded articulated image fit for preparation and offline diagnostics."""
from __future__ import annotations
import numpy as np
from scipy.optimize import least_squares
from scipy.sparse import lil_matrix
from scipy.spatial.transform import Rotation
from .ippe import square_object_points
from .motion_evidence import MotionEvidenceParameters, _axis, _fit_constraint
from .relative import _relative_pose
from .motion_image_types import ImageHingeGeometry
def _basis(axis):
other = np.array([1., 0, 0]) if abs(axis[0]) < .9 else np.array([0., 1, 0])
first = np.cross(axis, other)
first /= np.linalg.norm(first)
return np.stack((first, np.cross(axis, first)), axis=1)
class ImageHingeBundle:
"""Shared hinge geometry and per-frame geometric angles; no SDK inputs.
Relations are topologically sorted. Roots are actual, already frozen poses
for each image, so a previously frozen moving parent is supported too.
Every child has one parent. Each hinge removes the unobservable axial pivot
gauge explicitly. Validation may change angles only, never shared geometry.
"""
def __init__(self, frames, relations, poses):
self.frames, self.relations, self.poses = frames, relations, poses
self.count, self.joints = len(frames), len(relations)
self.child_roles = tuple(r.child_role for r in relations)
self.root_roles = tuple(dict.fromkeys(r.parent_role for r in relations
if r.parent_role not in self.child_roles))
self.axes, self.bases, self.references, angles, initial = [], [], [], [], []
self.motion_spans = []
for relation in relations:
pairs = [_relative_pose(parent, child) for parent, child in
zip(poses[relation.parent_role], poses[relation.child_role])]
rotations = Rotation.from_quat([r.as_quat() for r, _ in pairs])
points = np.asarray([t for _, t in pairs])
axis, theta = _axis(rotations)
# The full arc has passed the diagnostic minimum-span check. A
# half interval is only a counterfactual fit: retain its measured
# span in the report rather than treating it as new authorization.
span = float(np.ptp(theta))
if span < 1e-6:
raise ValueError("insufficient_interval_rotation")
self.motion_spans.append((relation.joint, span))
model, condition = _fit_constraint(relation, rotations, points,
tuple(f.evidence.stamp_ns for f in frames), MotionEvidenceParameters())
if not np.isfinite(condition) or condition > MotionEvidenceParameters().maximum_pivot_condition:
raise ValueError("ill_conditioned_interval_pivot")
pivot, mount = np.array(model.pivot_parent_xyz_m), np.array(model.mount_child_xyz_m)
axial = float(pivot @ axis)
pivot -= axial * axis
mount += rotations[0].inv().apply(axis) * axial
basis = _basis(axis)
self.axes.append(axis)
self.bases.append(basis)
self.references.append(rotations[0])
angles.append(theta)
initial.extend((0., 0., 0., 0., 0., *(basis.T @ pivot), *mount))
self.initial = np.r_[initial, np.asarray(angles).T[1:].ravel()]
self.scale = np.r_[np.tile([1, 1, 1, 1, 1, .05, .05, .05, .05, .05], self.joints),
np.ones(self.joints * (self.count - 1))]
self.root_transforms, self.matrices, self.observed, self.objects = self._image_inputs(frames, poses)
@property
def static_parameter_count(self):
return 10 * self.joints
def geometry_parameter_indices(self, index):
return tuple(range(index * 10, (index + 1) * 10))
def geometry_dependency_indices(self, index):
return self.geometry_parameter_indices(index)
def _image_inputs(self, frames, poses):
roots = {role: (Rotation.from_quat([p.quaternion_xyzw for p in poses[role]]),
np.asarray([p.translation_xyz_m for p in poses[role]])) for role in self.root_roles}
observations = [{o.role: o for o in frame.observations} for frame in frames]
observed = np.asarray([[o[role].corners_xy for role in self.child_roles] for o in observations])
objects = {role: square_object_points(observations[0][role].tag_size_m) for role in self.child_roles}
return roots, np.asarray([f.camera_matrix for f in frames]), observed, objects
def geometry_values(self, parameters, index):
values = parameters[index * 10:(index + 1) * 10]
change = Rotation.from_rotvec(self.bases[index] @ values[:2])
axis = change.apply(self.axes[index])
reference = Rotation.from_rotvec(values[2:5]) * self.references[index]
# Transport the initial tangent basis with the axis. Rebuilding
# _basis(axis) switches its helper at |axis.x|=.9, which changes the
# meaning of the same pivot coordinates discontinuously during a fit.
pivot = change.apply(self.bases[index] @ values[5:7])
return axis, reference, pivot, values[7:10]
def geometry(self, parameters):
result = []
for index, relation in enumerate(self.relations):
axis, reference, pivot, mount = self.geometry_values(parameters, index)
result.append(ImageHingeGeometry(relation, tuple(axis), tuple(reference.as_quat()),
tuple(pivot), tuple(mount)))
return tuple(result)
def project(self, parameters, angles, roots, matrices, objects):
transforms = dict(roots)
projected, valid = [], True
for index, relation in enumerate(self.relations):
axis, reference, pivot, mount = self.geometry_values(parameters, index)
relative = Rotation.from_rotvec(angles[:, index, None] * axis) * reference
parent_rotation, parent_translation = transforms[relation.parent_role]
translation = parent_translation + parent_rotation.apply(pivot + relative.apply(mount))
rotation = parent_rotation * relative
transforms[relation.child_role] = (rotation, translation)
camera_points = np.einsum('nij,kj->nki', rotation.as_matrix(), objects[relation.child_role])
camera_points += translation[:, None, :]
valid = valid and bool(np.all(camera_points[:, :, 2] > 1e-6))
pixels = np.einsum('nij,nkj->nki', matrices, camera_points)
depth = np.maximum(pixels[:, :, 2:], 1e-6)
projected.append(pixels[:, :, :2] / depth)
return np.stack(projected, axis=1), valid
def residual(self, parameters):
angles = np.vstack((np.zeros(self.joints), parameters[self.static_parameter_count:].reshape(-1, self.joints)))
pixels, _ = self.project(parameters, angles, self.root_transforms, self.matrices, self.objects)
return (pixels - self.observed).ravel()
def solve(self, parameters):
# A child's image depends on its ancestors' parameters and angles only.
ancestry = {}
for index, relation in enumerate(self.relations):
ancestry[relation.child_role] = (*ancestry.get(relation.parent_role, ()), index)
width = 8 * self.joints
sparsity = lil_matrix((self.count * width, len(self.initial)), dtype=int)
for frame in range(self.count):
for child, role in enumerate(self.child_roles):
rows = slice(frame * width + child * 8, frame * width + (child + 1) * 8)
for ancestor in ancestry[role]:
sparsity[rows, self.geometry_parameter_indices(ancestor)] = 1
if frame:
sparsity[rows, self.static_parameter_count + (frame - 1) * self.joints + ancestor] = 1
return least_squares(self.residual, self.initial, jac_sparsity=sparsity.tocsr(),
x_scale=self.scale, max_nfev=parameters.maximum_solver_evaluations,
# LSMR's default 1e-6 inner tolerance leaves coupled hinge/angle
# steps too approximate near the image minimum. Tighten that
# linear solve so the unchanged outer convergence tolerances can
# be reached within the bounded evaluation budget.
tr_options={"atol": 1e-10, "btol": 1e-10},
ftol=1e-9, xtol=1e-9, gtol=1e-7)
def validate(self, parameters, frames, poses, limits):
roots, matrices, observed, objects = self._image_inputs(frames, poses)
residuals, converged = [], True
for frame in range(len(frames)):
guess = []
for index, relation in enumerate(self.relations):
rotation, _ = _relative_pose(poses[relation.parent_role][frame], poses[relation.child_role][frame])
axis, reference, _, _ = self.geometry_values(parameters, index)
guess.append(float((rotation * reference.inv()).as_rotvec() @ axis))
frame_roots = {role: (r[frame:frame + 1], t[frame:frame + 1]) for role, (r, t) in roots.items()}
def residual(angles):
pixels, _ = self.project(parameters, angles.reshape(1, self.joints), frame_roots,
matrices[frame:frame + 1], objects)
return (pixels[0] - observed[frame]).ravel()
fit = least_squares(residual, guess, max_nfev=limits.maximum_frame_evaluations,
ftol=1e-9, xtol=1e-9, gtol=1e-7)
_, positive_depth = self.project(parameters, fit.x.reshape(1, self.joints), frame_roots,
matrices[frame:frame + 1], objects)
converged = converged and bool(fit.success and positive_depth)
residuals.append(fit.fun.reshape(self.joints, 4, 2))
return np.asarray(residuals), converged
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py
+121
View File
@@ -0,0 +1,121 @@
"""Immutable inputs and reports for image-domain motion diagnostics.
These types carry no branch authorization, zero reference or calibration map.
Image residuals and metric PnP residuals deliberately have different fields.
"""
from __future__ import annotations
from dataclasses import dataclass, field
from .motion_evidence import MotionEvidenceFrame, MotionRelation
@dataclass(frozen=True)
class ImageRoleObservation:
role: str
tag_size_m: float
corners_xy: tuple[tuple[float, float], ...]
@dataclass(frozen=True)
class ImageMotionFrame:
evidence: MotionEvidenceFrame
camera_matrix: tuple[tuple[float, float, float], ...]
observations: tuple[ImageRoleObservation, ...]
@dataclass(frozen=True)
class MotionImageDiagnosticParameters:
maximum_training_frames: int = 36
maximum_validation_frames: int = 72
maximum_solver_evaluations: int = 80
maximum_frame_evaluations: int = 25
maximum_relations: int = 3
maximum_hypotheses: int = 8
minimum_frames: int = 12
def __post_init__(self):
for name, value in vars(self).items():
if type(value) is not int or value <= 0:
raise ValueError(f"{name} must be a positive integer")
if self.minimum_frames < 12 or self.maximum_training_frames < self.minimum_frames:
raise ValueError("image diagnostics require at least 12 training frames")
if (self.maximum_training_frames > 72 or self.maximum_validation_frames > 144
or self.maximum_solver_evaluations > 150 or self.maximum_frame_evaluations > 50
or self.maximum_relations > 3 or self.maximum_hypotheses > 8):
raise ValueError("image diagnostic computation limit exceeded")
@dataclass(frozen=True)
class ImageRoleQuality:
role: str
frames: int
rms_px: float
frame_rms_p95_px: float
maximum_frame_rms_px: float
@dataclass(frozen=True)
class ImageHingeGeometry:
relation: MotionRelation
axis_parent_xyz: tuple[float, float, float]
reference_quaternion_xyzw: tuple[float, float, float, float]
pivot_parent_xyz_m: tuple[float, float, float]
mount_child_xyz_m: tuple[float, float, float]
@dataclass(frozen=True)
class ImageIntervalDiagnostic:
name: str
converged: bool
reason: str
solver_evaluations: int
training_stamps_ns: tuple[int, ...]
validation_stamps_ns: tuple[int, ...]
training_quality: tuple[ImageRoleQuality, ...] = ()
validation_quality: tuple[ImageRoleQuality, ...] = ()
geometry: tuple[ImageHingeGeometry, ...] = ()
training_motion_span_rad_by_joint: tuple[tuple[str, float], ...] = ()
@dataclass(frozen=True)
class PnpHingeDiagnostic:
joint: str
motion_span_rad: float
translation_rms_m: float
translation_p95_m: float
camera_depth_squared_error_fraction: float
off_axis_rms_rad: float
@dataclass(frozen=True)
class ImageAxisRepeatability:
joint: str
early_late_axis_difference_rad: float
early_late_pivot_difference_m: float
@dataclass(frozen=True)
class ImageHypothesisDiagnostic:
branches: tuple[tuple[str, int], ...]
reason: str
pnp_geometry: tuple[PnpHingeDiagnostic, ...] = ()
intervals: tuple[ImageIntervalDiagnostic, ...] = ()
repeatability: tuple[ImageAxisRepeatability, ...] = ()
@dataclass(frozen=True)
class MotionImageDiagnosticReport:
status: str
reason: str
source_stamps_ns: tuple[int, ...]
hypotheses: tuple[ImageHypothesisDiagnostic, ...] = ()
scope: str = field(default="diagnostic_not_branch_authorization", init=False)
branch_authorization_allowed: bool = field(default=False, init=False)
calibration_accuracy_verified: bool = field(default=False, init=False)
interpretation: str = (
"Pixel residuals test image consistency; PnP metric residuals test noisy pose estimates. "
"Neither lowest pixel score nor this diagnostic replaces the existing branch, "
"baseline, interval-repeatability or independent calibration holdout gates. "
"The first training angle is a solver gauge, not a joint zero reference."
)
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py
+14
View File
@@ -5,6 +5,10 @@ from __future__ import annotations
from dataclasses import dataclass
import math
BRANCH_TRANSLATION_EQUIVALENCE_M = 0.001
@dataclass(frozen=True)
class PoseTrackingParameters:
"""One shared set of defaults, with angles stored in radians."""
@@ -15,7 +19,17 @@ class PoseTrackingParameters:
maximum_translation_jump_m: float = 0.04
maximum_tag_tilt_rad: float = math.radians(75.0)
reset_after_seconds: float = 5.0
branch_confirmation_frames: int = 5
branch_confirmation_minimum_seconds: float = 0.1
branch_equivalence_rad: float = math.radians(1.0)
DEFAULT_POSE_TRACKING_PARAMETERS = PoseTrackingParameters()
DEFAULT_REPROJECTION_TIE_PX = DEFAULT_POSE_TRACKING_PARAMETERS.reprojection_tie_px
POSE_TRACKING_POLICY_VERSION = "confirmed_branch_v8_training_model_selection"
FROZEN_BRANCH_POLICY_VERSIONS = frozenset({
"confirmed_branch_v1", "confirmed_branch_v2_fixed_reference",
"confirmed_branch_v3_recoverable_ambiguity", "confirmed_branch_v5_image_motion",
"confirmed_branch_v6_source_geometry", "confirmed_branch_v7_final_image_sampling",
POSE_TRACKING_POLICY_VERSION,
})
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py
+230
View File
@@ -0,0 +1,230 @@
"""Pre-zero motion authorization from images, followed by frozen image geometry.
A hinge bundle minimizes measured corner errors, not the poorly conditioned
single-image PnP depth estimates. All IPPE families seed independent fits. A
model is authorized only with observable geometry, absolute image quality and
held-out, motion-stratified evidence separating every distinct alternative.
This preparation-image check does not replace final fourth-round acceptance.
"""
from dataclasses import dataclass, replace
from itertools import product
import numpy as np
from .image_motion_model import (
IMAGE_MOTION_MODEL_POLICY, ImageMotionModel, ImageMotionResolution,
ImageMotionHypothesis, ImageMotionFrameSelection, image_motion_model_from_dict,
)
from .image_motion_projection import select_image_motion_frame
from .motion_evidence import MotionEvidenceParameters
from .image_model_selection import (
compare_image_losses as _comparison, equivalent_frame_selections,
holm_adjusted_probabilities, training_candidate_index,
)
from .motion_image_diagnostics import (
_ordered_relations, _validate_frames, _sample, _subset, _quality, _pnp_diagnostics,
)
from .motion_image_solver import ImageHingeBundle
__all__ = ["ImageMotionModel", "ImageMotionResolution", "ImageMotionFrameSelection",
"resolve_image_motion", "select_image_motion_frame", "image_motion_model_from_dict",
"IMAGE_MOTION_MODEL_POLICY"]
@dataclass(frozen=True)
class ImageMotionParameters:
maximum_training_frames: int = 72
maximum_validation_frames: int = 72
maximum_solver_evaluations: int = 150
maximum_frame_evaluations: int = 40
minimum_frames: int = 24
maximum_hypotheses: int = 64
maximum_relations: int = 6
familywise_error_probability: float = .01
def __post_init__(self):
for name in ("maximum_training_frames", "maximum_validation_frames", "maximum_solver_evaluations",
"maximum_frame_evaluations", "minimum_frames", "maximum_hypotheses", "maximum_relations"):
if type(getattr(self, name)) is not int or getattr(self, name) <= 0:
raise ValueError(f"{name} must be a positive integer")
if (self.minimum_frames < 24 or self.maximum_training_frames < 12
or self.maximum_training_frames > 72 or self.maximum_validation_frames > 144
or self.maximum_solver_evaluations > 500 or self.maximum_frame_evaluations > 100
or self.maximum_hypotheses > 256 or self.maximum_relations > 8
or not 0 < self.familywise_error_probability <= .01):
raise ValueError("image_motion_limits_invalid")
def _uncertainty(bundle, fit):
"""Gaussian local confidence after estimating all per-image nuisance angles.
The bundle already fixes angle and axial-pivot gauges. SVD of the full
Jacobian accounts for coupling to per-frame angles. Residual variance has a
0.03 pixel floor, the existing image branch resolution, to avoid claiming
infinite precision on synthetic or rounded input. Three-sigma conservative
vector bounds cover the axis's two and pivot's three coordinates at >95%.
"""
jacobian = fit.jac.toarray() if hasattr(fit.jac, "toarray") else np.asarray(fit.jac)
scaled = jacobian * bundle.scale[None, :]
_, singular, vh = np.linalg.svd(scaled, full_matrices=False)
if singular[-1] <= singular[0] * np.finfo(float).eps * max(scaled.shape):
raise ValueError("image_motion_geometry_rank_deficient")
sigma_squared = max(.03 ** 2, float(np.sum(fit.fun ** 2) / max(1, len(fit.fun) - len(fit.x))))
covariance_scaled = (vh.T / singular ** 2) @ vh * sigma_squared
covariance = covariance_scaled * bundle.scale[:, None] * bundle.scale[None, :]
axes, pivots = [], []
for index, relation in enumerate(bundle.relations):
derivative = np.zeros((6, len(fit.x)))
for column in bundle.geometry_dependency_indices(index):
step = 1e-5 * bundle.scale[column]
upper, lower = fit.x.copy(), fit.x.copy()
upper[column] += step
lower[column] -= step
ua, _, up, _ = bundle.geometry_values(upper, index)
la, _, lp, _ = bundle.geometry_values(lower, index)
derivative[:, column] = np.r_[ua - la, up - lp] / (2 * step)
propagated = derivative @ covariance @ derivative.T
axes.append((relation.joint, float(3 * np.sqrt(max(0., np.linalg.eigvalsh(propagated[:3, :3])[-1])))))
pivots.append((relation.joint, float(3 * np.sqrt(max(0., np.linalg.eigvalsh(propagated[3:, 3:])[-1])))))
return tuple(axes), tuple(pivots)
def _model(bundle, fit, frames, branches, training, validation):
from .cad_hinge import CadImageHingeBundle
model_type, extra = ImageMotionModel, {}
if isinstance(bundle, CadImageHingeBundle):
from .cad_image_model import CadImageMotionModel
model_type = CadImageMotionModel
extra = {"constraints": bundle.constraints, "source_urdf_sha256": bundle.source_urdf_sha256}
return model_type(bundle.geometry(fit.x), tuple(tuple(v) for v in frames[0].camera_matrix),
tuple((role, next(o.tag_size_m for o in frames[0].observations if o.role == role))
for role, _ in branches), tuple(f.evidence.stamp_ns for f in frames),
tuple(frames[i].evidence.stamp_ns for i in training),
tuple(frames[i].evidence.stamp_ns for i in validation), branches, **extra)
def _fit_hypothesis(frames, relations, poses, branches, training, validation, limits,
*, geometry_constraints=(), source_urdf_sha256=""):
if any(pose is None for role in poses.values() for pose in role):
return ImageMotionHypothesis(branches, False, "incomplete_candidate_family"), None, None
try:
pnp = _pnp_diagnostics(frames, relations, poses)
selected = tuple(frames[i] for i in training)
if geometry_constraints:
from .cad_hinge import CadImageHingeBundle
bundle = CadImageHingeBundle(selected, relations, _subset(poses, training),
constraints=geometry_constraints, source_urdf_sha256=source_urdf_sha256)
else:
bundle = ImageHingeBundle(selected, relations, _subset(poses, training))
fit = bundle.solve(limits)
model = _model(bundle, fit, frames, branches, training, validation)
training_quality = _quality(fit.fun, bundle.child_roles)
training_converged = bool(fit.success and np.all(np.isfinite(fit.fun)))
axes, pivots = _uncertainty(bundle, fit) if training_converged else ((), ())
p = MotionEvidenceParameters()
reason = "" if training_converged else "image_motion_optimizer_unresolved"
if not reason and any(q.maximum_frame_rms_px > model.maximum_reprojection_error_px for q in training_quality):
reason = "image_motion_reprojection_failed"
if not reason and (any(v > p.maximum_split_axis_difference_rad for _, v in axes)
or any(v > p.maximum_frame_pivot_error_m for _, v in pivots)):
reason = "image_motion_geometry_uncertain"
training_accepted = not reason
try:
residuals, valid = bundle.validate(fit.x, tuple(frames[i] for i in validation),
_subset(poses, validation), limits)
validation_quality = _quality(residuals, bundle.child_roles)
except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error:
# Keep training eligibility: a failed holdout must veto the chosen
# candidate, never silently make the next candidate the winner.
residuals, valid, validation_quality = None, False, ()
reason = reason or str(error)
converged = bool(training_converged and valid and np.all(np.isfinite(residuals)))
if not reason and not converged:
reason = "image_motion_optimizer_unresolved"
if not reason and any(q.maximum_frame_rms_px > model.maximum_reprojection_error_px for q in validation_quality):
reason = "image_motion_reprojection_failed"
hypothesis = ImageMotionHypothesis(branches, converged, reason, training_quality, validation_quality,
tuple((r.joint, r.motion_span_rad) for r in pnp), axes, pivots,
training_solver_status=int(fit.status), training_solver_evaluations=int(fit.nfev),
training_accepted=training_accepted)
rms = None if residuals is None else np.sqrt(np.mean(np.sum(residuals ** 2, axis=-1), axis=(1, 2)))
return hypothesis, model, rms
except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error:
return ImageMotionHypothesis(branches, False, str(error)), None, None
def resolve_image_motion(frames, relations, *, parameters=None, geometry_constraints=(), source_urdf_sha256=""):
"""Only pre-zero preparation frames may enter this expensive training API.
Runtime owns session, motion and phase checks. This function knows no SDK
channels, baseline or round and cannot acquire extra training images itself.
Its output model retains the disjoint image identities needed for auditing.
"""
limits = parameters or ImageMotionParameters()
frames, relations = tuple(frames), tuple(relations)
try:
relations = _ordered_relations(relations)
if len(relations) > limits.maximum_relations:
raise ValueError("image_motion_graph_limit")
paths, roles = _validate_frames(frames, relations, limits)
if np.prod([len(paths[r]) for r in roles]) > limits.maximum_hypotheses:
raise ValueError("image_motion_hypothesis_limit")
training = _sample(list(range(0, len(frames), 2)), limits.maximum_training_frames)
validation = _sample([i for i in range(len(frames)) if i not in training], limits.maximum_validation_frames)
if min(len(training), len(validation)) < 12:
raise ValueError("image_motion_insufficient_independent_frames")
except (ValueError, TypeError) as error:
return ImageMotionResolution(False, str(error))
hypotheses, models, residuals = [], [], []
for indices in product(*(range(len(paths[r])) for r in roles)):
branches = tuple(zip(roles, indices))
poses = {role: paths[role][index] for role, index in branches}
geometry = {"geometry_constraints": geometry_constraints, "source_urdf_sha256": source_urdf_sha256}
hypothesis, model, rms = _fit_hypothesis(frames, relations, poses, branches, training, validation, limits,
**(geometry if geometry_constraints else {}))
hypotheses.append(hypothesis)
models.append(model)
residuals.append(rms)
winner = training_candidate_index(hypotheses)
if winner is None or hypotheses[winner].reason:
return ImageMotionResolution(False, "image_motion_no_observable_image_model", hypotheses=tuple(hypotheses))
model = models[winner]
checks = tuple(select_image_motion_frame(model, frames[i]) for i in validation)
if not all(c.resolved for c in checks):
return ImageMotionResolution(False, "image_motion_validation_frame_unresolved", hypotheses=tuple(hypotheses))
angles = [[v for _, v in c.angles_rad] for c in checks]
winner_errors = np.array([np.sqrt(np.mean([q.rms_px ** 2 for q in c.quality])) for c in checks])
comparisons = []
for index, alternative in enumerate(models):
if index == winner:
continue
if alternative is None or residuals[index] is None or not hypotheses[index].converged:
# Missing candidate evidence is not evidence excluding a branch.
return ImageMotionResolution(False, "image_motion_alternative_not_evaluable", hypotheses=tuple(hypotheses))
if hypotheses[index].reason == "image_motion_reprojection_failed":
# This converged model violates the unchanged absolute image gate
# on training/held-out images. It is already an inadmissible
# explanation, not a competing accepted family whose current-frame
# angle uncertainty should prevent the valid model being used.
continue
alternative_checks = tuple(select_image_motion_frame(alternative, frames[i]) for i in validation)
equivalent = all(equivalent_frame_selections(checked, other)
for checked, other in zip(checks, alternative_checks))
if equivalent:
comparisons.append((index, 0.))
continue
if not all(c.quality for c in alternative_checks):
return ImageMotionResolution(False, "image_motion_alternative_angles_unresolved", hypotheses=tuple(hypotheses))
alternative_errors = np.array([np.sqrt(np.mean([q.rms_px ** 2 for q in c.quality]))
for c in alternative_checks])
probability = _comparison(winner_errors, alternative_errors, angles, model.reprojection_tie_px)
comparisons.append((index, probability))
adjusted = holm_adjusted_probabilities(comparisons)
hypotheses[winner] = replace(hypotheses[winner], comparison_p_values=tuple(comparisons),
comparison_adjusted_p_values=adjusted)
if any(probability > limits.familywise_error_probability for _, probability in adjusted):
return ImageMotionResolution(False, "image_motion_families_not_distinguishable", hypotheses=tuple(hypotheses))
final = select_image_motion_frame(model, frames[-1])
if not final.resolved:
return ImageMotionResolution(False, final.reason, hypotheses=tuple(hypotheses))
return ImageMotionResolution(True, "", model, tuple(hypotheses), final.selections)
src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py
+348 -110
View File
@@ -2,7 +2,10 @@
from __future__ import annotations
from dataclasses import replace
from contextlib import ExitStack
from copy import deepcopy
from dataclasses import asdict, dataclass, field, replace
from threading import RLock
from typing import Sequence
import math
@@ -10,9 +13,69 @@ from scipy.spatial.transform import Rotation
import cv2
import numpy as np
from .ippe import rotation_distance_rad, solve_square_tag_ippe
from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS, DEFAULT_REPROJECTION_TIE_PX
from .ippe import fixed_pose_reprojection_error, rotation_distance_rad, solve_square_tag_ippe
from .branch_confirmation import BranchConfirmation
from .fixed_reference import FixedTagReference
from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS, DEFAULT_REPROJECTION_TIE_PX, PoseTrackingParameters
from .types import SquareTagPose
from .image_motion_commit import (VerifiedImageMotionDecision,
verify_image_motion_frame, commit_image_motion_frame)
@dataclass(frozen=True)
class TrackerImageObservation:
"""Exact immutable pixels and optics retained independently of callers."""
stamp_ns: int
corners_xy: tuple[tuple[float, float], ...]
camera_matrix: tuple[tuple[float, float, float], ...]
tag_size_m: float
@dataclass(frozen=True)
class MotionTrackerSnapshot:
"""One image's immutable inputs for a lock-free geometric decision."""
tracker: SquareTagPoseTracker = field(repr=False)
role: str
epoch: int
stamp_ns: int
revision: int
frozen: bool
candidates: tuple[SquareTagPose, ...]
previous_pose: SquareTagPose | None
evidence_id: str | None
motion_managed: bool
image_observation: TrackerImageObservation | None = None
@dataclass(frozen=True)
class MotionPoseSelection:
"""A candidate selected by a previously checked geometric constraint."""
snapshot: MotionTrackerSnapshot
pose: SquareTagPose
evidence_id: str
@dataclass(frozen=True)
class MotionFrameCommit:
accepted: bool
reason: str
selections: tuple[MotionPoseSelection, ...] = ()
observation_snapshots: tuple[MotionObservationSnapshot, ...] = ()
@dataclass(frozen=True)
class MotionObservationSnapshot:
"""Detached evidence captured in the same critical section as selection."""
tracker: SquareTagPoseTracker = field(repr=False)
role: str
stamp_ns: int
candidates: tuple[SquareTagPose, ...]
diagnostics: dict[str, object]
corrections: int
def _validate_reprojection_thresholds(maximum_error: float, tie_error: float) -> None:
@@ -42,13 +105,9 @@ def select_continuous_pose(
maximum_rotation = float(maximum_pose_jump_rad)
maximum_translation = float(maximum_translation_jump_m)
maximum_tilt = float(maximum_tag_tilt_rad)
if min(
maximum_error,
maximum_rotation,
maximum_translation,
maximum_tilt,
) <= 0.0:
raise ValueError("PnP selection thresholds must be positive")
if any(not math.isfinite(value) or value <= 0 for value in (
maximum_error, maximum_rotation, maximum_translation, maximum_tilt)):
raise ValueError("PnP selection thresholds must be finite and positive")
eligible: list[SquareTagPose] = []
for candidate in candidates:
@@ -67,13 +126,9 @@ def select_continuous_pose(
if previous is None:
return best, ""
# Temporal continuity must only break a genuine planar-PnP tie. The old
# implementation normalised reprojection error by the permissive 1.5 px
# rejection limit, which allowed a stale mirror branch at 0.25 px to beat
# the true branch at e.g. 0.05 px merely because it was closer to the
# preceding (already wrong) pose. Once one IPPE solution has a meaningful
# image-fit advantage, trust it and allow the tracker to leave the stale
# branch even if that correction is a large pose jump.
# Image fit identifies a proposed branch. A stateless selector cannot
# confirm a large correction; the tracker below owns that evidence and
# knows whether a reference has already been frozen.
competitive = [
candidate
for candidate in eligible
@@ -81,6 +136,14 @@ def select_continuous_pose(
<= best.reprojection_error_px + tie_error
]
if len(competitive) == 1:
nearest = min(eligible, key=lambda candidate:
rotation_distance_rad(previous.quaternion_xyzw, candidate.quaternion_xyzw) / maximum_rotation
+ np.linalg.norm(np.asarray(previous.translation_xyz_m) - candidate.translation_xyz_m) / maximum_translation)
if (rotation_distance_rad(previous.quaternion_xyzw, best.quaternion_xyzw) > maximum_rotation
or np.linalg.norm(np.asarray(previous.translation_xyz_m) - best.translation_xyz_m) > maximum_translation
or rotation_distance_rad(nearest.quaternion_xyzw, best.quaternion_xyzw)
> DEFAULT_POSE_TRACKING_PARAMETERS.branch_equivalence_rad):
return None, "pose_branch_unconfirmed"
return best, ""
previous_translation = np.asarray(previous.translation_xyz_m, dtype=float)
@@ -139,6 +202,9 @@ class SquareTagPoseTracker:
maximum_translation_jump_m: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_translation_jump_m,
maximum_tag_tilt_rad: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_tag_tilt_rad,
reset_after_seconds: float = DEFAULT_POSE_TRACKING_PARAMETERS.reset_after_seconds,
branch_confirmation_frames: int = DEFAULT_POSE_TRACKING_PARAMETERS.branch_confirmation_frames,
branch_confirmation_minimum_seconds: float = DEFAULT_POSE_TRACKING_PARAMETERS.branch_confirmation_minimum_seconds,
branch_equivalence_rad: float = DEFAULT_POSE_TRACKING_PARAMETERS.branch_equivalence_rad,
) -> None:
self.maximum_reprojection_error_px = float(
maximum_reprojection_error_px
@@ -150,23 +216,230 @@ class SquareTagPoseTracker:
self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
self.maximum_translation_jump_m = float(maximum_translation_jump_m)
self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad)
if any(not math.isfinite(v) or v <= 0 for v in (
self.maximum_pose_jump_rad, self.maximum_translation_jump_m,
self.maximum_tag_tilt_rad, float(reset_after_seconds))):
raise ValueError("PnP tracking limits must be finite and positive")
self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000)
if self.reset_after_ns <= 0:
raise ValueError("reset_after_seconds must be positive")
self._previous: dict[str, tuple[int, SquareTagPose]] = {}
self.branch_confirmation_frames = branch_confirmation_frames
self.branch_confirmation_minimum_seconds = float(branch_confirmation_minimum_seconds)
self.branch_equivalence_rad = float(branch_equivalence_rad)
self._lock = RLock()
self._epoch = 0
self._branches = BranchConfirmation(PoseTrackingParameters(
self.maximum_reprojection_error_px, self.reprojection_tie_px,
self.maximum_pose_jump_rad, self.maximum_translation_jump_m,
self.maximum_tag_tilt_rad, float(reset_after_seconds),
branch_confirmation_frames, self.branch_confirmation_minimum_seconds, self.branch_equivalence_rad))
self.last_candidates_by_role: dict[
str, tuple[SquareTagPose, ...]
] = {}
self.last_candidate_diagnostics_by_role: dict[
str, dict[str, float | int]
str, dict[str, object]
] = {}
self.branch_correction_counts: dict[str, int] = {}
self.branch_correction_counts = self._branches.corrections
self._motion_candidates_by_role: dict[str, tuple[SquareTagPose, ...]] = {}
self._image_observations_by_role: dict[str, TrackerImageObservation] = {}
self._image_model_sha256_by_role: dict[str, str] = {}
def reset(self) -> None:
self._previous.clear()
with self._lock:
self._epoch += 1
self._branches.reset()
self.last_candidates_by_role.clear()
self.last_candidate_diagnostics_by_role.clear()
self.branch_correction_counts.clear()
self._motion_candidates_by_role.clear()
self._image_observations_by_role.clear()
self._image_model_sha256_by_role.clear()
def observation_snapshot(self, role, stamp_ns):
"""Copy one image's evidence atomically, never a subsequent callback."""
with self._lock:
diagnostics = self.last_candidate_diagnostics_by_role.get(role, {})
if diagnostics.get("observation_stamp_ns") != stamp_ns:
return None
return (self.last_candidates_by_role.get(role, ()), deepcopy(diagnostics),
self.branch_correction_counts.get(role, 0))
def motion_snapshot(self, role: str, stamp_ns: int) -> MotionTrackerSnapshot | None:
"""Copy candidates and version metadata; callers solve outside locks."""
with self._lock:
state = self._branches.states.get(role)
diagnostics = self.last_candidate_diagnostics_by_role.get(role, {})
if state is None or diagnostics.get("observation_stamp_ns") != stamp_ns:
return None
return MotionTrackerSnapshot(self, role, self._epoch, stamp_ns,
state.revision, state.frozen, self._motion_candidates_by_role.get(role, ()),
state.pose, state.motion_evidence_id, diagnostics.get("motion_managed") is True,
self._image_observations_by_role.get(role))
@staticmethod
def _motion_snapshot_is_current(snapshot: MotionTrackerSnapshot) -> bool:
"""Check metadata while the caller holds the corresponding tracker lock."""
tracker = snapshot.tracker
state = tracker._branches.states.get(snapshot.role)
diagnostics = tracker.last_candidate_diagnostics_by_role.get(snapshot.role, {})
return (tracker._epoch == snapshot.epoch and state is not None
and state.observed_ns == snapshot.stamp_ns and state.revision == snapshot.revision
and state.frozen == snapshot.frozen and state.motion_evidence_id == snapshot.evidence_id
and diagnostics.get("motion_managed") is True and snapshot.motion_managed
and diagnostics.get("motion_selection_committed") is not True)
@staticmethod
def _motion_observation_snapshots(snapshots: Sequence[MotionTrackerSnapshot]
) -> tuple[MotionObservationSnapshot, ...]:
"""Detach diagnostics under the same locks used for a frame decision."""
return tuple(MotionObservationSnapshot(snapshot.tracker, snapshot.role, snapshot.stamp_ns,
snapshot.tracker.last_candidates_by_role[snapshot.role],
deepcopy(snapshot.tracker.last_candidate_diagnostics_by_role[snapshot.role]),
snapshot.tracker.branch_correction_counts.get(snapshot.role, 0)) for snapshot in snapshots)
@staticmethod
def reject_motion_frame(snapshots: Sequence[MotionTrackerSnapshot], reason: str
) -> MotionFrameCommit:
"""Discard one unresolved image without changing a reference or model."""
snapshots = tuple(snapshots)
if not snapshots or not isinstance(reason, str) or not reason:
return MotionFrameCommit(False, "pose_motion_selection_invalid")
keys = {(snapshot.tracker, snapshot.role) for snapshot in snapshots}
if len(keys) != len(snapshots):
return MotionFrameCommit(False, "pose_motion_selection_duplicate_role")
with ExitStack() as stack:
for tracker in sorted({tracker for tracker, _ in keys}, key=id):
stack.enter_context(tracker._lock)
if not all(SquareTagPoseTracker._motion_snapshot_is_current(snapshot) for snapshot in snapshots):
return MotionFrameCommit(False, "pose_observation_superseded")
result_reason = reason
for snapshot in snapshots:
tracker, role = snapshot.tracker, snapshot.role
state = tracker._branches.states[role]
state.pending = None
if state.conflict:
result_reason = "pose_branch_conflict"
tracker._branches.reject(state, result_reason, "frozen_conflict")
else:
tracker._branches.reject(state, reason, "motion_unavailable")
diagnostics = tracker.last_candidate_diagnostics_by_role[role]
diagnostics.update(tracker._branches.diagnostics(role))
diagnostics["motion_selection_committed"] = True
diagnostics["pose_constraint"] = "motion_geometry"
return MotionFrameCommit(False, result_reason, observation_snapshots=
SquareTagPoseTracker._motion_observation_snapshots(snapshots))
@staticmethod
def commit_motion_frame(selections: Sequence[MotionPoseSelection], *,
establish: bool = False) -> MotionFrameCommit:
"""Commit a joint geometric decision without interleaving image metadata.
No candidate evaluation or model fitting happens here. Every candidate
must have passed this tracker's original image and tilt gates; every
snapshot must still identify the same image and reference revision.
Existing evidence identities, especially frozen ones, are immutable.
"""
selections = tuple(selections)
if not selections:
return MotionFrameCommit(False, "pose_motion_selection_missing")
keys = {(item.snapshot.tracker, item.snapshot.role) for item in selections}
if len(keys) != len(selections):
return MotionFrameCommit(False, "pose_motion_selection_duplicate_role")
with ExitStack() as stack:
for tracker in sorted({tracker for tracker, _ in keys}, key=id):
stack.enter_context(tracker._lock)
for item in selections:
snapshot = item.snapshot
tracker = snapshot.tracker
if not SquareTagPoseTracker._motion_snapshot_is_current(snapshot):
return MotionFrameCommit(False, "pose_observation_superseded")
if (not isinstance(item.evidence_id, str) or not item.evidence_id
or item.pose not in snapshot.candidates
or item.pose not in tracker._motion_candidates_by_role.get(snapshot.role, ())):
return MotionFrameCommit(False, "pose_motion_selection_invalid")
if any(item.snapshot.role in item.snapshot.tracker._image_model_sha256_by_role
for item in selections):
return MotionFrameCommit(False, "pose_motion_solver_changed")
return SquareTagPoseTracker._commit_motion_selections_locked(selections, establish=establish)
@staticmethod
def _commit_motion_selections_locked(selections, *, establish, image_decision=None):
"""Share the recovery/identity transaction across both trusted solvers."""
trials = []
for item in selections:
tracker, role = item.snapshot.tracker, item.snapshot.role
original = tracker._branches.states[role]
trial = replace(original,
pending=None if original.pending is None else replace(original.pending))
pose, reason = tracker._branches.select_motion(trial, item.pose,
item.evidence_id, establish=establish)
trials.append((tracker, role, trial, pose, reason))
failure = next((reason for _, _, _, pose, reason in trials if pose is None), "")
for tracker, role, trial, pose, _ in trials:
diagnostics = tracker.last_candidate_diagnostics_by_role[role]
diagnostics["motion_selection_committed"] = True
# Recovery needs several images, even if another role is not yet
# ready. A successful role cannot advance or establish a new
# identity when the joint decision as a whole was rejected.
if failure and pose is not None:
continue
original = tracker._branches.states[role]
if (not failure and original.motion_evidence_id is None and original.pose is not None
and not tracker._branches.continuous(original.pose, trial.pose)):
tracker.branch_correction_counts[role] = tracker.branch_correction_counts.get(role, 0) + 1
tracker._branches.states[role] = trial
diagnostics.update(tracker._branches.diagnostics(role))
diagnostics["pose_constraint"] = "motion_geometry"
if image_decision is not None and not failure:
diagnostics.update({
"pose_constraint": "image_motion_geometry",
"pose_source": "image_constrained_current_image",
"frozen_image_model": asdict(image_decision.model),
"image_model_sha256": image_decision.model_sha256,
"image_motion_quality": [asdict(item) for item in image_decision.quality],
})
tracker._image_model_sha256_by_role[role] = image_decision.model_sha256
observations = SquareTagPoseTracker._motion_observation_snapshots(
tuple(item.snapshot for item in selections))
if failure:
return MotionFrameCommit(False, failure, observation_snapshots=observations)
return MotionFrameCommit(True, "", selections, observations)
verify_image_motion_frame = staticmethod(verify_image_motion_frame)
commit_image_motion_frame = staticmethod(commit_image_motion_frame)
def freeze_roles(self, expected_revisions, *, require_motion_evidence=False):
return self.freeze_references([(self, role, revision) for role, revision in expected_revisions.items()],
require_motion_evidence=require_motion_evidence)
@staticmethod
def freeze_references(references, *, require_motion_evidence=False):
"""Atomically freeze metadata across views after evidence validation.
Callers may hold the session lock. No PnP work takes place under these
locks, and tracker code never acquires the session lock in reverse.
"""
references = tuple(references)
if not references:
return False
roles = {(tracker, role) for tracker, role, _ in references}
required = (roles if require_motion_evidence is True else set()
if require_motion_evidence is False else set(require_motion_evidence))
if not required.issubset(roles):
return False
with ExitStack() as stack:
for tracker in sorted({tracker for tracker, _, _ in references}, key=id):
stack.enter_context(tracker._lock)
for tracker, role, revision in references:
state = tracker._branches.states.get(role)
if (type(revision) is not int or state is None or state.revision != revision or state.status != "tracking"
or state.pending is not None or state.conflict or state.recovery_required):
return False
if (tracker, role) in required and state.motion_evidence_id is None:
return False
for tracker, role, _ in references:
tracker._branches.states[role].frozen = True
tracker.last_candidate_diagnostics_by_role[role]["branch_frozen"] = True
return True
def estimate(
self,
@@ -177,7 +450,15 @@ class SquareTagPoseTracker:
camera_matrix: Sequence[Sequence[float]],
stamp_ns: int,
reprojection_tie_px: float | None = None,
fixed_reference: FixedTagReference | None = None,
motion_managed: bool = False,
) -> tuple[SquareTagPose | None, str]:
with self._lock:
epoch = self._epoch
image_observation = TrackerImageObservation(int(stamp_ns),
tuple(tuple(float(v) for v in point) for point in corners_xy),
tuple(tuple(float(v) for v in row) for row in camera_matrix), float(tag_size_m))
corners_xy, camera_matrix = image_observation.corners_xy, image_observation.camera_matrix
try:
candidates = solve_square_tag_ippe(
corners_xy,
@@ -185,33 +466,7 @@ class SquareTagPoseTracker:
camera_matrix=camera_matrix,
)
except (ValueError, cv2.error):
self.last_candidates_by_role[str(role)] = ()
self.last_candidate_diagnostics_by_role[str(role)] = {
"solved_candidate_count": 0,
"reprojection_candidate_count": 0,
"independent_tilt_candidate_count": 0,
"maximum_reprojection_error_px": float(
self.maximum_reprojection_error_px
),
"maximum_independent_tilt_deg": math.degrees(
self.maximum_tag_tilt_rad
),
}
return None, "pnp_solve_failed"
if not candidates:
self.last_candidates_by_role[str(role)] = ()
self.last_candidate_diagnostics_by_role[str(role)] = {
"solved_candidate_count": 0,
"reprojection_candidate_count": 0,
"independent_tilt_candidate_count": 0,
"maximum_reprojection_error_px": float(
self.maximum_reprojection_error_px
),
"maximum_independent_tilt_deg": math.degrees(
self.maximum_tag_tilt_rad
),
}
return None, "pnp_solve_failed"
candidates = []
reprojection_candidates = [
candidate
for candidate in candidates
@@ -242,19 +497,12 @@ class SquareTagPoseTracker:
# image detections. Group tracking therefore receives every
# reprojection-valid candidate; independent tracking below retains the
# original tilt safety gate.
self.last_candidates_by_role[str(role)] = tuple(
reprojection_candidates
)
diagnostics: dict[str, float | int] = {
diagnostics: dict[str, object] = {
"motion_managed": bool(motion_managed and fixed_reference is None),
"motion_selection_committed": False,
"solved_candidate_count": len(candidates),
"reprojection_candidate_count": len(reprojection_candidates),
"independent_tilt_candidate_count": len(independent_candidates),
"minimum_reprojection_error_px": float(
min(
candidate.reprojection_error_px
for candidate in candidates
)
),
"maximum_reprojection_error_px": float(
self.maximum_reprojection_error_px
),
@@ -262,6 +510,8 @@ class SquareTagPoseTracker:
self.maximum_tag_tilt_rad
),
}
if candidates:
diagnostics["minimum_reprojection_error_px"] = min(c.reprojection_error_px for c in candidates)
if candidate_tilts_rad:
diagnostics["minimum_candidate_tilt_deg"] = math.degrees(
min(candidate_tilts_rad)
@@ -269,56 +519,44 @@ class SquareTagPoseTracker:
diagnostics["maximum_candidate_tilt_deg"] = math.degrees(
max(candidate_tilts_rad)
)
tie = self.reprojection_tie_px if reprojection_tie_px is None else float(reprojection_tie_px)
_validate_reprojection_thresholds(self.maximum_reprojection_error_px, tie)
checked_fixed_pose = None
if fixed_reference is not None:
# Projection is outside the metadata lock, just like IPPE. Keep
# all current candidates as evidence, even when their ranking
# changes while the frozen fixed pose still explains the image.
diagnostics["pose_constraint"] = "fixed_reference"
try:
error = fixed_pose_reprojection_error(fixed_reference.pose, corners_xy,
tag_size_m=tag_size_m, camera_matrix=camera_matrix)
normal = Rotation.from_quat(fixed_reference.pose.quaternion_xyzw).as_matrix()[:, 2]
tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0)))
diagnostics["fixed_reference_reprojection_error_px"] = error
if (error <= self.maximum_reprojection_error_px
and tilt <= self.maximum_tag_tilt_rad and independent_candidates):
checked_fixed_pose = replace(fixed_reference.pose, reprojection_error_px=error)
except (ValueError, cv2.error):
pass
diagnostics["fixed_reference_verified"] = checked_fixed_pose is not None
with self._lock:
if epoch != self._epoch:
return None, "pose_observation_superseded"
if fixed_reference is None and motion_managed:
selected, reason = self._branches.observe_motion(str(role), independent_candidates, int(stamp_ns))
elif fixed_reference is None:
selected, reason = self._branches.update(str(role), independent_candidates, int(stamp_ns), tie)
else:
selected, reason = self._branches.update_fixed(str(role), fixed_reference,
checked_fixed_pose, int(stamp_ns))
if reason == "pose_observation_not_new":
return None, reason
if not candidates and reason == "no_pose_within_reprojection_or_tilt_limit":
reason = "pnp_solve_failed"
self._branches.states[str(role)].reason = reason
diagnostics.update(self._branches.diagnostics(str(role)))
self.last_candidates_by_role[str(role)] = tuple(reprojection_candidates)
self._motion_candidates_by_role[str(role)] = tuple(independent_candidates)
self._image_observations_by_role[str(role)] = image_observation
self.last_candidate_diagnostics_by_role[str(role)] = diagnostics
if not independent_candidates:
return None, "no_pose_within_reprojection_or_tilt_limit"
previous_record = self._previous.get(str(role))
previous: SquareTagPose | None = None
if previous_record is not None:
previous_stamp, previous_pose = previous_record
elapsed = int(stamp_ns) - previous_stamp
if 0 <= elapsed <= self.reset_after_ns:
previous = previous_pose
selected, reason = select_continuous_pose(
independent_candidates,
previous=previous,
maximum_reprojection_error_px=(
self.maximum_reprojection_error_px
),
reprojection_tie_px=(
self.reprojection_tie_px
if reprojection_tie_px is None
else float(reprojection_tie_px)
),
maximum_pose_jump_rad=self.maximum_pose_jump_rad,
maximum_translation_jump_m=self.maximum_translation_jump_m,
maximum_tag_tilt_rad=self.maximum_tag_tilt_rad,
)
if selected is not None:
if previous is not None:
rotation_jump = rotation_distance_rad(
previous.quaternion_xyzw,
selected.quaternion_xyzw,
)
translation_jump = float(
np.linalg.norm(
np.asarray(selected.translation_xyz_m, dtype=float)
- np.asarray(
previous.translation_xyz_m,
dtype=float,
)
)
)
if (
rotation_jump > self.maximum_pose_jump_rad
or translation_jump
> self.maximum_translation_jump_m
):
key = str(role)
self.branch_correction_counts[key] = (
self.branch_correction_counts.get(key, 0) + 1
)
self._previous[str(role)] = (int(stamp_ns), selected)
return selected, reason
src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py
+87 -16
View File
@@ -16,6 +16,11 @@ from .patch import validate_urdf_mimic_ranges
from .validate import validate_runtime_curve_limits, validate_structural_urdf_diff
ANGLE_MAE_LIMIT_DEG = 1.0
ANGLE_P95_LIMIT_DEG = 2.0
ANGLE_MAXIMUM_LIMIT_DEG = 3.0
@dataclass(frozen=True)
class JointHoldout:
"""Independent observation in the corrected coordinate convention.
@@ -45,7 +50,9 @@ class ErrorMetrics:
@property
def passed(self) -> bool:
return self.count > 0 and self.mae_deg <= 1.0 and self.p95_deg <= 2.0 and self.maximum_deg <= 3.0
return (self.count > 0 and self.mae_deg <= ANGLE_MAE_LIMIT_DEG
and self.p95_deg <= ANGLE_P95_LIMIT_DEG
and self.maximum_deg <= ANGLE_MAXIMUM_LIMIT_DEG)
def angular_metrics(errors_rad: Sequence[float]) -> ErrorMetrics:
@@ -59,8 +66,9 @@ def angular_metrics(errors_rad: Sequence[float]) -> ErrorMetrics:
class SerializedJointMapping:
"""Read the numeric legacy v4/v6/v7 curve shapes without model imports.
Byte payloads preserve their deployed nearest-integer lookup; continuous
payloads interpolate explicit knots. Neither path silently clamps inputs.
Legacy byte tables and byte commands retain nearest-integer lookup.
Unified feedback uses its declared piecewise-linear knots, including
feedback interpolated to image time. Neither path silently clamps inputs.
"""
def __init__(self, payload: Mapping[str, Any], *, input_kind=None) -> None:
@@ -68,6 +76,14 @@ class SerializedJointMapping:
self.measured = payload.get("format") in {"unified_calibration_v3", "unified_calibration_report_v3"}
self.compact = payload.get("format") in {"unified_calibration_v2", "unified_calibration_v3"}
input_kind = input_kind or ("command" if self.compact else "feedback")
declared_directions = payload.get("directional_command_joints", [])
if (not isinstance(declared_directions, list) or any(not isinstance(n, str) for n in declared_directions)
or len(set(declared_directions)) != len(declared_directions)
or not set(declared_directions) <= payload.get("joints", {}).keys()
or declared_directions and not self.measured):
raise ValueError("invalid directional command declaration")
self.directional_joints = frozenset(declared_directions) if input_kind == "command" else frozenset()
self.baseline = payload.get("baseline_command", ())
self.unified = payload.get("format") in {"unified_calibration_v1", "unified_calibration_report_v3"}
if self.compact:
if input_kind != "command" or payload.get("schema_version") != (3 if self.measured else 2):
@@ -78,11 +94,15 @@ class SerializedJointMapping:
joints = {}
for name, row in payload.get("joints", {}).items():
expected = {"sdk_channel", "angle_rad"} | ({"input_values"} if unit == "rad" else set())
if name in self.directional_joints:
expected |= {"increasing_rad", "decreasing_rad"}
index = row.get("sdk_channel")
if set(row) != expected or type(index) is not int or index < 0:
raise ValueError(f"invalid compact joint/channel fields:{name}")
joints[name] = {"motor_index": index, "angle_rad": row["angle_rad"],
"curve_input_knots_rad": row.get("input_values", ())}
if name in self.directional_joints:
joints[name].update({key: row[key] for key in ("increasing_rad", "decreasing_rad")})
payload = {"joints": joints, "curve_input_domain": f"command_{unit}"}
elif self.unified:
if input_kind not in {"command", "feedback"} or payload.get("schema_version") != (3 if self.measured else 1):
@@ -153,6 +173,9 @@ class SerializedJointMapping:
selected = direction if directions_by_joint is None else directions_by_joint.get(name, "")
if selected not in {"increasing", "decreasing", ""}:
raise ValueError("invalid per-joint SDK direction")
if name in self.directional_joints and not selected:
if index >= len(self.baseline) or value != self.baseline[index]:
raise ValueError(f"directional_command_requires_direction:{name}")
if self.domain.endswith("_rad") and selected:
increasing_branch = str(row.get("raw_increasing_curve_branch", "increasing"))
if increasing_branch not in {"increasing", "decreasing"}:
@@ -165,10 +188,10 @@ class SerializedJointMapping:
angle = float(values[int(math.floor(value + 0.5))])
else:
knots = row["curve_input_knots_rad"]
if self.domain.endswith("_u8"):
value = float(math.floor(value+0.5))
if not knots[0] <= value <= knots[-1]:
raise ValueError(f"holdout input is outside serialized knot domain: {name}")
if self.domain == "command_u8":
value = float(math.floor(value+0.5))
angle = float(np.interp(value, knots, values))
output[name] = angle
return output
@@ -246,15 +269,41 @@ def validate_standard_urdf_holdout(
def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str | Path,
source_sha256: str, authorized_fields: Mapping[str, Sequence[str]],
payload: Mapping[str, Any], zero_offsets_rad: Mapping[str, float]) -> tuple[str, ...]:
"""Validate physical ranges in CAD coordinates, including coordinate shifts."""
payload: Mapping[str, Any], zero_offsets_rad: Mapping[str, float],
range_evidence=None) -> tuple[str, ...]:
"""Validate source/safety bounds or exact trusted training motion ranges."""
if len(source_sha256) != 64 or hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest() != source_sha256:
raise ValueError("protected source URDF SHA256 mismatch")
before, after = UrdfKinematicModel(source_urdf), UrdfKinematicModel(corrected_urdf)
from .joint_control import independent_joint_sources, independent_export_authorization, validate_independent_output, uses_source_mimic
mapping = SerializedJointMapping(payload)
sources = independent_joint_sources(payload)
if sources is not None:
channels = {name: row["motor_index"] for name, row in mapping.joints.items()}
authorized_fields = independent_export_authorization(before, channels, sources, authorized_fields)
validate_independent_output(after, channels, sources)
changes = validate_structural_urdf_diff(source_urdf, corrected_urdf,
authorized_fields=authorized_fields, expected_source_sha256=source_sha256)
before, after = UrdfKinematicModel(source_urdf), UrdfKinematicModel(corrected_urdf)
from .limits import validate_range_evidence
motion_ranges = dict(range_evidence or {})
if not set(motion_ranges) <= before.joints.keys():
raise ValueError("range evidence references unknown joints")
if not set(zero_offsets_rad) <= before.joints.keys():
raise ValueError("declared coordinate zero targets missing joints")
envelopes = {}
if uses_source_mimic(payload):
from .source_mimic import compile_source_mimic_envelopes, validate_source_mimic_elements, validate_serialized_mimic_envelopes
validate_source_mimic_elements(source_urdf, corrected_urdf)
envelopes = compile_source_mimic_envelopes(before,
{name: (j.lower, j.upper) for name, j in after.joints.items() if j.lower is not None},
zero_offsets_rad, motion_ranges)
validate_serialized_mimic_envelopes(payload, after)
from dataclasses import asdict
for name, envelope in envelopes.items():
declared = payload["urdf_correction"]["mimic_envelopes"][name]
if any(not np.allclose(values, declared[key], atol=1e-10, rtol=0)
for key, values in asdict(envelope).items()):
raise ValueError(f"source_mimic_envelope_differs_from_trusted_ranges:{name}")
# No model bypass, no inherited-CAD excess allowance at final acceptance.
validate_urdf_mimic_ranges(corrected_urdf)
for name, joint in after.joints.items():
@@ -263,12 +312,14 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str
if not math.isfinite(delta):
raise ValueError("nonfinite coordinate zero")
if joint.lower is not None and original.lower is not None:
if joint.lower + delta < original.lower - 1e-8 or joint.upper + delta > original.upper + 1e-8:
if name in motion_ranges:
interval = envelopes[name].base_limits_rad if name in envelopes else (joint.lower, joint.upper)
validate_range_evidence(name, original, delta, interval, motion_ranges[name])
elif joint.lower + delta < original.lower - 1e-8 or joint.upper + delta > original.upper + 1e-8:
raise ValueError(f"corrected range exceeds source mechanical range: {name}")
expected = original.origin @ before._motion(original, delta)
if not np.allclose(expected, joint.origin, atol=1e-8, rtol=0):
raise ValueError(f"URDF origin does not match declared coordinate zero: {name}")
mapping = SerializedJointMapping(payload)
if mapping.compact:
validate_compact_urdf_tables(payload, after)
for branch in ("angle_rad", "increasing_rad", "decreasing_rad"):
@@ -280,6 +331,13 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str
validate_runtime_curve_limits(corrected_urdf,
{name: row[branch] for name, row in commands.joints.items()})
for name, row in payload["joints"].items():
if sources is not None:
if row.get("passive") != (name in sources):
raise ValueError(f"passive_provenance_differs_from_source_urdf:{name}")
if name in sources and (row.get("mimic_approximation", {}).get("joint") != sources[name]
or row["mimic_approximation"].get("usage") != "diagnostic_only_not_exported"):
raise ValueError(f"diagnostic_mimic_source_differs:{name}")
continue
if row.get("passive"):
joint = after.joints[name]
expected = {"joint": joint.mimic_joint, "multiplier": joint.mimic_multiplier,
@@ -293,20 +351,31 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str
def validate_compact_urdf_tables(payload, model):
"""Redundant passive lookup entries must equal the written URDF, recursively."""
"""Validate curve ranges and the declared URDF angle-provider contract."""
mapping = SerializedJointMapping(payload, input_kind="command")
from .joint_control import independent_joint_sources, validate_independent_output, uses_affine_mimic_approximation, uses_source_mimic
sources = independent_joint_sources(payload)
preserved = uses_source_mimic(payload)
affine = uses_affine_mimic_approximation(payload) or preserved
if preserved:
from .source_mimic import validate_serialized_mimic_envelopes
validate_serialized_mimic_envelopes(payload, model)
if sources is not None:
validate_independent_output(model, {name: row["motor_index"] for name, row in mapping.joints.items()}, sources)
moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"}
if set(mapping.joints) != moving:
raise ValueError("compact JSON joints differ from the exported URDF")
for name, row in mapping.joints.items():
joint = model.joints[name]
values = np.asarray(row["angle_rad"])
if joint.lower is not None and (np.min(values) < joint.lower-1e-9 or np.max(values) > joint.upper+1e-9):
raise ValueError(f"compact table exceeds exported URDF range:{name}")
for branch in ("angle_rad", "increasing_rad", "decreasing_rad"):
branch_values = np.asarray(row.get(branch, row["angle_rad"]))
if joint.lower is not None and (np.min(branch_values) < joint.lower-1e-9 or np.max(branch_values) > joint.upper+1e-9):
raise ValueError(f"compact table exceeds exported URDF range:{name}:{branch}")
if joint.mimic_joint:
parent = mapping.joints[joint.mimic_joint]
if mapping.measured:
if row["motor_index"] != parent["motor_index"] or abs(joint.mimic_offset) > 1e-10:
if row["motor_index"] != parent["motor_index"] or (not affine and abs(joint.mimic_offset) > 1e-10):
raise ValueError(f"v3 mimic channel or shared zero differs:{name}")
continue
if (row["motor_index"] != parent["motor_index"]
@@ -323,6 +392,8 @@ def validate_compact_urdf_tables(payload, model):
index = row["motor_index"]
if index >= len(baseline):
raise ValueError("v3 baseline channel missing")
value = mapping.evaluate(baseline, "", active_joints=(name,))[name]
branches = ("", "increasing", "decreasing") if name in mapping.directional_joints else ("",)
for direction in branches:
value = mapping.evaluate(baseline, direction, active_joints=(name,))[name]
if abs(value) > 1e-10:
raise ValueError(f"v3 baseline is not zero:{name}")
raise ValueError(f"v3 baseline is not zero:{name}:{direction}")
src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py
+165
View File
@@ -0,0 +1,165 @@
"""Validate frozen final-file motion and its independent image evidence.
Primary measured poses must explain their original pixels. Their difference
from final-file motion is judged in degrees and metres, not by reusing the PnP
pixel limit for the sum of measurement and motion error. Extra views provide a
separate consistency check, never a substitute for independent 3-D accuracy.
No shared calibration parameter is estimated or changed during acceptance.
"""
from dataclasses import asdict, dataclass
import numpy as np
from scipy.spatial.transform import Rotation
from ..fitting.tag_installation import rigid_matrix
from ..geometry.tag_pose.ippe import square_object_points
from ..geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS
from .acceptance import SerializedJointMapping, angular_metrics
from .image_consistency import find_projection_witness, projection_error
from .kinematics import UrdfKinematicModel
from .joint_control import independent_joint_sources
from .tag_acceptance import POSITION_P95_LIMIT_M
FINAL_IMAGE_ACCEPTANCE_POLICY = "independent_measurement_and_physical_consistency_v2"
@dataclass(frozen=True)
class TagImageObservation:
sample_id: str
role: str
cycle: int
sdk_values: tuple[float, ...]
directions: tuple[tuple[str, str], ...]
view: str
tag_size_m: float
corners_xy: tuple[tuple[float, float], ...]
camera_matrix: tuple[tuple[float, float, float], ...]
view_from_common: tuple[tuple[float, ...], ...]
task_name: str = ""
class ImageHoldoutFailure(ValueError):
def __init__(self, report, *, input_kind, holdout_cycle):
self.report = report
self.input_kind = input_kind
self.holdout_cycle = holdout_cycle
failed = [role for role, metrics in report.items() if not metrics["passed"]]
super().__init__("final_image_holdout_failed:" + input_kind + ":" + ",".join(failed))
def _physical_metrics(values):
angles = angular_metrics([angle for angle, _ in values])
position = float(np.percentile([distance for _, distance in values], 95))
return {**asdict(angles), "translation_p95_m": position,
"passed": angles.passed and position <= POSITION_P95_LIMIT_M}
def _pixel_metrics(values):
return {"rms_px": float(np.sqrt(np.mean(np.square(values)))),
"p95_px": float(np.percentile(values, 95)), "maximum_px": max(values)}
def _image_geometry(row):
if not np.isfinite(row.tag_size_m) or row.tag_size_m <= 0:
raise ValueError("final_image_holdout_size_invalid")
matrix, measured = np.asarray(row.camera_matrix), np.asarray(row.corners_xy)
if (matrix.shape != (3, 3) or measured.shape != (4, 2)
or not np.all(np.isfinite(matrix)) or not np.all(np.isfinite(measured))
or matrix[0, 0] <= 0 or matrix[1, 1] <= 0 or not np.array_equal(matrix[2], [0, 0, 1])):
raise ValueError("final_image_holdout_projection_invalid")
return matrix, measured, square_object_points(row.tag_size_m)
def validate_serialized_image_holdout(*, corrected_urdf, payload, common_from_base,
installations, observations, independent_observations, required_roles, input_kind,
holdout_cycle, minimum_samples):
"""Keep 1.5 px measurement accuracy and the original 3-D motion limits.
Auxiliary witnesses only prove image compatibility within those limits.
They are grouped by motion task so stationary images cannot dilute the
errors in a different moving task. Independent poses remain mandatory.
"""
roles = frozenset(required_roles)
if (holdout_cycle not in {3, 5} or not roles or not roles <= installations.keys()
or minimum_samples < 1):
raise ValueError("final_image_holdout_contract_invalid")
model = UrdfKinematicModel(corrected_urdf)
mapping = SerializedJointMapping(payload, input_kind=input_kind)
active = tuple(name for name, joint in model.joints.items()
if joint.kind != "fixed" and (mapping.measured or joint.mimic_joint is None))
base = rigid_matrix(common_from_base)
training_ids = {sample for mount in installations.values() for sample in mount.training_sample_ids}
independent = {}
for pose in independent_observations:
identity = (pose.role, pose.sample_id)
if (identity in independent or pose.role not in roles or not pose.sample_id
or pose.cycle != holdout_cycle or pose.sample_id in training_ids):
raise ValueError("final_image_holdout_independent_identity_invalid")
independent[identity] = pose
errors = {role: [] for role in roles}
motion = {role: [] for role in roles}
auxiliary = {role: {} for role in roles}
seen = set()
limit = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px
for row in observations:
identity = (row.role, row.sample_id)
if (row.cycle != holdout_cycle or row.sample_id in training_ids or not row.sample_id
or identity in seen or row.role not in roles):
raise ValueError("final_image_holdout_identity_invalid")
seen.add(identity)
mount = installations[row.role]
if mount.role != row.role or mount.link not in model.links:
raise ValueError("final_image_holdout_installation_invalid")
angles = mapping.evaluate(row.sdk_values, "", active_joints=active,
directions_by_joint=dict(row.directions))
link = np.eye(4) if mount.link == model.root_link else model.link_transform(
model.parent_joint_by_child[mount.link], zero_offsets={}, joint_angles=angles,
independent_mimic_angles=mapping.measured and independent_joint_sources(payload) is None)
predicted = rigid_matrix(row.view_from_common) @ base @ link @ rigid_matrix(mount.link_from_tag)
camera, pixels, objects = _image_geometry(row)
nominal = projection_error(predicted, camera, objects, pixels)
measured = independent.get(identity)
if measured is not None:
if (tuple(measured.sdk_values) != row.sdk_values
or dict(measured.sdk_directions_by_joint) != dict(row.directions)):
raise ValueError("final_image_holdout_independent_input_changed")
observed = rigid_matrix(row.view_from_common) @ rigid_matrix(measured.common_from_tag)
residual = projection_error(observed, camera, objects, pixels)
angle = float(Rotation.from_matrix(predicted[:3, :3].T @ observed[:3, :3]).magnitude())
distance = float(np.linalg.norm(predicted[:3, 3]-observed[:3, 3]))
motion[row.role].append((angle, distance))
else:
if not row.task_name:
raise ValueError("final_image_holdout_auxiliary_task_missing")
witness = find_projection_witness(predicted, camera, objects, pixels, image_limit_px=limit)
residual = witness.residual_px
auxiliary[row.role].setdefault(row.task_name, []).append(witness)
errors[row.role].append((row, nominal, residual))
results = {}
for role in sorted(roles):
if len(motion[role]) < minimum_samples:
raise ValueError(f"final_image_holdout_missing:{role}:{len(motion[role])}")
values = errors[role]
residuals = [value[2] for value in values]
physical = _physical_metrics(motion[role])
tasks = {}
for task, witnesses in auxiliary[role].items():
metrics = _physical_metrics([(row.angle_rad, row.position_m) for row in witnesses])
tasks[task] = {**metrics, "interpretation": "feasible_pose_displacement_only",
"optimizer_not_converged_count": sum(not row.optimizer_converged for row in witnesses),
"maximum_px": max(row.residual_px for row in witnesses)}
tasks[task]["passed"] = metrics["passed"] and tasks[task]["maximum_px"] <= limit
worst = values[int(np.argmax(residuals))][0]
results[role] = {"count": len(values), **_pixel_metrics(residuals), "limit_px": limit,
"nominal_projection": _pixel_metrics([value[1] for value in values]),
"independent_motion": physical, "auxiliary_consistency_by_task": tasks,
"worst_sample_id": worst.sample_id, "view": worst.view,
"passed": (max(residuals) <= limit and physical["passed"]
and all(value["passed"] for value in tasks.values()))}
if not independent.keys() <= seen:
raise ValueError("final_image_holdout_independent_source_missing")
if not all(metrics["passed"] for metrics in results.values()):
raise ImageHoldoutFailure(results, input_kind=input_kind, holdout_cycle=holdout_cycle)
return results
src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_consistency.py
+109
View File
@@ -0,0 +1,109 @@
"""Check whether extra-view pixels admit a pose within a physical error budget.
A local pose is an existence witness, not an independently measured pose. It
never changes the base, mounting, camera, joints or exported calibration. The
caller must separately validate independent 3-D observations and check the
witness displacement against the same physical accuracy limits.
"""
from dataclasses import dataclass
import numpy as np
from scipy.optimize import minimize
from scipy.spatial.transform import Rotation
from .acceptance import ANGLE_MAXIMUM_LIMIT_DEG
from .tag_acceptance import POSITION_P95_LIMIT_M
@dataclass(frozen=True)
class ProjectionConsistency:
nominal_px: float
residual_px: float
angle_rad: float
position_m: float
iterations: int
optimizer_converged: bool
def _cross_matrix(vector):
a, b, c = vector
return np.array([[0., -c, b], [c, 0., -a], [-b, a, 0.]])
def _rigid_projection(x, pose, camera, objects, measured, angle_scale, position_scale):
"""Pixel residual and analytic Jacobian for a Tag-centred rigid displacement."""
vector = x[:3] * angle_scale
theta = float(np.linalg.norm(vector))
skew = _cross_matrix(vector)
if theta < 1e-4:
left = np.eye(3) + (.5-theta**2/24)*skew + (1/6-theta**2/120)*(skew @ skew)
else:
left = (np.eye(3) + (1-np.cos(theta))/theta**2*skew
+ (theta-np.sin(theta))/theta**3*(skew @ skew))
rotated = objects @ (Rotation.from_rotvec(vector).as_matrix() @ pose[:3, :3]).T
points = rotated + pose[:3, 3] + x[3:] * position_scale
if not np.all(np.isfinite(points)) or np.any(points[:, 2] <= 0):
raise ValueError("final_image_holdout_projection_invalid")
homogeneous = points @ camera.T
pixels = homogeneous[:, :2] / homogeneous[:, 2:]
point_jacobian = np.empty((4, 3, 6))
point_jacobian[:, :, :3] = [-_cross_matrix(point) @ left * angle_scale for point in rotated]
point_jacobian[:, :, 3:] = np.eye(3) * position_scale
projection = ((camera[:2, :][None] - pixels[:, :, None]*camera[2, :][None, None])
/ homogeneous[:, 2, None, None])
return pixels-measured, np.einsum("nki,nij->nkj", projection, point_jacobian)
def projection_error(pose, camera, objects, measured):
residual, _ = _rigid_projection(np.zeros(6), pose, camera, objects, measured, 1., 1.)
return float(np.sqrt(np.mean(np.sum(residual**2, axis=1))))
def find_projection_witness(pose, camera, objects, measured, *, image_limit_px):
"""Find a feasible pose; optimizer success alone never establishes acceptance.
The objective favours a small displacement in physical units. Its returned
displacement is not claimed to be globally minimal. The caller checks both
the directly evaluated pixel residual and the physical displacement.
"""
angle_scale = np.deg2rad(ANGLE_MAXIMUM_LIMIT_DEG)
position_scale = POSITION_P95_LIMIT_M
def evaluate(x):
return _rigid_projection(x, pose, camera, objects, measured, angle_scale, position_scale)
def rms(x):
residual, _ = evaluate(x)
return float(np.sqrt(np.mean(np.sum(residual**2, axis=1))))
zero = np.zeros(6)
nominal = rms(zero)
if nominal <= image_limit_px:
return ProjectionConsistency(nominal, nominal, 0., 0., 0, True)
# A small inward margin avoids accepting a constraint rounded above its
# limit. It does not enlarge either image or physical tolerances.
target = image_limit_px * (1.-1e-6)
def constraint_jacobian(x):
residual, jacobian = evaluate(x)
return -2*np.mean(np.einsum("nk,nkj->nj", residual, jacobian), axis=0)
constraint = {"type": "ineq", "fun": lambda x: target**2-rms(x)**2,
"jac": constraint_jacobian}
# Bounds limit numerical exploration, not the permitted physical error.
# Ensure every explored point remains in front of the image plane.
depth = pose[2, 3] - float(np.max(np.linalg.norm(objects, axis=1)))
lower_z = max(-5., -depth / position_scale * (1.-1e-6))
if lower_z >= 0:
raise ValueError("final_image_holdout_projection_invalid")
bounds = [(-5., 5.)]*5 + [(lower_z, 5.)]
result = minimize(lambda x: float(x @ x), zero, jac=lambda x: 2*x,
constraints=constraint, bounds=bounds, method="SLSQP",
options={"maxiter": 60, "ftol": 1e-10})
if not np.all(np.isfinite(result.x)):
raise ValueError("final_image_holdout_witness_nonfinite")
return ProjectionConsistency(nominal, rms(result.x),
float(np.linalg.norm(result.x[:3])*angle_scale),
float(np.linalg.norm(result.x[3:])*position_scale),
int(result.nit), bool(result.success))
src/linkerhand_calibration/linkerhand_calibration/core/urdf/joint_control.py
+78
View File
@@ -0,0 +1,78 @@
"""Versioned roles of measured JSON and standard URDF mimic approximations.
Production keeps mimic for standalone linear motion and uses explicit JSON
angles for measured motion. Version 2's mimic-free export remains readable;
its removal rights never authorize extra geometry or SDK actuation changes.
"""
JSON_JOINT_ANGLES = "calibration_json"
AFFINE_MIMIC_APPROXIMATION = "range_bounded_affine_approximation"
PRESERVE_SOURCE_MIMIC = "preserve_source_mimic"
def uses_source_mimic(payload):
correction = payload.get("urdf_correction", {})
if correction.get("schema_version") != 4:
return False
if (payload.get("format") not in {"unified_calibration_v3", "unified_calibration_report_v3"}
or correction.get("joint_angle_source") != JSON_JOINT_ANGLES
or correction.get("mimic_policy") != PRESERVE_SOURCE_MIMIC):
raise ValueError("unsupported_source_mimic_contract")
return True
def uses_affine_mimic_approximation(payload):
correction = payload.get("urdf_correction", {})
if correction.get("schema_version") != 3:
return False
if (payload.get("format") not in {"unified_calibration_v3", "unified_calibration_report_v3"}
or correction.get("joint_angle_source") != JSON_JOINT_ANGLES
or correction.get("mimic_policy") != AFFINE_MIMIC_APPROXIMATION):
raise ValueError("unsupported_urdf_mimic_approximation_contract")
return True
def independent_joint_sources(payload):
"""None denotes a URDF retaining its linear mimic constraints."""
correction = payload.get("urdf_correction", {})
version = correction.get("schema_version", 1)
if version == 1 or uses_affine_mimic_approximation(payload) or uses_source_mimic(payload):
return None
if (version != 2
or payload.get("format") not in {"unified_calibration_v3", "unified_calibration_report_v3"}
or correction.get("joint_angle_source") != JSON_JOINT_ANGLES):
raise ValueError("unsupported_urdf_joint_control_contract")
sources = correction.get("passive_joint_sources")
if not isinstance(sources, dict) or any(
not isinstance(name, str) or not isinstance(parent, str) or name == parent
for name, parent in sources.items()
):
raise ValueError("invalid_independent_passive_joint_sources")
return sources
def validate_joint_channels(model, channels, sources):
moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"}
if set(channels) != moving or any(type(index) is not int or index < 0 for index in channels.values()):
raise ValueError("independent_joint_channel_coverage_differs")
for name, parent in sources.items():
if name not in moving or parent not in moving or channels[name] != channels[parent]:
raise ValueError(f"independent_passive_sdk_channel_differs:{name}")
def independent_export_authorization(model, channels, sources, authorized_fields):
"""Authorize only the exact source mimic removals, after binding validation."""
original = {name: joint.mimic_joint for name, joint in model.joints.items() if joint.mimic_joint}
if sources != original:
raise ValueError("independent_passive_sources_differ_from_source_urdf")
validate_joint_channels(model, channels, sources)
rights = {name: tuple(fields) for name, fields in authorized_fields.items()}
for name in sources:
rights[name] = tuple(sorted(set(rights.get(name, ())) | {"mimic.remove"}))
return rights
def validate_independent_output(model, channels, sources):
validate_joint_channels(model, channels, sources)
if any(joint.mimic_joint is not None for joint in model.joints.values()):
raise ValueError("json_joint_angles_must_not_be_overridden_by_urdf_mimic")
src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py
+102
View File
@@ -0,0 +1,102 @@
"""Training-supported URDF motion envelopes and independent safety bounds.
CAD limits remain hard bounds unless the protected Profile explicitly declares
them to be nominal motion envelopes. This module never changes SDK commands,
adds tolerance padding, fits a curve, or consumes validation observations.
"""
from dataclasses import dataclass
import math
@dataclass(frozen=True)
class MotionRangeEvidence:
joint: str
source_joint: str
independently_measured: bool
source_limits_cad_rad: tuple[float, float]
output_limits_rad: tuple[float, float]
feedback_bounds_rad: tuple[float, float]
command_bounds_rad: tuple[float, float]
zero_offset_rad: float
baseline_command: float
training_cycles: tuple[int, ...]
training_sample_ids: tuple[str, ...]
reference_sample_ids: tuple[str, ...]
source_limits: str
evidence_source: str
independent_safety_limits_cad_rad: tuple[float, float] | None
safety_evidence_source: str
command_training_cycles: tuple[int, ...] = (0, 1, 2)
command_training_sample_ids: tuple[str, ...] = ()
def _interval(value):
return len(value) == 2 and all(math.isfinite(v) for v in value) and value[0] < value[1]
def validate_range_evidence(name, original, delta, interval, evidence):
"""Validate trusted fit evidence at both planning and final read-back."""
if (not isinstance(evidence, MotionRangeEvidence) or evidence.joint != name
or evidence.source_limits != "calibrated_motion_envelope" or not evidence.evidence_source.strip()
or evidence.training_cycles != (0, 1, 2) or not evidence.training_sample_ids
or evidence.command_training_cycles not in {(0, 1, 2), (4,)}
or (evidence.command_training_cycles == (4,) and not evidence.command_training_sample_ids)
or not evidence.reference_sample_ids or not evidence.source_joint
or evidence.independently_measured != (evidence.source_joint == name)
or evidence.source_limits_cad_rad != (original.lower, original.upper)
or evidence.zero_offset_rad != delta
or not _interval(evidence.feedback_bounds_rad) or not _interval(evidence.command_bounds_rad)):
raise ValueError(f"invalid_training_motion_range_evidence:{name}")
expected = (min(0.0, evidence.feedback_bounds_rad[0], evidence.command_bounds_rad[0]),
max(0.0, evidence.feedback_bounds_rad[1], evidence.command_bounds_rad[1]))
if evidence.output_limits_rad != expected or not _interval(interval) \
or any(abs(a-b) > 1e-10 for a, b in zip(interval, expected)):
raise ValueError(f"motion_range_differs_from_training_support:{name}")
safety = evidence.independent_safety_limits_cad_rad
if safety is None:
if evidence.safety_evidence_source:
raise ValueError(f"independent_safety_evidence_without_bounds:{name}")
elif (not _interval(safety) or not evidence.safety_evidence_source.strip()
or interval[0]+delta < safety[0]-1e-9 or interval[1]+delta > safety[1]+1e-9):
raise ValueError(f"measured_motion_exceeds_independent_safety_range:{name}")
def compile_motion_ranges(profile, model, fit):
"""Freeze bounds from existing 3-cycle fitted mappings, without a refit.
Copied curves retain donor image identities and never become independent
recipient measurements. All angular values already share the frozen zero.
"""
result = {}
from ..domain.sampling import command_training_cycles
command_cycles = command_training_cycles(profile)
for name, policy in profile.urdf_limit_policies.items():
source = profile.measurement.transferred_motion_sources.get(name, name)
curve = fit.curves[source]
reference = fit.joint_zero_references[source]
command_evidence = fit.command_holdout_metrics[source]
if (tuple(curve.circle.get("training_cycles", ())) != (0, 1, 2)
or tuple(command_evidence.get("training_cycles", ())) != command_cycles
or not curve.circle.get("training_sample_ids")
or not reference.samples):
raise ValueError(f"motion_range_requires_training_provenance:{name}")
feedback, command = fit.output_mappings[name], fit.command_mappings[name]
if (feedback.transferred_from != (source if source != name else None)
or command.transferred_from != feedback.transferred_from
or abs(command.evaluate(reference.baseline_command)) > 1e-10):
raise ValueError(f"motion_range_requires_shared_baseline_and_donor:{name}")
bounds = (min(0.0, feedback.bounds_rad[0], command.bounds_rad[0]),
max(0.0, feedback.bounds_rad[1], command.bounds_rad[1]))
original = model.joints[name]
evidence = MotionRangeEvidence(name, source, source == name,
(original.lower, original.upper), bounds, feedback.bounds_rad, command.bounds_rad,
fit.zero_offsets_rad[name], reference.baseline_command, (0, 1, 2),
tuple(sorted(curve.circle["training_sample_ids"])),
tuple(sorted(f"{sample.view}:{sample.image_stamp_ns}" for sample in reference.samples)),
policy.source_limits, policy.evidence_source, policy.independent_safety_limits_cad_rad,
policy.safety_evidence_source, command_cycles,
tuple(command_evidence.get("training_sample_ids", ())))
validate_range_evidence(name, original, fit.zero_offsets_rad[name], bounds, evidence)
result[name] = evidence
return result
src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py
+10 -3
View File
@@ -21,9 +21,15 @@ def validate_transferred_joint_tables(payload, measured_joints, transferred_moti
or recipient["curve_input_knots_rad"] != donor["curve_input_knots_rad"]
or baseline[recipient["motor_index"]] != baseline[donor["motor_index"]]):
raise ValueError(f"transferred_final_table_differs_from_source:{target}")
for branch in ("increasing_rad", "decreasing_rad"):
if recipient.get(branch) != donor.get(branch):
raise ValueError(f"transferred_final_table_differs_from_source:{target}:{branch}")
def validate_measured_joint_holdout(payload, curves, observations, *, transferred_motion_sources=None):
def validate_measured_joint_holdout(payload, curves, observations, *, transferred_motion_sources=None,
holdout_cycle=3):
if holdout_cycle not in {3, 5}:
raise ValueError("command validation requires a declared independent partition")
validate_transferred_joint_tables(payload, curves, transferred_motion_sources or {})
mapping = SerializedJointMapping(payload, input_kind="command")
errors = {name: {d: [] for d in ("increasing", "decreasing")} for name in curves}
@@ -31,13 +37,14 @@ def validate_measured_joint_holdout(payload, curves, observations, *, transferre
unit = payload["input_unit"]
for row in observations:
name, identity = row["joint"], row["sample_id"]
if name not in curves or row["cycle"] != 3 or row.get("sample_phase") != "steady":
if name not in curves or row["cycle"] != holdout_cycle or row.get("sample_phase") != "steady":
raise ValueError("v3 angle holdout has wrong partition or joint")
if (name, identity) in seen or identity in curves[name].circle["training_sample_ids"]:
raise ValueError("v3 angle holdout duplicates or overlaps training")
seen.add((name, identity))
measured = measure_joint_curve_observation(curves[name], quaternion_xyzw=row["relative_quaternion_xyzw"])
predicted = mapping.evaluate(row[f"command_vector_{unit}"], "", active_joints=(name,))[name]
predicted = mapping.evaluate(row[f"command_vector_{unit}"],
row["direction"] if name in mapping.directional_joints else "", active_joints=(name,))[name]
errors[name][row["direction"]].append(predicted-measured)
results = {}
for name, branches in errors.items():
src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py
+25 -8
View File
@@ -2,7 +2,7 @@
Model profiles decide *what* values are authorized. This module owns the
shared mechanics of locating those fields in the original XML text, changing
only the declared attributes, materializing mesh resources and atomically
only the declared attributes or explicit mimic removals, materializing mesh resources and atomically
publishing a new file.
"""
@@ -16,7 +16,7 @@ from pathlib import Path
import re
import shutil
import tempfile
from typing import Mapping, Sequence
from typing import Callable, Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
@@ -25,13 +25,24 @@ from scipy.spatial.transform import Rotation
@dataclass(frozen=True)
class UrdfJointPatch:
"""Authorized attribute replacements inside one top-level URDF joint."""
"""Attribute edits or explicit removal of one joint's linear mimic."""
origin_rpy: str | None = None
limit_lower: str | None = None
limit_upper: str | None = None
mimic_multiplier: str | None = None
mimic_offset: str | None = None
remove_mimic: bool = False
def __post_init__(self) -> None:
if type(self.remove_mimic) is not bool or (self.remove_mimic and (
self.mimic_multiplier is not None or self.mimic_offset is not None
)):
raise ValueError("mimic removal cannot also replace its coefficients")
def requested_fields(self) -> tuple[str, ...]:
return tuple(f"{element}.{attribute}" for element, attribute, _ in self.replacements()) + (
("mimic.remove",) if self.remove_mimic else ())
def replacements(self) -> tuple[tuple[str, str, str], ...]:
values = (
@@ -69,7 +80,7 @@ class UrdfPatchSet:
def __post_init__(self) -> None:
empty = [
name for name, patch in self.joints.items() if not patch.replacements()
name for name, patch in self.joints.items() if not patch.requested_fields()
]
if empty:
raise ValueError(
@@ -193,6 +204,12 @@ def apply_urdf_patch_text(
block = match.group(0)
for element, attribute, value in patch.replacements():
block = _replace_attribute(block, element, attribute, value)
if patch.remove_mimic:
if len(top_level_joints[name].findall("mimic")) != 1:
raise ValueError(f"mimic removal requires one original constraint:{name}")
block, count = re.subn(r"<mimic\b[^>]*(?:/>|>\s*</mimic\s*>)", "", block)
if count != 1:
raise ValueError(f"could not uniquely remove mimic constraint:{name}")
applied_joints.add(name)
return block
@@ -411,6 +428,7 @@ def write_urdf_patches(
authorized_fields: Mapping[str, Sequence[str]] | None = None,
validate_mimic_ranges: bool = True,
expected_source_sha256: str | None = None,
validate_output: Callable[[Path], None] | None = None,
) -> Path:
"""Validate and atomically materialize one patched URDF."""
source = Path(source_urdf).expanduser().resolve()
@@ -446,10 +464,7 @@ def write_urdf_patches(
from .validate import validate_structural_urdf_diff
requested_authorization = {
name: tuple(
f"{element}.{attribute}"
for element, attribute, _value in patch.replacements()
)
name: patch.requested_fields()
for name, patch in patches.joints.items()
}
authorization = (
@@ -481,6 +496,8 @@ def write_urdf_patches(
# when calibration does not make it worse.
if validate_mimic_ranges:
validate_urdf_mimic_ranges(temporary, reference_urdf=source)
if validate_output is not None:
validate_output(temporary)
materialize_relative_mesh_assets(
source=source, output=destination.parent, urdf_root=root
)
src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py
+63 -10
View File
@@ -13,6 +13,9 @@ from ..domain import CalibrationProfile
from ..fitting.coupling import LinearMimicFit
from .kinematics import UrdfKinematicModel, corrected_mimic_offset
from .patch import UrdfJointPatch, UrdfPatchSet, corrected_origin_rpy, write_urdf_patches
from .limits import MotionRangeEvidence, validate_range_evidence
from .joint_control import independent_export_authorization, AFFINE_MIMIC_APPROXIMATION, PRESERVE_SOURCE_MIMIC
from .source_mimic import MimicEnvelope, compile_source_mimic_envelopes
@dataclass(frozen=True)
@@ -21,7 +24,8 @@ class StandardUrdfPlan:
Limits are expressed in output coordinates. Mimic fits enter in CAD
coordinates and are transformed exactly once here, including chains whose
parent is a passive joint. Nothing is inferred from JSON runtime curves.
parent is a passive joint. Current measured exports preserve source mimic
parameters as a standalone fallback; their JSON curves remain independent.
"""
source_sha256: str
@@ -30,6 +34,10 @@ class StandardUrdfPlan:
mimic_output: Mapping[str, tuple[str, float, float]]
patches: UrdfPatchSet
authorized_fields: Mapping[str, Sequence[str]]
range_evidence: Mapping[str, MotionRangeEvidence] = field(default_factory=dict)
independent_joint_sources: Mapping[str, str] | None = None
mimic_policy: str | None = None
mimic_envelopes: Mapping[str, MimicEnvelope] = field(default_factory=dict)
def write(self, source_urdf: str | Path, destination_urdf: str | Path) -> Path:
source = Path(source_urdf)
@@ -50,22 +58,50 @@ def build_standard_correction_plan(
fitted_mimics_cad: Mapping[str, LinearMimicFit] | None = None,
transferred_zero_sources: Mapping[str, str] | None = None,
require_shared_mimic_zero: bool = False,
range_evidence: Mapping[str, MotionRangeEvidence] | None = None,
independent_joint_channels: Mapping[str, int] | None = None,
fitted_mimics_output: Mapping[str, LinearMimicFit] | None = None,
preserve_source_mimic: bool = False,
) -> StandardUrdfPlan:
"""Generate authorized standard-URDF edits, never clamp an invalid fit.
q_CAD = q_output + delta. Consequently both bounds shift by -delta and
b_output = b_CAD + a*delta_parent - delta_child. Unobserved joints retain
their own CAD geometry and physical range, including transferred joints.
An explicit training-supported motion envelope can replace nominal CAD
limits; separately declared hardware safety bounds remain mandatory.
With preserve_source_mimic, the original coefficients are kept verbatim
as an approximation in output coordinates, not rebased as a physical law.
"""
source = Path(source_urdf)
if hashlib.sha256(source.read_bytes()).hexdigest() != source_sha256:
raise ValueError("protected source URDF SHA256 mismatch")
model = UrdfKinematicModel(source)
if preserve_source_mimic and (fitted_mimics_cad is not None or fitted_mimics_output is not None
or require_shared_mimic_zero or independent_joint_channels is not None):
raise ValueError("preserved_source_mimic_cannot_be_fitted_rebased_or_removed")
approximations = dict(fitted_mimics_output or {})
if fitted_mimics_output is not None:
if fitted_mimics_cad or require_shared_mimic_zero or independent_joint_channels is not None:
raise ValueError("affine_mimic_export_requires_output_coordinate_approximations")
if set(approximations) != {name for name, joint in model.joints.items() if joint.mimic_joint}:
raise ValueError("affine_mimic_export_requires_all_source_couplings")
independent_sources = None
if independent_joint_channels is not None:
if fitted_mimics_cad or require_shared_mimic_zero:
raise ValueError("independent_joint_export_cannot_apply_linear_mimic_fits")
independent_sources = {name: joint.mimic_joint for name, joint in model.joints.items()
if joint.mimic_joint is not None}
authorized_fields = independent_export_authorization(
model, independent_joint_channels, independent_sources, authorized_fields)
nodes = {str(node.get("name")): node for node in ET.parse(source).getroot().findall("joint")}
offsets = {str(name): float(value) for name, value in zero_offsets_rad.items()}
ranges = dict(measured_ranges_output_rad or {})
mimics = dict(fitted_mimics_cad or {})
requested = set(offsets) | set(ranges) | set(mimics)
motion_ranges = dict(range_evidence or {})
if not motion_ranges.keys() <= ranges.keys():
raise ValueError("motion range evidence requires explicit output ranges")
requested = set(offsets) | set(ranges) | set(mimics) | set(approximations)
if not requested <= model.joints.keys():
raise ValueError("correction references a missing URDF joint")
for target, donor in (transferred_zero_sources or {}).items():
@@ -78,12 +114,17 @@ def build_standard_correction_plan(
for name, value in offsets.items():
if not math.isfinite(value) or model.joints[name].kind not in {"revolute", "continuous"}:
raise ValueError("zero corrections require finite revolute coordinates")
envelopes = {}
if preserve_source_mimic:
base_ranges = {name: tuple(ranges.get(name, (joint.lower-offsets.get(name, 0),
joint.upper-offsets.get(name, 0)))) for name, joint in model.joints.items() if joint.lower is not None}
envelopes = compile_source_mimic_envelopes(model, base_ranges, offsets, motion_ranges)
edits: dict[str, UrdfJointPatch] = {}
output_ranges: dict[str, tuple[float, float]] = {}
output_mimics: dict[str, tuple[str, float, float]] = {}
for name, joint in model.joints.items():
delta = offsets.get(name, 0.0)
fields: dict[str, str] = {}
fields: dict[str, str | bool] = {}
if delta != 0:
fields["origin_rpy"] = corrected_origin_rpy(nodes[name], delta)
if joint.lower is not None:
@@ -91,8 +132,14 @@ def build_standard_correction_plan(
interval = tuple(float(v) for v in ranges.get(name, mechanical))
if len(interval) != 2 or not all(math.isfinite(v) for v in interval) or interval[0] >= interval[1]:
raise ValueError(f"invalid corrected interval: {name}")
if interval[0] < mechanical[0] - 1e-9 or interval[1] > mechanical[1] + 1e-9:
if name in motion_ranges:
validate_range_evidence(name, joint, delta, interval, motion_ranges[name])
elif interval[0] < mechanical[0] - 1e-9 or interval[1] > mechanical[1] + 1e-9:
raise ValueError(f"corrected range exceeds source mechanical range: {name}")
if name in envelopes:
if interval != envelopes[name].base_limits_rad:
raise ValueError(f"source_mimic_base_range_differs_from_policy:{name}")
interval = envelopes[name].output_limits_rad
output_ranges[name] = interval
if interval[0] != joint.lower:
fields["limit_lower"] = f"{interval[0]:.15g}"
@@ -102,21 +149,24 @@ def build_standard_correction_plan(
raise ValueError(f"cannot bound an unbounded joint without changing its contract: {name}")
if name in mimics and joint.mimic_joint is None:
raise ValueError(f"cannot create a new mimic topology: {name}")
if joint.mimic_joint is not None:
fit = mimics.get(name)
if joint.mimic_joint is not None and independent_sources is not None:
fields["remove_mimic"] = True
elif joint.mimic_joint is not None:
fit = approximations.get(name, mimics.get(name))
if fit is not None:
if not isinstance(fit, LinearMimicFit) or fit.target_joint != name or fit.source_joint != joint.mimic_joint:
raise ValueError(f"mimic fit does not match source URDF topology: {name}")
if fit.training_cycles != (0, 1, 2) or not fit.residuals_rad or not all(math.isfinite(v) for v in fit.residuals_rad):
raise ValueError(f"mimic fit lacks training-only evidence: {name}")
if not fit.offset_observed and not math.isclose(fit.offset_rad, joint.mimic_offset, rel_tol=0, abs_tol=1e-12):
if name not in approximations and not fit.offset_observed and not math.isclose(fit.offset_rad, joint.mimic_offset, rel_tol=0, abs_tol=1e-12):
raise ValueError(f"unobservable passive static offset must retain CAD: {name}")
multiplier, offset = fit.multiplier, fit.offset_rad
else:
multiplier, offset = joint.mimic_multiplier, joint.mimic_offset
if not math.isfinite(multiplier) or not math.isfinite(offset):
raise ValueError(f"nonfinite mimic fit: {name}")
shifted = corrected_mimic_offset(multiplier, offset, offsets.get(joint.mimic_joint, 0), delta)
shifted = offset if preserve_source_mimic or name in approximations else corrected_mimic_offset(
multiplier, offset, offsets.get(joint.mimic_joint, 0), delta)
if require_shared_mimic_zero:
if fit is None or not fit.offset_observed or abs(shifted) > 1e-12:
raise ValueError(f"mimic lacks two supported baseline datums:{name}")
@@ -129,7 +179,7 @@ def build_standard_correction_plan(
if fields:
patch = UrdfJointPatch(**fields)
rights = set(authorized_fields.get(name, ()))
if any(f"{element}.{attribute}" not in rights for element, attribute, _ in patch.replacements()):
if not set(patch.requested_fields()) <= rights:
raise ValueError(f"correction requests a field absent from Profile authorization: {name}")
edits[name] = patch
# Reject inconsistent full mimic ranges before creating any artifacts.
@@ -155,7 +205,10 @@ def build_standard_correction_plan(
for name in output_mimics:
resolve(name)
return StandardUrdfPlan(source_sha256, offsets, output_ranges, output_mimics,
UrdfPatchSet(edits), {name: tuple(fields) for name, fields in authorized_fields.items()})
UrdfPatchSet(edits), {name: tuple(fields) for name, fields in authorized_fields.items()},
motion_ranges, independent_sources,
PRESERVE_SOURCE_MIMIC if preserve_source_mimic else (
AFFINE_MIMIC_APPROXIMATION if fitted_mimics_output is not None else None), envelopes)
@dataclass(frozen=True)
src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py
+8 -10
View File
@@ -71,19 +71,17 @@ def prepare_measured_result(profile, model, source_sha256, fit):
validate_mapping_provenance(profile, fit)
validate_zero_provenance(profile, fit)
passive = {name for name, joint in model.joints.items() if joint.mimic_joint}
if set(fit.mimic_approximations) != passive:
raise ValueError("independent_release_missing_paired_mimic_observations")
mimics = {name: evidence.in_cad_coordinates(fit.zero_offsets_rad)
for name, evidence in fit.mimic_approximations.items()}
# Keep legal mechanical limits for passive approximations; do not expand
# them to disguise an invalid linear reachable range.
# Measured support and the original linear fallback have separate envelopes.
# Only explicit nominal-limit policies permit expansion beyond CAD bounds.
ranges = {name: (min(fit.output_mappings[name].bounds_rad[0], fit.command_mappings[name].bounds_rad[0], 0),
max(fit.output_mappings[name].bounds_rad[1], fit.command_mappings[name].bounds_rad[1], 0))
for name in moving - passive}
from .limits import compile_motion_ranges
range_evidence = compile_motion_ranges(profile, model, fit)
ranges.update({name: evidence.output_limits_rad for name, evidence in range_evidence.items()})
plan = build_standard_correction_plan(source_urdf=model.source, source_sha256=source_sha256,
zero_offsets_rad=fit.zero_offsets_rad, measured_ranges_output_rad=ranges,
fitted_mimics_cad=mimics, authorized_fields=profile.urdf_authorized_fields, require_shared_mimic_zero=True)
for name, (_, _, offset) in plan.mimic_output.items():
if abs(offset) > 1e-12:
raise ValueError(f"mimic_does_not_pass_shared_zero:{name}")
authorized_fields=profile.urdf_authorized_fields,
preserve_source_mimic=True,
range_evidence=range_evidence)
return PreparedCalibration(plan, dict(fit.output_mappings))
src/linkerhand_calibration/linkerhand_calibration/core/urdf/source_mimic.py
+120
View File
@@ -0,0 +1,120 @@
"""Original CAD mimic fallback and its minimal representation envelopes.
The measured JSON curves own actual joint motion. A retained CAD mimic is a
standalone approximation, including its original offset in output coordinates.
Its envelope is recorded separately from training evidence, never as additional
measured travel. Declared hardware bounds and default hard CAD bounds still apply.
"""
from dataclasses import asdict, dataclass
import math
import xml.etree.ElementTree as ET
@dataclass(frozen=True)
class MimicEnvelope:
base_limits_rad: tuple[float, float]
mimic_reachable_limits_rad: tuple[float, float]
output_limits_rad: tuple[float, float]
def _resolve_envelopes(model, ranges, bases):
reachable, envelopes, visiting = {}, {}, set()
def resolve(name):
if name in reachable:
return reachable[name]
if name in visiting:
raise ValueError(f"source_mimic_cycle:{name}")
if name not in model.joints:
raise ValueError(f"source_mimic_parent_missing:{name}")
visiting.add(name)
joint = model.joints[name]
if joint.mimic_joint is None:
if name not in ranges:
raise ValueError(f"source_mimic_requires_bounded_parent:{name}")
interval = ranges[name]
if (len(interval) != 2 or any(v is None or not math.isfinite(v) for v in interval)
or interval[0] >= interval[1]):
raise ValueError(f"invalid_source_mimic_parent_range:{name}")
else:
if not all(math.isfinite(v) for v in (joint.mimic_multiplier, joint.mimic_offset)):
raise ValueError(f"invalid_source_mimic_coefficients:{name}")
parent = resolve(joint.mimic_joint)
endpoints = [joint.mimic_multiplier*v+joint.mimic_offset for v in parent]
interval = (min(endpoints), max(endpoints))
base = bases[name]
if len(base) != 2 or not all(math.isfinite(v) for v in base) or base[0] >= base[1]:
raise ValueError(f"invalid_source_mimic_base_range:{name}")
output = (min(base[0], interval[0]), max(base[1], interval[1]))
envelopes[name] = MimicEnvelope(base, interval, output)
visiting.remove(name)
reachable[name] = interval
return interval
for name in bases:
resolve(name)
return envelopes
def compile_source_mimic_envelopes(model, ranges, offsets, motion_ranges):
"""Compute from source coefficients and trusted ranges; never refit or pad."""
bases = {}
for name, joint in model.joints.items():
if joint.mimic_joint is None:
continue
if joint.lower is None:
raise ValueError(f"source_mimic_requires_bounded_child:{name}")
delta = offsets.get(name, 0.0)
bases[name] = (motion_ranges[name].output_limits_rad if name in motion_ranges else
(joint.lower-delta, joint.upper-delta))
envelopes = _resolve_envelopes(model, ranges, bases)
for name, envelope in envelopes.items():
evidence = motion_ranges.get(name)
delta = offsets.get(name, 0.0)
safety = (evidence.independent_safety_limits_cad_rad if evidence is not None else
(model.joints[name].lower, model.joints[name].upper))
if safety is not None:
lo, hi = envelope.output_limits_rad
if lo+delta < safety[0]-1e-9 or hi+delta > safety[1]+1e-9:
raise ValueError(f"source_mimic_exceeds_safety_range:{name}:"
f"required_cad_rad=({lo+delta:.9g},{hi+delta:.9g}):"
f"allowed_cad_rad=({safety[0]:.9g},{safety[1]:.9g})")
return envelopes
def validate_source_mimic_elements(source, output):
"""Even omitted coefficient attributes must stay omitted in new exports."""
def elements(path):
return {node.get("name"): [ET.tostring(item, encoding="unicode").strip()
for item in node.findall("mimic")]
for node in ET.parse(path).getroot().findall("joint")}
if elements(source) != elements(output):
raise ValueError("exported_mimic_differs_from_source_urdf")
def validate_serialized_mimic_envelopes(payload, model):
"""Check declared minimal envelopes against the actual exported model."""
declared = payload["urdf_correction"].get("mimic_envelopes")
passive = {name for name, joint in model.joints.items() if joint.mimic_joint is not None}
if not isinstance(declared, dict) or set(declared) != passive:
raise ValueError("source_mimic_envelope_coverage_differs")
keys = {"base_limits_rad", "mimic_reachable_limits_rad", "output_limits_rad"}
for name, row in declared.items():
if not isinstance(row, dict) or set(row) != keys:
raise ValueError(f"invalid_source_mimic_envelope:{name}")
for key, values in row.items():
if (not isinstance(values, (list, tuple)) or len(values) != 2
or any(type(v) not in (float, int) or not math.isfinite(v) for v in values)
or values[0] > values[1]
or (key != "mimic_reachable_limits_rad" and values[0] == values[1])):
raise ValueError(f"invalid_source_mimic_envelope:{name}:{key}")
ranges = {name: (j.lower, j.upper) for name, j in model.joints.items() if j.lower is not None}
expected = _resolve_envelopes(model, ranges,
{name: tuple(row["base_limits_rad"]) for name, row in declared.items()})
for name, envelope in expected.items():
for key, values in asdict(envelope).items():
if any(abs(a-b) > 1e-10 for a, b in zip(values, declared[name][key])):
raise ValueError(f"source_mimic_envelope_differs:{name}:{key}")
if any(abs(a-b) > 1e-10 for a, b in zip(envelope.output_limits_rad, ranges[name])):
raise ValueError(f"source_mimic_envelope_differs_from_urdf:{name}")
src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py
+15 -5
View File
@@ -1,7 +1,8 @@
"""Replay raw fourth-cycle Tag poses with the *written* URDF and JSON.
Mounting transforms and base registration are frozen from training. Passive
poses are resolved only through the exported standard URDF mimic graph.
Mounting transforms and base registration are frozen from training. Measured
v3 motion uses the complete JSON mapping, including nonlinear passive joints;
standard URDF mimic is evaluated separately as an approximation.
"""
from __future__ import annotations
@@ -16,6 +17,10 @@ from scipy.spatial.transform import Rotation
from ..fitting.tag_installation import FrozenTagInstallation, rigid_matrix
from .acceptance import SerializedJointMapping, angular_metrics
from .kinematics import UrdfKinematicModel
from .joint_control import independent_joint_sources
POSITION_P95_LIMIT_M = 0.003
@dataclass(frozen=True)
@@ -36,7 +41,10 @@ def validate_serialized_tag_holdout(
input_kind: str = "feedback",
mimic_approximation: bool = False,
enforce_precision: bool = True,
holdout_cycle: int = 3,
) -> dict[str, dict[str, Any]]:
if holdout_cycle not in {3, 5}:
raise ValueError("Tag validation requires a declared independent partition")
model = UrdfKinematicModel(corrected_urdf)
mapping = SerializedJointMapping(payload, input_kind=input_kind)
base = rigid_matrix(common_from_base)
@@ -45,14 +53,16 @@ def validate_serialized_tag_holdout(
raise ValueError("final Tag holdout requires declared frozen installations")
active = tuple(name for name, joint in model.joints.items()
if joint.kind != "fixed" and joint.mimic_joint is None)
independent = mapping.measured and not mimic_approximation
# Full measured motion must use the JSON angles rather than the approximate
# mimic. Correction version 2 is the historical mimic-free export.
independent = mapping.measured and not mimic_approximation and independent_joint_sources(payload) is None
if independent:
active = tuple(name for name, joint in model.joints.items() if joint.kind != "fixed")
training_ids = {sample for mount in installations.values() for sample in mount.training_sample_ids}
seen = set()
errors: dict[str, list[tuple[float, float]]] = {role: [] for role in roles}
for row in observations:
if row.cycle != 3 or not row.sample_id or row.sample_id in training_ids:
if row.cycle != holdout_cycle or not row.sample_id or row.sample_id in training_ids:
raise ValueError("final Tag holdout overlaps training or has no image identity")
if row.role not in roles:
raise ValueError("final Tag holdout contains an undeclared Tag")
@@ -83,7 +93,7 @@ def validate_serialized_tag_holdout(
raise ValueError(f"missing independent Tag holdout samples:{role}:{len(values)}")
metrics = angular_metrics([value[0] for value in values])
translation_p95 = float(np.percentile([value[1] for value in values], 95))
if enforce_precision and (not metrics.passed or translation_p95 > 0.003):
if enforce_precision and (not metrics.passed or translation_p95 > POSITION_P95_LIMIT_M):
raise ValueError(f"serialized_urdf_tag_holdout_failed:{role}:mae_deg={metrics.mae_deg}:p95_deg={metrics.p95_deg}:max_deg={metrics.maximum_deg}:translation_p95_m={translation_p95}")
results[role] = {"count": metrics.count, "mae_deg": metrics.mae_deg,
"p95_deg": metrics.p95_deg, "maximum_deg": metrics.maximum_deg,
src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py
+6 -1
View File
@@ -47,7 +47,7 @@ def validate_structural_urdf_diff(
field
for fields in allowed.values()
for field in fields
if field not in URDF_FIELD_TO_XML
if field not in URDF_FIELD_TO_XML and field != "mimic.remove"
}
if unsupported:
raise ValueError(
@@ -118,6 +118,11 @@ def validate_structural_urdf_diff(
changes.append(f"{current_joint}.{field}")
left_children = list(left)
right_children = list(right)
if (left.tag == "joint" and path.count("/") == 2
and "mimic.remove" in allowed.get(current_joint, ())
and len(left.findall("mimic")) == 1 and not right.findall("mimic")):
left_children = [child for child in left_children if child.tag != "mimic"]
changes.append(f"{current_joint}.mimic.remove")
if len(left_children) != len(right_children):
raise ValueError(f"URDF topology changed at {path}")
for index, (left_child, right_child) in enumerate(
src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py
+19 -1
View File
@@ -231,6 +231,7 @@ class HikrobotCameraNode(Node):
self._closed = False
self._frame_count = 0
self._timeout_count = 0
self._camera_timing = None
self.selector = str(self.get_parameter("serial_number").value)
self.expected_model = str(self.get_parameter("expected_model").value)
@@ -286,6 +287,7 @@ class HikrobotCameraNode(Node):
self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
self.declare_parameter("grab_timeout_ms", 1000)
self.declare_parameter("timestamp_journal_path", "")
def _load_calibration(self) -> CameraCalibration | None:
if self.camera_info_path is None or not self.camera_info_path.exists():
@@ -407,6 +409,13 @@ class HikrobotCameraNode(Node):
"set latest-frame grab strategy",
camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
)
from .camera_timing import MvsCameraTiming
self._camera_timing = MvsCameraTiming(camera, mvs,
lambda: int(self.get_clock().now().nanoseconds),
exposure_us=0. if bool(self.get_parameter("auto_exposure").value)
else float(self.get_parameter("exposure_time_us").value),
journal_path=str(self.get_parameter("timestamp_journal_path").value))
self._camera_timing.start()
self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
self._grabbing = True
self.get_logger().info(
@@ -464,6 +473,11 @@ class HikrobotCameraNode(Node):
assert self._mvs is not None
timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
while not self._stop_event.is_set():
try:
self._camera_timing.refresh_if_due()
except ValueError as error:
self.get_logger().error(f"Camera timestamp source failed: {error}")
return
frame = self._mvs.MV_FRAME_OUT()
memset(byref(frame), 0, sizeof(frame))
result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
@@ -506,7 +520,9 @@ class HikrobotCameraNode(Node):
raise RuntimeError(
f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
)
stamp = self.get_clock().now().to_msg()
from rclpy.time import Time
received_ns = int(self.get_clock().now().nanoseconds)
stamp = Time(nanoseconds=self._camera_timing.timestamp(info, received_ns)).to_msg()
image = Image()
image.header.stamp = stamp
image.header.frame_id = self.frame_id
@@ -596,6 +612,8 @@ class HikrobotCameraNode(Node):
self._stop_event.set()
if self._grab_thread is not None:
self._grab_thread.join(timeout=2.0)
if self._camera_timing is not None:
self._camera_timing.close()
if self._camera is not None and self._camera_opened:
if self._grabbing:
result = self._camera.MV_CC_StopGrabbing()
src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py
+31
View File
@@ -0,0 +1,31 @@
"""Temporary diagnostic task selections, declared beside hand Profiles."""
from dataclasses import dataclass
from ..core.domain.profile import ProfileKey
@dataclass(frozen=True)
class DiagnosticCapturePreset:
profile_key: ProfileKey
tasks: tuple[tuple[str, tuple[str, ...]], ...]
label_zh: str = "诊断采集"
hold_command_values: tuple[float, ...] = ()
hold_seconds: float = 0.0
DIAGNOSTIC_CAPTURE_PRESETS = {
"o6_thumb": DiagnosticCapturePreset(
ProfileKey("O6", "right", "o6_right_8", 1),
(("thumb_yaw_top", ("rh_thumb_cmc_yaw",)),
("thumb_pitch_ip_front", ("rh_thumb_cmc_pitch", "rh_thumb_ip"))),
label_zh="拇指短程诊断",
),
"o6_yaw_hold": DiagnosticCapturePreset(
ProfileKey("O6", "right", "o6_right_8", 1),
(("thumb_yaw_top", ("rh_thumb_cmc_yaw",)),),
label_zh="拇指yaw方向差保持诊断",
hold_command_values=(64.0, 128.0, 192.0),
hold_seconds=10.0,
),
}
src/linkerhand_calibration/linkerhand_calibration/profiles/export.py
+3
View File
@@ -51,6 +51,9 @@ def hand_profile_payload(profile: CalibrationProfile) -> dict[str, Any]:
"acquisition": _plain(asdict(profile.acquisition)),
"urdf": {
"authorized_fields": _plain(profile.urdf_authorized_fields),
**({"limit_policies": {name: _plain(asdict(policy))
for name, policy in profile.urdf_limit_policies.items()}}
if profile.urdf_limit_policies else {}),
},
"joint_coverage": _plain(profile.joint_coverage),
}
src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py
+11 -2
View File
@@ -8,7 +8,7 @@ import re
from typing import Any, Mapping
import yaml
from ..core.domain.profile import ReferenceWaypoint, JointZeroSpec, KnownBaselineGeometry
from ..core.domain.profile import ReferenceWaypoint, JointZeroSpec, KnownBaselineGeometry, JointLimitPolicy
from ..core import (
AcquisitionPolicy,
@@ -56,7 +56,9 @@ def _validate_keys(payload):
("quality", QualityPolicy), ("scope", ScopePolicy), ("artifacts", ArtifactPolicy),
("acquisition", AcquisitionPolicy)):
_keys(payload.get(name, {}), {f.name for f in fields(contract)}, name)
_keys(payload.get("urdf", {}), {"authorized_fields"}, "urdf")
_keys(payload.get("urdf", {}), {"authorized_fields", "limit_policies"}, "urdf")
for policy in payload.get("urdf", {}).get("limit_policies", {}).values():
_keys(policy, {f.name for f in fields(JointLimitPolicy)}, "URDF limit policy")
for view in payload.get("vision", {}).get("views", ()):
_keys(view, {"name", "tags"}, "view")
for tag in view.get("tags", ()):
@@ -318,6 +320,7 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile:
str(artifacts.get("publication_pointer", "latest_passed")),
_set(artifacts.get("session_compatibility_tokens")),
bool(artifacts.get("publish_corrected_urdf", False)),
_set(artifacts.get("directional_command_joints")),
),
acquisition=AcquisitionPolicy(**dict(acquisition)),
sdk_adapter=str(payload["sdk_adapter"]),
@@ -333,6 +336,12 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile:
payload.get("joint_coverage", {}), "joint_coverage"
).items()
},
urdf_limit_policies={str(name): JointLimitPolicy(
source_limits=str(row["source_limits"]), evidence_source=str(row["evidence_source"]),
independent_safety_limits_cad_rad=(None if row.get("independent_safety_limits_cad_rad") is None
else tuple(float(v) for v in row["independent_safety_limits_cad_rad"])),
safety_evidence_source=str(row.get("safety_evidence_source", "")))
for name, row in _map(urdf.get("limit_policies", {}), "urdf.limit_policies").items()},
)
validate_profile(profile)
return profile
src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py
+35
View File
@@ -42,3 +42,38 @@ def compile_tag_links(profile: CalibrationProfile, model: UrdfKinematicModel) ->
raise ValueError(f"Tag is assigned to different kinematic branches:{tag.role}")
bind(tag.role, model.joints[descendants[0]].child)
return result
def compile_parallel_axis_geometry(profile, model, relations):
"""Read invariant geometry only when both Tags identify adjacent links."""
import numpy as np
from ..core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry
links = compile_tag_links(profile, model)
by_child_role = {relation.child_role: relation for relation in relations}
result = []
for child in relations:
parent = by_child_role.get(child.parent_role)
if parent is None:
continue
parent_name = profile.command.urdf_joint_by_joint.get(parent.joint, parent.joint)
child_name = profile.command.urdf_joint_by_joint.get(child.joint, child.joint)
first, second = model.joints[parent_name], model.joints[child_name]
if (first.kind not in {"revolute", "continuous"} or second.kind not in {"revolute", "continuous"}
or links[parent.child_role] != first.child or links[child.child_role] != second.child):
continue
chain = model._chain(child_name)
positions = [i for i, joint in enumerate(chain) if joint.name == parent_name]
if not positions or any(joint.kind != "fixed" for joint in chain[positions[0]+1:-1]):
continue
a, b = (model.link_transform(name, zero_offsets={}, joint_angles={}, independent_mimic_angles=True)
for name in (parent_name, child_name))
axis_a, axis_b = a[:3, :3] @ first.axis, b[:3, :3] @ second.axis
if np.linalg.norm(np.cross(axis_a, axis_b)) > 1e-8:
continue
displacement = b[:3, 3] - a[:3, 3]
distance = float(np.linalg.norm(displacement - axis_a * (axis_a @ displacement)))
if distance > 1e-6:
result.append(ParallelAxisGeometry(parent.joint, child.joint,
1 if axis_a @ axis_b > 0 else -1, distance))
return tuple(result)
src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py
+17 -2
View File
@@ -6,7 +6,18 @@ from ..core.urdf.kinematics import UrdfKinematicModel
from .observations import compile_tag_links
def require_release_geometry(report):
"""Reject structurally impossible releases before acquiring any images."""
coverage = report["independent_measurement_coverage"]
missing_motion = coverage["unresolved_motion_joints"]
missing_zero = coverage["missing_baseline_geometry"]
if missing_motion or missing_zero:
raise ValueError(f"release_geometry_incomplete:motion={missing_motion}:baseline={missing_zero}")
def validate_executable_profile(profile, source_urdf):
if profile.artifacts.output_schema_version not in {2, 3}:
raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only")
validate_profile(profile)
if not profile.command.sdk_to_joint_direction:
raise ValueError("executable Profile must declare fixed SDK-to-joint directions")
@@ -26,8 +37,6 @@ def validate_executable_profile(profile, source_urdf):
raise ValueError(f"unsupported measurement primitive:{name}:{spec.kind}")
if not set(profile.zero.axis_joints) <= measured:
raise ValueError("spatial observer axes require declared measured tasks")
if profile.artifacts.output_schema_version not in {2, 3}:
raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only")
if profile.artifacts.output_schema_version == 3:
required = {name for task in profile.motion.tasks for name in task.joints}
if required != set(profile.motion.joint_zero_references):
@@ -39,6 +48,9 @@ def validate_executable_profile(profile, source_urdf):
_validate_transfer_geometry(profile, model)
if not set(profile.urdf_authorized_fields) <= model.joints.keys():
raise ValueError("URDF authorization references a missing joint")
for name in profile.urdf_limit_policies:
if name not in model.joints or model.joints[name].lower is None:
raise ValueError(f"calibrated motion envelope requires an existing bounded CAD joint:{name}")
conflicts = []
for name, joint in model.joints.items():
if joint.mimic_joint is None or joint.lower is None:
@@ -71,6 +83,9 @@ def validate_executable_profile(profile, source_urdf):
"cad_zero_assumptions": dict(profile.zero.cad_zero_assumptions),
"transferred": dict(profile.zero.transferred_zero_sources),
"cad_constraint_conflicts": conflicts,
"calibrated_motion_envelope_joints": sorted(profile.urdf_limit_policies),
"independent_angular_safety_limits_declared": sorted(name for name, policy
in profile.urdf_limit_policies.items() if policy.independent_safety_limits_cad_rad is not None),
"observability": "structural_checks_only; numerical_rank_and_holdout_required",
"arbitrary_multiaxis_validated": False}
src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py
+20 -19
View File
@@ -14,6 +14,23 @@ from typing import Protocol, Sequence, runtime_checkable
from ...core import CommandLayout
def ordered_feedback_values(layout, names, values):
"""Apply the same channel order to positions and their measurement times."""
if len(values) != layout.command_count:
return None
if not names or layout.feedback_by_index:
return tuple(values)
if len(names) != len(values) or len(set(names)) != len(names):
return None
by_name = dict(zip((str(name) for name in names), values))
for alias, canonical in layout.feedback_name_aliases.items():
if alias in by_name and canonical not in by_name:
by_name[canonical] = by_name[alias]
if any(name not in by_name for name in layout.names):
return None
return tuple(by_name[name] for name in layout.names)
@dataclass(frozen=True)
class HardwareHealth:
connected: bool
@@ -77,27 +94,11 @@ class ProfileSdkAdapter:
def parse_feedback(
self, names: Sequence[str], values: Sequence[float]
) -> tuple[float, ...] | None:
layout = self.command_layout
if len(values) != layout.command_count:
return None
if names and not layout.feedback_by_index and (
len(names) != len(values) or len(set(names)) != len(names)
):
return None
if names and not layout.feedback_by_index:
by_name = dict(zip((str(name) for name in names), values))
for alias, canonical in layout.feedback_name_aliases.items():
if alias in by_name and canonical not in by_name:
by_name[canonical] = by_name[alias]
if any(name not in by_name for name in layout.names):
ordered = ordered_feedback_values(self.command_layout, names, values)
if ordered is None:
return None
try:
parsed = tuple(float(by_name[name]) for name in layout.names)
except (ValueError, TypeError, OverflowError):
return None
else:
try:
parsed = tuple(float(value) for value in values)
parsed = tuple(float(value) for value in ordered)
except (ValueError, TypeError, OverflowError):
return None
return parsed if all(math.isfinite(value) for value in parsed) else None
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py
+18 -2
View File
@@ -25,6 +25,10 @@ class FinalizationInputs:
source_urdf: Path
protected_inputs: Mapping[str, str]
records: tuple[dict[str, Any], ...]
require_motion_evidence: bool = False
journal_path: Path | None = None
journal_size: int | None = None
camera_extrinsics_file: Path | None = None
class FinalizationProtocolError(RuntimeError):
@@ -32,17 +36,29 @@ class FinalizationProtocolError(RuntimeError):
class FinalizationController:
def __init__(self, profile, session_dir: Path, *, finalizer=finalize_profile_session):
def __init__(self, profile, session_dir: Path, *, finalizer=finalize_profile_session, isolated=False):
self.worker = FinalizationWorker()
self._events: SimpleQueue[str] = SimpleQueue()
self._isolated = isolated
if isolated:
from .process_finalizer import isolated_finalize_profile_session
finalizer = partial(isolated_finalize_profile_session, finalizer=finalizer)
self._finalizer = partial(finalizer, profile=profile, phase_changed=self._events.put)
self._publisher = ArtifactPublisher(session_dir.parent, profile.artifacts.publication_pointer)
self.started = False
def start(self, inputs: FinalizationInputs) -> None:
transport = {}
if inputs.camera_extrinsics_file is not None:
transport["camera_extrinsics_file"] = inputs.camera_extrinsics_file
if self._isolated:
if inputs.journal_path is None or inputs.journal_size is None:
raise ValueError("isolated_finalization_requires_synced_journal")
transport.update(journal_path=inputs.journal_path, journal_size=inputs.journal_size)
self.worker.start(self._finalizer, session_dir=inputs.session_dir,
serial_number=inputs.serial_number, source_urdf=inputs.source_urdf,
protected_inputs=dict(inputs.protected_inputs), records=inputs.records)
protected_inputs=dict(inputs.protected_inputs), records=inputs.records,
require_motion_evidence=inputs.require_motion_evidence, **transport)
self.started = True
def drain_events(self, on_event: Callable[[str], None]) -> None:
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py
+38
View File
@@ -0,0 +1,38 @@
"""Direction state for tables measured on complete endpoint-to-endpoint sweeps."""
from math import floor
def next_input_directions(values, previous, directions, *, resolution, channels, byte_input=False):
"""Pure transition: callers commit it only after every mapping check passes.
A hold keeps its arrival direction. Initial state requires the recorded
baseline; an interior reversal needs unmeasured minor-loop hysteresis and
is rejected. An endpoint reset is the scope actually covered by calibration.
"""
result = list(directions)
for index, (baseline, lower, upper) in channels.items():
if previous is None:
if values[index] != baseline:
raise ValueError(f"directional_mapping_requires_baseline_initialization:channel={index}")
if baseline not in (lower, upper):
raise ValueError("directional_mapping_requires_endpoint_baseline")
result[index] = "increasing" if baseline == upper else "decreasing"
if previous is not None:
for index, (old, new) in enumerate(zip(previous, values)):
tolerance = resolution
if byte_input and index in channels:
# The serialized byte table uses floor(v + .5). Compare the
# same effective inputs so small float steps cannot accumulate
# into a changed table index with a stale travel direction.
old, new = floor(old+.5), floor(new+.5)
tolerance = 0
if abs(new-old) <= tolerance:
continue
direction = "increasing" if new > old else "decreasing"
if index in channels and direction != result[index]:
_, lower, upper = channels[index]
if min(abs(old-lower), abs(old-upper)) > tolerance:
raise ValueError(f"directional_mapping_interior_reversal_unvalidated:channel={index}")
result[index] = direction
return result
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py
+35 -14
View File
@@ -32,13 +32,42 @@ def _pose(value):
return matrix_tuple(value)
def _replay_directions(mappings, row, task, *, directions_are_task_relative=False):
"""A held ancestor retains its arrival branch in both replay paths."""
direction = row["direction"]
if directions_are_task_relative and task.end_value > task.start_value:
direction = {"increasing": "decreasing", "decreasing": "increasing"}[direction]
if direction not in {"increasing", "decreasing"}:
raise ValueError("Tag replay requires an explicit scan direction")
# Older CaptureFrame records encode unavailable history as an empty
# vector. Preserve that explicit unknown state; a nonempty partial vector
# is damaged evidence and must not silently change only some branches.
history = row.get("command_direction_by_index")
if history is not None and not isinstance(history, (list, tuple)):
raise ValueError("Tag replay has an invalid SDK direction history")
if history:
covered = len(history) > max(m.motor_index for m in mappings.values())
valid = all(value in {"", "increasing", "decreasing"} for value in history)
if not covered or not valid:
raise ValueError("Tag replay has an invalid SDK direction history")
directions = {}
for joint, mapping in mappings.items():
if mapping.motor_index == task.command_index:
directions[joint] = direction
else:
directions[joint] = history[mapping.motor_index] if history else ""
return directions
def prepare_command_holdout(profile, mappings, records):
"""Raw steady fourth-cycle poses; no registration or mounting fit here."""
"""Raw independent command-validation poses; no registration or fit here."""
from ...core.domain.sampling import command_holdout_cycle
holdout_cycle = command_holdout_cycle(profile)
specs = profile.measurement.measurements
tasks = {task.key: task for task in profile.motion.tasks}
seen, result = set(), []
for row in records:
if row["cycle"] != 3:
if row["cycle"] != holdout_cycle:
continue
if row.get("sample_phase") != "steady":
raise ValueError("command holdout requires stable command observations")
@@ -48,13 +77,8 @@ def prepare_command_holdout(profile, mappings, records):
if identity in seen:
continue
seen.add(identity)
directions = {joint: row["direction"] for joint, m in mappings.items()
if m.motor_index == task.command_index}
for joint, mapping in mappings.items():
previous = row.get("command_direction_by_index", ())
if mapping.motor_index != task.command_index and len(previous) > mapping.motor_index:
directions[joint] = previous[mapping.motor_index]
result.append(TagHoldout(row["sample_id"], spec.child_role, 3,
directions = _replay_directions(mappings, row, task)
result.append(TagHoldout(row["sample_id"], spec.child_role, holdout_cycle,
tuple(row[f"command_vector_{profile.command.unit}"]), directions, _pose(row["child_pose_common"])))
return tuple(result)
@@ -105,11 +129,8 @@ def prepare_tag_replay(
input_domain = next(iter(domain))
field = f"state_{profile.command.unit}" if input_domain.startswith("feedback_") else f"command_vector_{profile.command.unit}"
sdk = tuple(float(value) for value in row[field])
direction = str(row["direction"])
if directions_are_task_relative and task.end_value > task.start_value:
direction = {"increasing": "decreasing", "decreasing": "increasing"}[direction]
directions = {joint: direction for joint, mapping in output_mappings.items()
if mapping.motor_index == task.command_index}
directions = _replay_directions(output_mappings, row, task,
directions_are_task_relative=directions_are_task_relative)
pose = _pose(row["child_pose_common"])
cycle = int(row["cycle"])
key = (role, identity)
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py
+101 -20
View File
@@ -10,11 +10,13 @@ from ...core.artifacts.storage import atomic_write_json
from ...core.fitting.session import fit_profile_calibration
from ...core.fitting.command_mapping import fit_command_mappings
from ...core.fitting.motion_fit import channel_for_joint
from ...core.fitting.rotation_diagnostics import RotationHoldoutFailure
from ...core.geometry.candidate_selection import resolve_chain_observations
from ...core.geometry.extrinsics import transform_matrix
from ...core.urdf.kinematics import UrdfKinematicModel
from ...core.urdf.result_plan import prepare_standard_result
from ..acquisition import accepted_joint_records, accepted_secondary_records
from ..diagnostic_capture import reject_diagnostic_capture
from .evidence import prepare_tag_replay
from .publisher import ArtifactPublisher, FrozenTagArtifactValidator, StagedRelease
from .serializers.compact_v2 import from_report, REPORT_FILENAME
@@ -28,47 +30,96 @@ class CorrectedArtifact:
staged_release: StagedRelease
def _write_artifact_pair(profile, fit, prepared, directory, source, serial_number, protected_inputs):
"""Save the calibration first, then rebuild its URDF from those bytes.
These are reviewable candidate files. Only independent final-file
acceptance and the publication controller can create a valid release.
"""
from .urdf_from_json import serialize_urdf_correction, rebuild_urdf_from_json
report_writer, command_writer = serialize_report, from_report
if profile.artifacts.output_schema_version >= 3:
from .serializers.measured_v3 import serialize_report as report_writer, from_report as command_writer
report = report_writer(profile, fit, prepared, serial_number=serial_number, protected_inputs=protected_inputs)
report["urdf_correction"] = serialize_urdf_correction(prepared.plan)
if prepared.plan.range_evidence:
report["motion_range_evidence"] = {name: asdict(item) for name, item in prepared.plan.range_evidence.items()}
payload = command_writer(report)
naming = {"serial_number": serial_number, "side": profile.key.side, "model": profile.key.model.lower()}
json_path = directory / profile.artifacts.calibration_filename.format(**naming)
urdf_path = directory / profile.artifacts.corrected_urdf_filename.format(**naming)
atomic_write_json(json_path, payload)
atomic_write_json(directory / REPORT_FILENAME, report)
rebuild_urdf_from_json(json_path, source, urdf_path,
authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id)
return payload, json_path, urdf_path
def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf,
protected_inputs, records, publish=True, timestamp=None,
standard_loader: Callable = validate_with_robot_state_publisher,
cancelled: Callable[[], bool] = lambda: False,
directions_are_task_relative: bool = False,
require_motion_evidence: bool = False,
camera_extrinsics_file=None,
phase_changed: Callable[[str], None] = lambda _phase: None):
"""No worker may publish after an operator abort.
The runtime owns cancellation synchronization. This function also checks
immediately before publication and never returns the hand to a pose.
"""
reject_diagnostic_capture(records)
del timestamp
if profile.artifacts.output_schema_version not in {2, 3}:
raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only")
if profile.artifacts.output_schema_version >= 3:
from ...profiles.validator import validate_executable_profile, require_release_geometry
require_release_geometry(validate_executable_profile(profile, source_urdf))
directory, source = Path(session_dir).resolve(), Path(source_urdf).resolve()
directory.mkdir(parents=True, exist_ok=True)
def check_cancelled():
if cancelled():
raise ValueError("operator_abort:no_artifact_publication")
check_cancelled()
from ...core.fitting.command_sampling import validate_sampling_plans
sampling_plans = validate_sampling_plans(profile, records)
if sampling_plans:
atomic_write_json(directory / "command_sampling_plans.json", sampling_plans)
from ..motion_provenance import validate_source_geometry
validate_source_geometry(profile, source, records)
if any(row.get("projection_reprocessing_scope") for row in records):
raise ValueError("partial external projection is diagnostic-only; cannot certify a whole session")
pose_reports = {}
for task in profile.motion.tasks:
if task.key not in profile.measurement.candidate_selection_tasks:
continue
try:
records, report = resolve_chain_observations(records, task_key=task.key,
require_frozen_branches=True,
fixed_roles={tag.role for view in profile.vision.views for tag in view.tags if tag.fixed_reference},
role_pairs={name: (profile.measurement.measurements[name].parent_role,
profile.measurement.measurements[name].child_role) for name in task.joints})
except ValueError as error:
pose_reports[task.key] = {'status': 'failed', 'reason': str(error), 'publication_allowed': False}
atomic_write_json(directory / "pose_selection_diagnostics.json", pose_reports)
raise
pose_reports[task.key] = report
atomic_write_json(directory / "pose_selection_diagnostics.json", pose_reports)
if report["status"] == "legacy_projection_unverified":
raise ValueError("camera_projection_unverified:no_artifact_publication")
from ..motion_provenance import validate_motion_provenance
try:
motion_provenance = validate_motion_provenance(profile, records,
required=require_motion_evidence, require_complete=True)
except ValueError as error:
atomic_write_json(directory / "motion_branch_provenance.json", {
"status": "failed", "reason": str(error), "publication_allowed": False})
raise
atomic_write_json(directory / "motion_branch_provenance.json", motion_provenance)
accepted = accepted_joint_records(profile, records, directions_are_task_relative=directions_are_task_relative)
secondary = accepted_secondary_records(profile, records, directions_are_task_relative=directions_are_task_relative)
try:
from ...core.domain.reference import read_joint_zero_references
if profile.artifacts.output_schema_version >= 3:
from ...profiles.validator import validate_executable_profile
validate_executable_profile(profile, source)
references = read_joint_zero_references(profile, records) if profile.artifacts.output_schema_version >= 3 else None
def save_relative_motion(motion):
if profile.artifacts.output_schema_version >= 3:
@@ -85,8 +136,11 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf
from ...core.fitting.motion_transfer import expand_profile_transfers
fit = expand_profile_transfers(profile, fit)
except ValueError as error:
atomic_write_json(directory / "fit_diagnostics.json", {
"passed": False, "reason": str(error), "publication_allowed": False})
diagnostic = {"passed": False, "reason": str(error), "publication_allowed": False}
if isinstance(error, RotationHoldoutFailure):
diagnostic.update(stage="relative_rotation_holdout",
rotation_holdout=asdict(error.diagnostics))
atomic_write_json(directory / "fit_diagnostics.json", diagnostic)
raise
check_cancelled()
phase_changed("fit_complete")
@@ -96,53 +150,80 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf
atomic_write_json(directory / "fit_diagnostics.json", {"passed": False, "reason": str(error),
"publication_allowed": False, "relative_motion": {name: asdict(curve) for name, curve in fit.curves.items()}})
raise
check_cancelled()
phase_changed("holdout_complete")
payload, json_path, urdf_path = _write_artifact_pair(
profile, fit, prepared, directory, source, serial_number, protected_inputs)
phase_changed("artifacts_built")
zero = fit.spatial_zero
replay_records = [row for rows in accepted.values() for row in rows]
replay_records.extend(dict(row, joint=profile.measurement.cross_view_sources[name])
for name, rows in secondary.items() for row in rows)
from ...core.fitting.tag_installation import TagInstallationFailure
try:
replay = prepare_tag_replay(profile=profile, source_model=UrdfKinematicModel(source),
plan=prepared.plan, output_mappings=fit.output_mappings,
common_from_base=transform_matrix(zero.base_translation_xyz_m, zero.base_quaternion_xyzw),
records=replay_records)
except TagInstallationFailure as error:
atomic_write_json(directory / "tag_installation_diagnostics.json", {
"passed": False, "publication_allowed": False,
"stage": "training_rigid_tag_installation",
"rotation_p95_limit_deg": 2.0, "translation_p95_limit_m": 0.003,
"roles": error.diagnostics})
raise
from .evidence import prepare_command_holdout
from ...core.domain.sampling import command_holdout_cycle
command_validation_cycle = command_holdout_cycle(profile)
steady_secondary = accepted_secondary_records(profile, records, sample_phase="steady",
directions_are_task_relative=directions_are_task_relative)
steady_rows = [r for rows in steady.values() for r in rows]
steady_rows.extend(dict(r, joint=profile.measurement.cross_view_sources[name])
for name, rows in steady_secondary.items() for r in rows)
command_holdout = prepare_command_holdout(profile, fit.command_mappings, steady_rows)
from ..motion_provenance import uses_motion_evidence
image_inputs = {}
if require_motion_evidence or uses_motion_evidence(records):
if camera_extrinsics_file is None:
raise ValueError("final_image_holdout_requires_protected_camera_file")
from .image_evidence import ImageEvidence
image_evidence = ImageEvidence(profile, records, camera_extrinsics_file=camera_extrinsics_file,
protected_inputs=protected_inputs)
image_inputs = {
"image_observations": image_evidence.all_view_holdout(replay.holdout,
mappings=fit.output_mappings, input_kind="feedback", holdout_cycle=profile.quality.holdout_cycle),
"command_image_observations": image_evidence.all_view_holdout(command_holdout,
mappings=fit.command_mappings, input_kind="command", holdout_cycle=command_validation_cycle),
"require_image_holdout": True,
}
validator = FrozenTagArtifactValidator(profile.key.profile_id, source,
protected_inputs["source_urdf_sha256"], profile.urdf_authorized_fields,
fit.zero_offsets_rad, replay.common_from_base, replay.installations,
replay.holdout, replay.required_roles, standard_loader, protected_inputs=dict(protected_inputs),
command_observations=command_holdout,
joint_observations=tuple(r for rows in steady.values() for r in rows if r["cycle"] == 3),
joint_observations=tuple(r for rows in steady.values() for r in rows if r["cycle"] == command_validation_cycle),
command_holdout_cycle=command_validation_cycle,
joint_curves=fit.curves,
joint_channels={name: channel_for_joint(profile, name) for name in fit.output_mappings},
transferred_motion_sources=profile.measurement.transferred_motion_sources,
cad_zero_assumptions=profile.zero.cad_zero_assumptions,
transferred_zero_sources=profile.zero.transferred_zero_sources,
joint_zero_methods=fit.zero_method_by_joint)
report_writer, command_writer = serialize_report, from_report
if profile.artifacts.output_schema_version >= 3:
from .serializers.measured_v3 import serialize_report as report_writer, from_report as command_writer
report = report_writer(profile, fit, prepared, serial_number=serial_number, protected_inputs=protected_inputs)
payload = command_writer(report)
naming = {"serial_number": serial_number, "side": profile.key.side, "model": profile.key.model.lower()}
json_path = directory / profile.artifacts.calibration_filename.format(**naming)
urdf_path = directory / profile.artifacts.corrected_urdf_filename.format(**naming)
joint_zero_methods=fit.zero_method_by_joint, range_evidence=prepared.plan.range_evidence,
directional_command_joints=profile.artifacts.directional_command_joints, **image_inputs)
check_cancelled()
phase_changed("holdout_complete")
prepared.plan.write(source, urdf_path)
atomic_write_json(json_path, payload)
atomic_write_json(directory / REPORT_FILENAME, report)
atomic_write_json(directory / "frozen_tag_installations.json", {
"common_from_base": replay.common_from_base,
"installations": {name: asdict(mount) for name, mount in replay.installations.items()}})
phase_changed("artifacts_built")
publisher = ArtifactPublisher(directory.parent, profile.artifacts.publication_pointer)
from ...core.urdf.image_acceptance import ImageHoldoutFailure
try:
staged = publisher.prepare(session_directory=directory, calibration_json=json_path,
corrected_urdf=urdf_path, validate=validator, cancelled=cancelled)
except ImageHoldoutFailure as error:
atomic_write_json(directory / "final_image_diagnostics.json", {
"passed": False, "publication_allowed": False, "reason": str(error),
"input_kind": error.input_kind, "holdout_cycle": error.holdout_cycle, "roles": error.report})
raise
import json
acceptance = json.loads(staged.validation_json)
atomic_write_json(directory / "standard_urdf_acceptance.json", acceptance)
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py
+153
View File
@@ -0,0 +1,153 @@
"""Bind final-file image checks to raw pixels and protected camera identities."""
import hashlib
import json
from numbers import Real
from pathlib import Path
import numpy as np
from ...core.geometry.extrinsics import camera_info_fingerprint, load_camera_extrinsics
from ...core.urdf.image_acceptance import TagImageObservation
from ..image_capture import IMAGE_OBSERVATION_POLICY
from .evidence import _replay_directions
class ImageEvidence:
"""Validate source identities once, then prepare disjoint input holdouts.
Primary observations retain their existing provenance checks. Additional
views contribute original pixels and synchronized SDK values directly,
without manufacturing a PnP pose or fitting against a holdout image.
"""
def __init__(self, profile, records, *, camera_extrinsics_file, protected_inputs):
path = Path(camera_extrinsics_file)
if hashlib.sha256(path.read_bytes()).hexdigest() != protected_inputs["camera_extrinsics_sha256"]:
raise ValueError("final_image_camera_extrinsics_changed")
self.profile = profile
self.extrinsics = load_camera_extrinsics(path, 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)
self.models, self.frames, self.raw_frames, self.completed = {}, {}, {}, {}
self.tags = {tag.role: (view.name, tag) for view in profile.vision.views for tag in view.tags}
self.tasks = {task.key: task for task in profile.motion.tasks}
for row in records:
kind = row.get("kind")
if kind == "rectified_camera_model":
self._bind(self.models, row["view"], row)
elif kind in {"pnp_candidate_frame", "image_observation_frame"}:
target = self.frames if kind == "pnp_candidate_frame" else self.raw_frames
self._bind(target, f"{row['view']}:{row['image_stamp_ns']}", row)
elif kind == "scan_unit_complete":
key = (row["task_name"], row["cycle"], row["direction"])
self.completed[key] = row
for view, model in self.models.items():
identity = self.extrinsics.cameras[view]
fingerprint = camera_info_fingerprint(width=model["width"], height=model["height"],
camera_matrix=model["raw_k"], distortion=model["raw_d"],
rectification=model["rectification_r"], projection=model["projection_p"])
if (fingerprint != identity.intrinsics_sha256
or model.get("matrix_source") != "CameraInfo.P[:3,:3]"
or model.get("input_is_rectified") is not True):
raise ValueError("final_image_camera_intrinsics_changed")
if not np.array_equal(np.asarray(model["projection_p"]).reshape(3, 4)[:, :3], model["camera_matrix"]):
raise ValueError("final_image_camera_projection_changed")
@staticmethod
def _bind(mapping, key, row):
if key in mapping and json.dumps(mapping[key], sort_keys=True) != json.dumps(row, sort_keys=True):
raise ValueError("final_image_source_identity_reused")
mapping[key] = row
def _image(self, frame, *, sample_id, role, cycle, sdk_values, directions):
view, tag = self.tags[role]
model = self.models.get(view)
if (model is None or frame["view"] != view or frame["cycle"] != cycle
or frame.get("input_is_rectified") is not True
or frame.get("camera_matrix_source") != "CameraInfo.P[:3,:3]"
or not np.array_equal(frame["camera_matrix"], model["camera_matrix"])):
raise ValueError("final_image_holdout_source_missing_or_changed")
field = "tags" if frame["kind"] == "image_observation_frame" else "roles"
pixel = frame[field].get(role, {})
if pixel.get("tag_size_m") != tag.size_m or pixel.get("tag_id") != tag.tag_id:
raise ValueError("final_image_holdout_tag_changed")
corners = np.asarray(pixel.get("corners_xy", ()))
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
raise ValueError("final_image_holdout_corners_missing")
return TagImageObservation(sample_id, role, cycle, tuple(sdk_values), tuple(sorted(directions.items())),
view, tag.size_m, tuple(map(tuple, corners)), tuple(map(tuple, frame["camera_matrix"])),
tuple(map(tuple, np.linalg.inv(self.extrinsics.transform(view)))),
task_name=frame.get("task_name", ""))
def primary_holdout(self, observations):
result = []
for observation in observations:
frame = self.frames.get(observation.sample_id)
if frame is None:
raise ValueError("final_image_holdout_source_missing_or_changed")
result.append(self._image(frame, sample_id=observation.sample_id, role=observation.role,
cycle=observation.cycle, sdk_values=observation.sdk_values,
directions=observation.sdk_directions_by_joint))
return tuple(result)
def all_view_holdout(self, observations, *, mappings, input_kind, holdout_cycle):
"""Use every synchronized image in the accepted independent scan units.
Duplicate primary/raw identities must agree in both pixels and SDK
conditions. Missing feedback is explicit unavailable evidence; it is
never replaced with a command value or a fitted angle.
"""
if input_kind not in {"feedback", "command"}:
raise ValueError("final_image_input_kind_invalid")
primary = self.primary_holdout(observations)
expected = {(row.role, row.sample_id): row for row in primary}
roles = {row.role for row in primary}
phase = "sweep" if input_kind == "feedback" else "steady"
result = {}
count = self.profile.command.command_count
for sample_id, frame in self.raw_frames.items():
if frame.get("cycle") != holdout_cycle or frame.get("sample_phase") != phase:
continue
unit = self.completed.get((frame["task_name"], holdout_cycle, frame["direction"]))
if unit is None or not unit["passed"] or unit["attempt"] != frame["attempt"]:
continue
if frame.get("policy") != IMAGE_OBSERVATION_POLICY or frame.get("command_unit") != self.profile.command.unit:
raise ValueError("final_image_raw_policy_or_unit_changed")
vectors = [frame.get("command_vector"), frame.get("feedback_vector")]
skew = frame.get("state_image_sync_error_ns")
if any(values is None for values in vectors) or skew is None:
continue
if (not isinstance(skew, Real) or isinstance(skew, bool)
or not np.isfinite(skew) or skew < 0):
raise ValueError("final_image_raw_sync_error_invalid")
arrays = [np.asarray(values) for values in vectors]
if any(values.shape != (count,) or values.dtype.kind not in "fiu"
or not np.all(np.isfinite(values)) for values in arrays):
raise ValueError("final_image_raw_sdk_vector_invalid")
history = frame.get("command_direction_by_index")
if (not isinstance(history, (list, tuple)) or len(history) != count
or any(value not in {"", "increasing", "decreasing"} for value in history)):
raise ValueError("final_image_raw_direction_history_invalid")
task = self.tasks.get(frame["task_name"])
if task is None:
raise ValueError("final_image_raw_task_unknown")
directions = _replay_directions(mappings, frame, task)
values = frame["feedback_vector" if input_kind == "feedback" else "command_vector"]
for role in sorted(roles.intersection(frame["tags"])):
image = self._image(frame, sample_id=sample_id, role=role, cycle=holdout_cycle,
sdk_values=values, directions=directions)
identity = (role, sample_id)
if identity in expected and image != expected[identity]:
raise ValueError("final_image_primary_raw_disagree")
result[identity] = image
if not expected or not expected.keys() <= result.keys():
raise ValueError("final_image_all_view_source_missing")
return tuple(result.values())
def prepare_image_holdout(profile, records, observations, *, camera_extrinsics_file, protected_inputs):
"""Historical primary-image binding; no new evidence is imputed to old files."""
return ImageEvidence(profile, records, camera_extrinsics_file=camera_extrinsics_file,
protected_inputs=protected_inputs).primary_holdout(observations)
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py
+102
View File
@@ -0,0 +1,102 @@
"""Isolate CPU-heavy finalization from live ROS callbacks.
The parent thread transports events/results and owns cancellation. Only the
controller may publish. A bounded, synced journal prefix avoids pickling the
entire capture while holding the control lock.
"""
import json
import multiprocessing
from pathlib import Path
import time
import traceback
from .finalization import finalize_profile_session
def read_journal_prefix(path, size):
"""Decode the frozen prefix without retaining copies of the raw journal.
Captures can contain hundreds of megabytes of JSONL. Reading the whole
prefix and then splitting it keeps two additional copies alive while the
decoded records accumulate, alongside the live camera processes.
"""
if not isinstance(size, int) or isinstance(size, bool) or size <= 0:
raise ValueError("invalid_finalization_journal_size")
rows = []
remaining = size
with Path(path).open("rb") as stream:
while remaining:
line = stream.readline(remaining)
if not line or not line.endswith(b"\n"):
raise ValueError("incomplete_finalization_journal_prefix")
remaining -= len(line)
row = json.loads(line)
if not isinstance(row, dict):
raise ValueError("invalid_finalization_journal_rows")
rows.append(row)
return tuple(rows)
def _prepare(connection, finalizer, journal_path, journal_size, kwargs):
started = time.monotonic()
try:
records = read_journal_prefix(journal_path, journal_size)
result = finalizer(**kwargs, records=records, publish=False,
phase_changed=lambda phase: connection.send(("phase", phase)))
connection.send(("result", result))
except Exception:
connection.send(("error", traceback.format_exc()))
finally:
from ...core.artifacts.storage import atomic_write_json
atomic_write_json(Path(kwargs["session_dir"]) / "finalization_timing.json", {
"backend": "isolated_process", "elapsed_seconds": time.monotonic()-started,
"journal_prefix_bytes": journal_size})
connection.close()
def isolated_finalize_profile_session(*, journal_path, journal_size, records,
phase_changed, cancelled, publish=False, finalizer=finalize_profile_session,
timeout_seconds=600., **kwargs):
"""Same production finalizer, with bounded cancellation and no publication."""
del records # The synced journal is the immutable capture transport.
if publish:
raise ValueError("isolated_worker_cannot_publish")
if cancelled():
raise ValueError("operator_abort:no_artifact_publication")
context = multiprocessing.get_context("spawn")
receive, send = context.Pipe(duplex=False)
process = context.Process(target=_prepare,
args=(send, finalizer, journal_path, journal_size, kwargs), daemon=True)
started = time.monotonic()
try:
process.start()
send.close()
while not cancelled():
if receive.poll(.05):
kind, value = receive.recv()
if kind == "phase":
phase_changed(value)
elif kind == "result":
return value
elif kind == "error":
raise RuntimeError(f"finalization_process_failed:\n{value}")
else:
raise RuntimeError(f"invalid_finalization_process_event:{kind}")
elif not process.is_alive():
raise RuntimeError(f"finalization_process_exited:{process.exitcode}")
if time.monotonic()-started >= timeout_seconds:
raise TimeoutError("finalization_process_timeout:no_artifact_publication")
raise ValueError("operator_abort:no_artifact_publication")
finally:
receive.close()
send.close()
if process.pid is not None:
process.join(timeout=.2)
if process.is_alive():
process.terminate()
process.join(timeout=1.)
if process.is_alive():
process.kill()
process.join(timeout=1.)
process.close()
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py
+63 -7
View File
@@ -109,6 +109,12 @@ class FrozenTagArtifactValidator:
cad_zero_assumptions: Mapping[str, str] = field(default_factory=dict)
transferred_zero_sources: Mapping[str, str] = field(default_factory=dict)
joint_zero_methods: Mapping[str, str] = field(default_factory=dict)
range_evidence: Mapping = field(default_factory=dict)
directional_command_joints: frozenset[str] = frozenset()
command_holdout_cycle: int = 3
image_observations: tuple = ()
command_image_observations: tuple = ()
require_image_holdout: bool = False
def __call__(self, json_path: Path, urdf_path: Path) -> Mapping[str, Any]:
payload = json.loads(json_path.read_text(encoding="utf-8"))
@@ -121,6 +127,8 @@ class FrozenTagArtifactValidator:
if (measured_from_report if measured else from_report)(feedback_payload) != payload:
raise ValueError("compact JSON differs from the frozen calibration report")
if measured:
if payload.get("directional_command_joints", []) != sorted(self.directional_command_joints):
raise ValueError("directional command declaration differs from the Profile")
if feedback_payload.get("protected_inputs") != dict(self.protected_inputs):
raise ValueError("v3 report protected inputs differ from the session")
if {name: row["sdk_channel"] for name, row in payload["joints"].items()} != dict(self.joint_channels):
@@ -129,11 +137,16 @@ class FrozenTagArtifactValidator:
validate_artifact_structure(source_urdf=self.source_urdf,
corrected_urdf=urdf_path, source_sha256=self.source_sha256,
authorized_fields=self.authorized_fields, payload=feedback_payload,
zero_offsets_rad=self.zero_offsets_rad)
zero_offsets_rad=self.zero_offsets_rad, range_evidence=self.range_evidence)
changes = validate_artifact_structure(source_urdf=self.source_urdf,
corrected_urdf=urdf_path, source_sha256=self.source_sha256,
authorized_fields=self.authorized_fields, payload=payload,
zero_offsets_rad=self.zero_offsets_rad)
zero_offsets_rad=self.zero_offsets_rad, range_evidence=self.range_evidence)
rebuilt_from_json = "urdf_correction" in payload
if rebuilt_from_json:
from .urdf_from_json import validate_json_urdf_correspondence
validate_json_urdf_correspondence(payload, self.source_urdf, urdf_path,
expected_profile_id=self.profile_id)
metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path,
payload=feedback_payload, common_from_base=self.common_from_base,
installations=self.installations, observations=self.observations,
@@ -144,32 +157,64 @@ class FrozenTagArtifactValidator:
command_metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path,
payload=payload, common_from_base=self.common_from_base,
installations=self.installations, observations=self.command_observations,
required_roles=self.required_roles, minimum_samples=3, input_kind="command")
required_roles=self.required_roles, minimum_samples=3, input_kind="command",
holdout_cycle=self.command_holdout_cycle)
except ValueError as error:
if payload.get("format") == COMPACT_FORMAT:
raise ValueError(f"compact_command_table_holdout_failed:{error}") from error
raise
image_metrics = {}
image_policy = None
if self.require_image_holdout:
from ...core.urdf.image_acceptance import FINAL_IMAGE_ACCEPTANCE_POLICY, validate_serialized_image_holdout
image_policy = FINAL_IMAGE_ACCEPTANCE_POLICY
common = dict(corrected_urdf=urdf_path, common_from_base=self.common_from_base,
installations=self.installations, required_roles=self.required_roles)
image_metrics["feedback"] = validate_serialized_image_holdout(**common,
payload=feedback_payload, observations=self.image_observations,
independent_observations=self.observations,
input_kind="feedback", holdout_cycle=3, minimum_samples=self.minimum_samples)
image_metrics["command"] = validate_serialized_image_holdout(**common,
payload=payload, observations=self.command_image_observations,
independent_observations=self.command_observations,
input_kind="command", holdout_cycle=self.command_holdout_cycle, minimum_samples=3)
self.standard_loader(urdf_path)
evidence = {"base": self.common_from_base,
"installations": {k: asdict(v) for k, v in self.installations.items()},
"observations": [asdict(row) for row in self.observations]}
evidence["steady_command_observations"] = [asdict(row) for row in self.command_observations]
evidence["image_observations"] = [asdict(row) for row in self.image_observations]
evidence["command_image_observations"] = [asdict(row) for row in self.command_image_observations]
evidence["motion_range_evidence"] = {name: asdict(item) for name, item in self.range_evidence.items()}
extra = {}
if measured:
from ...core.urdf.joint_control import (
JSON_JOINT_ANGLES, independent_joint_sources, uses_affine_mimic_approximation,
uses_source_mimic,
)
from ...core.urdf.measured_acceptance import validate_measured_joint_holdout
extra = {"independent_mapping_holdout": validate_measured_joint_holdout(payload,
self.joint_curves, self.joint_observations,
transferred_motion_sources=self.transferred_motion_sources),
transferred_motion_sources=self.transferred_motion_sources,
holdout_cycle=self.command_holdout_cycle),
"full_json_urdf_holdout": command_metrics,
"independently_measured_joints": sorted(self.joint_curves),
"transferred_joints": sorted(self.transferred_motion_sources),
"transferred_motion_sources": dict(self.transferred_motion_sources),
"cad_zero_assumptions": dict(self.cad_zero_assumptions),
"precision_scope": "observed_joints_travel_directions_and_held_poses",
"urdf_mimic_approximation": validate_serialized_tag_holdout(corrected_urdf=urdf_path,
"directional_command_joints": sorted(self.directional_command_joints),
"precision_scope": "observed_joints_travel_directions_and_held_poses"}
if independent_joint_sources(payload) is not None:
extra["urdf_joint_angle_source"] = JSON_JOINT_ANGLES
else:
extra["urdf_mimic_approximation"] = validate_serialized_tag_holdout(corrected_urdf=urdf_path,
payload=payload, common_from_base=self.common_from_base, installations=self.installations,
observations=self.command_observations, required_roles=self.required_roles, minimum_samples=3,
input_kind="command", mimic_approximation=True, enforce_precision=False)}
input_kind="command", mimic_approximation=True, enforce_precision=False,
holdout_cycle=self.command_holdout_cycle)
if uses_affine_mimic_approximation(payload) or uses_source_mimic(payload):
extra["urdf_joint_angle_source"] = JSON_JOINT_ANGLES
extra["urdf_mimic_policy"] = payload["urdf_correction"]["mimic_policy"]
evidence["joint_observations"] = self.joint_observations
evidence["frozen_joint_curves"] = {key: asdict(value) for key, value in self.joint_curves.items()}
evidence["transferred_motion_sources"] = dict(self.transferred_motion_sources)
@@ -177,10 +222,16 @@ class FrozenTagArtifactValidator:
acceptance_kind = ("measured_and_transferred_json_urdf_v3" if self.transferred_motion_sources
else "independently_measured_json_urdf_v3") if measured else "serialized_standard_urdf_tag_replay_v1"
return {"acceptance": acceptance_kind,
"final_file_image_holdout": image_metrics,
"final_file_image_holdout_policy": image_policy,
"final_file_image_holdout_verified": self.require_image_holdout,
**extra,
"urdf_rebuilt_from_json": rebuilt_from_json,
"profile_id": self.profile_id, "source_urdf_sha256": self.source_sha256,
"protected_inputs": dict(self.protected_inputs),
"motion_range_evidence": evidence["motion_range_evidence"],
"holdout_cycle": 3, "tag_holdout": metrics,
"command_holdout_cycle": self.command_holdout_cycle,
"steady_command_tag_holdout": command_metrics,
"final_file_spatial_replay": "passed",
"holdout_evidence_sha256": hashlib.sha256(json.dumps(evidence,
@@ -189,6 +240,11 @@ class FrozenTagArtifactValidator:
def _validate_report_provenance(self, report):
"""The report cannot grant itself new transfer or CAD authorizations."""
if report.get("quality", {}).get("command_holdout_cycle", 3) != self.command_holdout_cycle:
raise ValueError("v3 command validation partition differs from the Profile")
ranges = json.loads(json.dumps({name: asdict(item) for name, item in self.range_evidence.items()}))
if report.get("motion_range_evidence", {}) != ranges:
raise ValueError("v3 motion range evidence differs from the frozen training fit")
if report.get("transferred_motion_sources", {}) != dict(self.transferred_motion_sources):
raise ValueError("v3 transfer sources differ from the measured Profile")
rows = report["joints"]
src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py
+31 -6
View File
@@ -7,13 +7,19 @@ from pathlib import Path
from ...core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables
from ...core.urdf.kinematics import UrdfKinematicModel
from ...core.urdf.joint_control import (
JSON_JOINT_ANGLES, independent_joint_sources, uses_affine_mimic_approximation,
uses_source_mimic,
)
from .serializers.compact_v2 import FORMAT as COMPACT_FORMAT, from_report
from .serializers.measured_v3 import FORMAT as MEASURED_FORMAT, from_report as measured_from_report
from .direction import next_input_directions
class UnifiedCommandMapper:
def __init__(self, payload, corrected_urdf, *, input_kind="command", expected_side=None, report=None):
self.measured = payload.get("format") == MEASURED_FORMAT
self.json_mimic_override = self.measured and independent_joint_sources(payload) is None
compact = payload.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT}
if payload.get("format") not in {"unified_calibration_v1", COMPACT_FORMAT, MEASURED_FORMAT}:
raise ValueError("requires a unified calibration artifact")
@@ -45,6 +51,13 @@ class UnifiedCommandMapper:
self.feedback_name_aliases = dict(metadata.get("feedback_name_aliases", {}))
self.previous = None
self.directions = [""]*len(self.command_names)
self.directional_channels = {}
for name in self.mapping.directional_joints:
row = self.mapping.joints[name]
knots = row["curve_input_knots_rad"]
lower, upper = (knots[0], knots[-1]) if knots else (0., 255.)
index = row["motor_index"]
self.directional_channels[index] = (metadata["baseline_command"][index], lower, upper)
def map_positions(self, positions, names=()):
values = tuple(float(v) for v in positions)
@@ -56,14 +69,12 @@ class UnifiedCommandMapper:
raise ValueError("feedback names differ from the serialized SDK layout")
named = dict(zip(names, values))
values = tuple(named[n] for n in self.command_names)
directions = self.directions.copy()
if self.previous is not None:
for i, (old, new) in enumerate(zip(self.previous, values)):
if abs(new-old) > (0.5 if self.input_domain.endswith("u8") else 1e-9):
directions[i] = "increasing" if new > old else "decreasing"
directions = next_input_directions(values, self.previous, self.directions,
resolution=0.5 if self.input_domain.endswith("u8") else 1e-9,
channels=self.directional_channels, byte_input=self.input_domain.endswith("u8"))
active = self.mapping.evaluate(values, "", active_joints=self.active,
directions_by_joint={name: directions[row["motor_index"]] for name, row in self.mapping.joints.items()})
result = self.urdf.resolve_angles(active, independent_mimic_angles=self.measured)
result = self.urdf.resolve_angles(active, independent_mimic_angles=self.json_mimic_override)
for name, angle in result.items():
joint = self.urdf.joints[name]
if joint.lower is not None and not joint.lower-1e-9 <= angle <= joint.upper+1e-9:
@@ -91,8 +102,20 @@ def load_unified_mapper(calibration_file, *, expected_side=None, input_kind="com
if validation.get("final_file_spatial_replay") != "passed" or not validation.get("steady_command_tag_holdout"):
raise ValueError("release lacks independent command and feedback URDF evidence")
payload = json.loads(json_path.read_text())
if "urdf_correction" in payload and validation.get("urdf_rebuilt_from_json") is not True:
raise ValueError("release lacks JSON-to-URDF correspondence validation")
report = json.loads(artifact("calibration_report").read_text()) if payload.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT} else None
if payload.get("format") == MEASURED_FORMAT:
if (independent_joint_sources(payload) is not None
and validation.get("urdf_joint_angle_source") != JSON_JOINT_ANGLES):
raise ValueError("release lacks explicit JSON joint angle acceptance")
if (uses_affine_mimic_approximation(payload) or uses_source_mimic(payload)) and (
validation.get("urdf_mimic_policy") != payload["urdf_correction"]["mimic_policy"]
or validation.get("urdf_joint_angle_source") != JSON_JOINT_ANGLES
or not validation.get("urdf_mimic_approximation")):
raise ValueError("release lacks separate mimic approximation acceptance")
if validation.get("directional_command_joints", []) != payload.get("directional_command_joints", []):
raise ValueError("v3 release directional mapping differs from its acceptance")
transfers = report.get("transferred_motion_sources", {})
expected_kind = "measured_and_transferred_json_urdf_v3" if transfers else "independently_measured_json_urdf_v3"
observed = {name for name, row in report["joints"].items() if row.get("independently_measured") is True}
@@ -109,5 +132,7 @@ def load_unified_mapper(calibration_file, *, expected_side=None, input_kind="com
if "cad_zero_assumption" in row}
if validation.get("cad_zero_assumptions", {}) != assumptions:
raise ValueError("v3 release CAD assumptions differ from its report")
if validation.get("motion_range_evidence", {}) != report.get("motion_range_evidence", {}):
raise ValueError("v3 release motion range evidence differs from its report")
return UnifiedCommandMapper(payload, urdf_path,
input_kind=input_kind, expected_side=expected_side, report=report)

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