Compare commits
17 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 admin
|
93e654578a | ||
|
admin
|
50ff372ca3 | ||
|
admin
|
b362e9bb40 | ||
|
admin
|
d6c83dd23d | ||
|
admin
|
9952dbf4a7 | ||
|
admin
|
6830bc6801 | ||
|
admin
|
8456aa7f2d | ||
|
admin
|
54b8f66b2f | ||
|
admin
|
db4c7af3e7 | ||
|
admin
|
5ee4a3bb7c | ||
|
admin
|
1d866f7a51 | ||
|
admin
|
a8eaa4c367 | ||
|
admin
|
32b6af62e2 | ||
|
admin
|
af2dc9c38f | ||
|
admin
|
7a04780b52 | ||
|
admin
|
467651fc47 | ||
|
admin
|
69c2da6808 |
+12
-2
@@ -50,7 +50,8 @@ Thumbs.db
|
|||||||
|
|
||||||
# Runtime and calibration scratch files
|
# Runtime and calibration scratch files
|
||||||
/logs/
|
/logs/
|
||||||
/MvSdkLog/
|
# The camera SDK writes logs relative to the launch working directory.
|
||||||
|
MvSdkLog/
|
||||||
*.tmp
|
*.tmp
|
||||||
*.log
|
*.log
|
||||||
*.bak
|
*.bak
|
||||||
@@ -61,8 +62,15 @@ Thumbs.db
|
|||||||
# Reproducible seed profiles remain under
|
# Reproducible seed profiles remain under
|
||||||
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
||||||
/profiles/
|
/profiles/
|
||||||
/calibration_output/
|
# Calibration outputs may be created from the workspace root or a subdirectory.
|
||||||
|
calibration_output/
|
||||||
|
range_calibration_output/
|
||||||
/config/*_three_camera_extrinsics.yaml
|
/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
|
*.wear_check.json
|
||||||
*.checkpoint.json
|
*.checkpoint.json
|
||||||
*.verification.json
|
*.verification.json
|
||||||
@@ -73,6 +81,8 @@ Thumbs.db
|
|||||||
/src/linkerhand_calibration/urdf/*/*_calibrated_*.urdf
|
/src/linkerhand_calibration/urdf/*/*_calibrated_*.urdf
|
||||||
/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
|
/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
|
||||||
/src/linkerhand_calibration/urdf/*/*_transferred_from_*.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_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_calibrated_*.urdf
|
||||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
|
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
|
||||||
|
|||||||
@@ -1 +1,2 @@
|
|||||||
避免产生屎山代码，要求代码结构清晰可读性好
|
1.不要补丁式修复问题，避免产生屎山代码，要求代码结构清晰可读性好。
|
||||||
|
2.永远用中文回复。
|
||||||
@@ -0,0 +1,60 @@
|
|||||||
|
schema_version: 1
|
||||||
|
reference_view: front
|
||||||
|
cameras:
|
||||||
|
front:
|
||||||
|
serial_number: DB2163742
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07
|
||||||
|
side:
|
||||||
|
serial_number: DB2163749
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655
|
||||||
|
top:
|
||||||
|
serial_number: DB2163739
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446
|
||||||
|
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.8438350117109596
|
||||||
|
- 0.0030839597011880146
|
||||||
|
- 1.058604518460327
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.04630065903711772
|
||||||
|
- 0.6864139609264401
|
||||||
|
- -0.015210755676403294
|
||||||
|
- 0.7255761546038824
|
||||||
|
top:
|
||||||
|
translation_xyz_m:
|
||||||
|
- 0.008505305967195427
|
||||||
|
- -0.5708652170072022
|
||||||
|
- 1.1230571120977015
|
||||||
|
quaternion_xyzw:
|
||||||
|
- 0.7264186042657113
|
||||||
|
- 0.05091741831973884
|
||||||
|
- 0.0330385816024251
|
||||||
|
- -0.6845669288053643
|
||||||
|
quality:
|
||||||
|
passed: true
|
||||||
|
reprojection_rms_px: 0.9332749561975657
|
||||||
|
maximum_rotation_repeatability_deg: 0.05294934187781907
|
||||||
|
maximum_translation_repeatability_m: 0.0004960858291558162
|
||||||
|
front_side_captures: 15
|
||||||
|
front_top_captures: 15
|
||||||
|
front_side_candidates: 15
|
||||||
|
front_top_candidates: 15
|
||||||
|
front_side_rejected: 0
|
||||||
|
front_top_rejected: 0
|
||||||
@@ -0,0 +1,60 @@
|
|||||||
|
schema_version: 1
|
||||||
|
reference_view: front
|
||||||
|
cameras:
|
||||||
|
front:
|
||||||
|
serial_number: DB2163742
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07
|
||||||
|
side:
|
||||||
|
serial_number: DB2163749
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655
|
||||||
|
top:
|
||||||
|
serial_number: DB2163739
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446
|
||||||
|
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.8521079207290448
|
||||||
|
- 0.0021412076894364238
|
||||||
|
- 1.0361400609750895
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.04900255095488596
|
||||||
|
- 0.676537184995331
|
||||||
|
- -0.016467372664989974
|
||||||
|
- 0.7345917321587682
|
||||||
|
top:
|
||||||
|
translation_xyz_m:
|
||||||
|
- 0.0004373257585413154
|
||||||
|
- -0.5686488899083978
|
||||||
|
- 1.1253160870539256
|
||||||
|
quaternion_xyzw:
|
||||||
|
- 0.7278983925982527
|
||||||
|
- 0.060947357492050866
|
||||||
|
- 0.04110103817603059
|
||||||
|
- -0.6817331254446042
|
||||||
|
quality:
|
||||||
|
passed: true
|
||||||
|
reprojection_rms_px: 0.9071907304479074
|
||||||
|
maximum_rotation_repeatability_deg: 0.05898535309651316
|
||||||
|
maximum_translation_repeatability_m: 0.0007701935623164992
|
||||||
|
front_side_captures: 15
|
||||||
|
front_top_captures: 15
|
||||||
|
front_side_candidates: 15
|
||||||
|
front_top_candidates: 15
|
||||||
|
front_side_rejected: 0
|
||||||
|
front_top_rejected: 0
|
||||||
@@ -0,0 +1,60 @@
|
|||||||
|
schema_version: 1
|
||||||
|
reference_view: front
|
||||||
|
cameras:
|
||||||
|
front:
|
||||||
|
serial_number: DB2163742
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07
|
||||||
|
side:
|
||||||
|
serial_number: DB2163749
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655
|
||||||
|
top:
|
||||||
|
serial_number: DB2163739
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446
|
||||||
|
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.8080948548192018
|
||||||
|
- -0.008581421683932288
|
||||||
|
- 1.0532250983464067
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.021327054698668555
|
||||||
|
- 0.6912906965521914
|
||||||
|
- 0.01197673364064859
|
||||||
|
- 0.7221626461189797
|
||||||
|
top:
|
||||||
|
translation_xyz_m:
|
||||||
|
- 0.04741691749425549
|
||||||
|
- -0.5804548270182806
|
||||||
|
- 1.0631239748696568
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.7015214400964395
|
||||||
|
- -0.04845091102701706
|
||||||
|
- -0.028754624245378058
|
||||||
|
- 0.710417729149672
|
||||||
|
quality:
|
||||||
|
passed: true
|
||||||
|
reprojection_rms_px: 1.0243965778937039
|
||||||
|
maximum_rotation_repeatability_deg: 0.07214266243246935
|
||||||
|
maximum_translation_repeatability_m: 0.0008227910293034463
|
||||||
|
front_side_captures: 15
|
||||||
|
front_top_captures: 15
|
||||||
|
front_side_candidates: 15
|
||||||
|
front_top_candidates: 15
|
||||||
|
front_side_rejected: 0
|
||||||
|
front_top_rejected: 0
|
||||||
@@ -0,0 +1,60 @@
|
|||||||
|
schema_version: 1
|
||||||
|
reference_view: front
|
||||||
|
cameras:
|
||||||
|
front:
|
||||||
|
serial_number: DB2163742
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07
|
||||||
|
side:
|
||||||
|
serial_number: DB2163749
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655
|
||||||
|
top:
|
||||||
|
serial_number: DB2163739
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446
|
||||||
|
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.8138585956461776
|
||||||
|
- -0.008439712508702443
|
||||||
|
- 1.0397597678627144
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.017450615663592975
|
||||||
|
- 0.6859174861464342
|
||||||
|
- 0.01494743127074939
|
||||||
|
- 0.7273164734212502
|
||||||
|
top:
|
||||||
|
translation_xyz_m:
|
||||||
|
- 0.03984872898745615
|
||||||
|
- -0.5795215841986667
|
||||||
|
- 1.0606733822933996
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.7006937955854443
|
||||||
|
- -0.05305859990878122
|
||||||
|
- -0.03489268952144818
|
||||||
|
- 0.7106303469608818
|
||||||
|
quality:
|
||||||
|
passed: true
|
||||||
|
reprojection_rms_px: 0.8784854263662394
|
||||||
|
maximum_rotation_repeatability_deg: 0.03530519844936986
|
||||||
|
maximum_translation_repeatability_m: 0.0005675535417705807
|
||||||
|
front_side_captures: 15
|
||||||
|
front_top_captures: 15
|
||||||
|
front_side_candidates: 15
|
||||||
|
front_top_candidates: 15
|
||||||
|
front_side_rejected: 0
|
||||||
|
front_top_rejected: 0
|
||||||
@@ -0,0 +1,60 @@
|
|||||||
|
schema_version: 1
|
||||||
|
reference_view: front
|
||||||
|
cameras:
|
||||||
|
front:
|
||||||
|
serial_number: DB2163742
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07
|
||||||
|
side:
|
||||||
|
serial_number: DB2163749
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655
|
||||||
|
top:
|
||||||
|
serial_number: DB2163739
|
||||||
|
width: 1624
|
||||||
|
height: 1240
|
||||||
|
intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446
|
||||||
|
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.807055290135415
|
||||||
|
- 0.007757362896197391
|
||||||
|
- 1.109645424653359
|
||||||
|
quaternion_xyzw:
|
||||||
|
- -0.038239828751376485
|
||||||
|
- 0.7081714897073992
|
||||||
|
- -0.008111842845009399
|
||||||
|
- 0.7049574842983983
|
||||||
|
top:
|
||||||
|
translation_xyz_m:
|
||||||
|
- 0.04396900884358515
|
||||||
|
- -0.5702983486721814
|
||||||
|
- 1.106333731407498
|
||||||
|
quaternion_xyzw:
|
||||||
|
- 0.7207445525815062
|
||||||
|
- 0.025265750150805705
|
||||||
|
- 0.010996906531907304
|
||||||
|
- -0.6926528710978753
|
||||||
|
quality:
|
||||||
|
passed: true
|
||||||
|
reprojection_rms_px: 0.8851530873841623
|
||||||
|
maximum_rotation_repeatability_deg: 0.02169207759215767
|
||||||
|
maximum_translation_repeatability_m: 0.00023144069642531324
|
||||||
|
front_side_captures: 15
|
||||||
|
front_top_captures: 15
|
||||||
|
front_side_candidates: 15
|
||||||
|
front_top_candidates: 15
|
||||||
|
front_side_rejected: 0
|
||||||
|
front_top_rejected: 0
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
+10
@@ -7,6 +7,7 @@ from enum import Enum
|
|||||||
from utils.open_can import OpenCan
|
from utils.open_can import OpenCan
|
||||||
from can.exceptions import CanError
|
from can.exceptions import CanError
|
||||||
from utils.color_msg import ColorMsg
|
from utils.color_msg import ColorMsg
|
||||||
|
from utils.feedback import ReceivedPositionFrames
|
||||||
current_dir = os.path.dirname(os.path.abspath(__file__))
|
current_dir = os.path.dirname(os.path.abspath(__file__))
|
||||||
target_dir = os.path.abspath(os.path.join(current_dir, ".."))
|
target_dir = os.path.abspath(os.path.join(current_dir, ".."))
|
||||||
sys.path.append(target_dir)
|
sys.path.append(target_dir)
|
||||||
@@ -178,6 +179,10 @@ class LinkerHandG20Can:
|
|||||||
|
|
||||||
# 串联控制数据存储
|
# 串联控制数据存储
|
||||||
self.x41, self.x42, self.x43, self.x44, self.x45 = [], [], [], [], []
|
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.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.x51, self.x52, self.x53, self.x54, self.x55 = [], [], [], [], []
|
||||||
self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], []
|
self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], []
|
||||||
@@ -354,6 +359,8 @@ class LinkerHandG20Can:
|
|||||||
response_data = msg.data[1:]
|
response_data = msg.data[1:]
|
||||||
if len(list(response_data)) == 0:
|
if len(list(response_data)) == 0:
|
||||||
return
|
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)
|
if frame_type == 0x01: self.x01 = list(response_data)
|
||||||
elif frame_type == 0x02: self.x02 = list(response_data)
|
elif frame_type == 0x02: self.x02 = list(response_data)
|
||||||
@@ -1040,6 +1047,9 @@ class LinkerHandG20Can:
|
|||||||
return None
|
return None
|
||||||
return self.joint_state_to_cmd_state(state=state)
|
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):
|
def get_current_pub_status(self):
|
||||||
"""API接口:获取手指当前状态"""
|
"""API接口:获取手指当前状态"""
|
||||||
self.get_current_status()
|
self.get_current_status()
|
||||||
|
|||||||
+6
@@ -6,6 +6,7 @@ import threading
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
from utils.open_can import OpenCan
|
from utils.open_can import OpenCan
|
||||||
from utils.color_msg import ColorMsg
|
from utils.color_msg import ColorMsg
|
||||||
|
from utils.feedback import ReceivedPositionFrames
|
||||||
from can.exceptions import CanError
|
from can.exceptions import CanError
|
||||||
|
|
||||||
|
|
||||||
@@ -17,6 +18,7 @@ class LinkerHandL6Can:
|
|||||||
self.open_can = OpenCan(load_yaml=yaml)
|
self.open_can = OpenCan(load_yaml=yaml)
|
||||||
|
|
||||||
self.x01 = [0] * 6 # 关节位置
|
self.x01 = [0] * 6 # 关节位置
|
||||||
|
self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
|
||||||
self.x02 = [-1] * 6 # 转矩限制
|
self.x02 = [-1] * 6 # 转矩限制
|
||||||
self.x05 = [0] * 6 # 速度
|
self.x05 = [0] * 6 # 速度
|
||||||
self.x07 = [-1] * 6 # 加速度
|
self.x07 = [-1] * 6 # 加速度
|
||||||
@@ -233,6 +235,7 @@ class LinkerHandL6Can:
|
|||||||
break
|
break
|
||||||
if not is_position_echo:
|
if not is_position_echo:
|
||||||
self.x01 = list(response)
|
self.x01 = list(response)
|
||||||
|
self.position_feedback.accept(frame_type, response, msg.timestamp)
|
||||||
elif frame_type == 0x02: # 0x02
|
elif frame_type == 0x02: # 0x02
|
||||||
self.x02 = list(response_data)
|
self.x02 = list(response_data)
|
||||||
elif frame_type == 0x05: # Set speed
|
elif frame_type == 0x05: # Set speed
|
||||||
@@ -326,6 +329,9 @@ class LinkerHandL6Can:
|
|||||||
def get_current_pub_status(self):
|
def get_current_pub_status(self):
|
||||||
return self.x01
|
return self.x01
|
||||||
|
|
||||||
|
def get_feedback_snapshot(self):
|
||||||
|
return self.position_feedback.snapshot()
|
||||||
|
|
||||||
def get_speed(self):
|
def get_speed(self):
|
||||||
#self.send_frame(0x05, [],sleep=0.003)
|
#self.send_frame(0x05, [],sleep=0.003)
|
||||||
#print("L6暂不支持读取实时速度")
|
#print("L6暂不支持读取实时速度")
|
||||||
|
|||||||
+6
@@ -4,6 +4,7 @@ import threading
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
from utils.open_can import OpenCan
|
from utils.open_can import OpenCan
|
||||||
from utils.color_msg import ColorMsg
|
from utils.color_msg import ColorMsg
|
||||||
|
from utils.feedback import ReceivedPositionFrames
|
||||||
from can.exceptions import CanError
|
from can.exceptions import CanError
|
||||||
|
|
||||||
|
|
||||||
@@ -15,6 +16,7 @@ class LinkerHandO6Can:
|
|||||||
self.open_can = OpenCan(load_yaml=yaml)
|
self.open_can = OpenCan(load_yaml=yaml)
|
||||||
|
|
||||||
self.x01 = [0] * 6 # 关节位置
|
self.x01 = [0] * 6 # 关节位置
|
||||||
|
self.position_feedback = ReceivedPositionFrames({0x01: 6}, [(0x01, i) for i in range(6)])
|
||||||
self.x02 = [-1] * 6 # 转矩限制
|
self.x02 = [-1] * 6 # 转矩限制
|
||||||
self.x05 = [0] * 6 # 速度
|
self.x05 = [0] * 6 # 速度
|
||||||
self.x07 = [-1] * 6 # 加速度
|
self.x07 = [-1] * 6 # 加速度
|
||||||
@@ -224,6 +226,7 @@ class LinkerHandO6Can:
|
|||||||
return
|
return
|
||||||
if frame_type == 0x01: # 0x01
|
if frame_type == 0x01: # 0x01
|
||||||
self.x01 = list(response_data)
|
self.x01 = list(response_data)
|
||||||
|
self.position_feedback.accept(frame_type, response_data, msg.timestamp)
|
||||||
elif frame_type == 0x02: # 0x02
|
elif frame_type == 0x02: # 0x02
|
||||||
self.x02 = list(response_data)
|
self.x02 = list(response_data)
|
||||||
elif frame_type == 0x05: # Set speed
|
elif frame_type == 0x05: # Set speed
|
||||||
@@ -317,6 +320,9 @@ class LinkerHandO6Can:
|
|||||||
def get_current_pub_status(self):
|
def get_current_pub_status(self):
|
||||||
return self.x01
|
return self.x01
|
||||||
|
|
||||||
|
def get_feedback_snapshot(self):
|
||||||
|
return self.position_feedback.snapshot()
|
||||||
|
|
||||||
def get_speed(self):
|
def get_speed(self):
|
||||||
self.send_frame(0x05, [],sleep=0.002)
|
self.send_frame(0x05, [],sleep=0.002)
|
||||||
#print("L6暂不支持读取实时速度")
|
#print("L6暂不支持读取实时速度")
|
||||||
|
|||||||
@@ -207,6 +207,14 @@ class LinkerHandApi:
|
|||||||
getter = getattr(self.hand, "get_cached_current_status", None)
|
getter = getattr(self.hand, "get_cached_current_status", None)
|
||||||
return getter() if getter is not None else 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):
|
def get_state_for_pub(self):
|
||||||
return self.hand.get_current_pub_status()
|
return self.hand.get_current_pub_status()
|
||||||
|
|||||||
@@ -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))
|
||||||
@@ -168,6 +168,8 @@ class LinkerHand(Node):
|
|||||||
self.last_position_command_time = None
|
self.last_position_command_time = None
|
||||||
|
|
||||||
self.last_hand_state = [-1] * 10
|
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.last_hand_vel = [-1] * 10
|
||||||
self.force = [[-1] * 5] * 4
|
self.force = [[-1] * 5] * 4
|
||||||
self.matrix_dic = {
|
self.matrix_dic = {
|
||||||
@@ -245,6 +247,7 @@ class LinkerHand(Node):
|
|||||||
COMMAND_QOS,
|
COMMAND_QOS,
|
||||||
)
|
)
|
||||||
self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10)
|
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)
|
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.is_touch == True:
|
||||||
if self.modbus != "None":
|
if self.modbus != "None":
|
||||||
@@ -391,7 +394,7 @@ class LinkerHand(Node):
|
|||||||
self.last_hand_vel_cmd = None
|
self.last_hand_vel_cmd = None
|
||||||
|
|
||||||
def _poll_state_if_due(self):
|
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
|
return
|
||||||
now = time.monotonic()
|
now = time.monotonic()
|
||||||
if state_reads_deferred(
|
if state_reads_deferred(
|
||||||
@@ -400,8 +403,7 @@ class LinkerHand(Node):
|
|||||||
self.command_quiet_period,
|
self.command_quiet_period,
|
||||||
self.defer_state_reads_while_commanding,
|
self.defer_state_reads_while_commanding,
|
||||||
):
|
):
|
||||||
# pub_state continues to publish the last completed state as a
|
# A repeated heartbeat retains its original measurement time.
|
||||||
# heartbeat. A fresh blocking read is made after motion settles.
|
|
||||||
return
|
return
|
||||||
if not state_poll_due(
|
if not state_poll_due(
|
||||||
self.last_state_poll_time, now, self.state_poll_period
|
self.last_state_poll_time, now, self.state_poll_period
|
||||||
@@ -411,6 +413,7 @@ class LinkerHand(Node):
|
|||||||
# another read on the following timer callback.
|
# another read on the following timer callback.
|
||||||
self.last_state_poll_time = now
|
self.last_state_poll_time = now
|
||||||
self.last_hand_state = self.api.get_state()
|
self.last_hand_state = self.api.get_state()
|
||||||
|
self.last_hand_state_stamp_ns = self.get_clock().now().nanoseconds
|
||||||
time.sleep(0.003)
|
time.sleep(0.003)
|
||||||
if state_poll_due(
|
if state_poll_due(
|
||||||
self.last_velocity_poll_time, now, self.velocity_poll_period
|
self.last_velocity_poll_time, now, self.velocity_poll_period
|
||||||
@@ -478,9 +481,7 @@ class LinkerHand(Node):
|
|||||||
|
|
||||||
def pub_state(self):
|
def pub_state(self):
|
||||||
while True:
|
while True:
|
||||||
if self.hand_state_pub.get_subscription_count() > 0:
|
self._publish_feedback()
|
||||||
msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
|
|
||||||
self.hand_state_pub.publish(msg)
|
|
||||||
if self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
|
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 = Float32MultiArray()
|
||||||
msg.data = [float(val) for sublist in self.force for val in sublist]
|
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)
|
self.hand_info_pub.publish(msg)
|
||||||
time.sleep(self.hz)
|
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):
|
def pub_matrix_mass(self, dic):
|
||||||
"""发布矩阵数据合值 单位g 克 JSON格式"""
|
"""发布矩阵数据合值 单位g 克 JSON格式"""
|
||||||
msg = String()
|
msg = String()
|
||||||
@@ -556,10 +577,12 @@ class LinkerHand(Node):
|
|||||||
msg.data = json.dumps(self.matrix_dic)
|
msg.data = json.dumps(self.matrix_dic)
|
||||||
self.matrix_touch_pub.publish(msg)
|
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 = JointState()
|
||||||
joint_state.header = Header()
|
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.name = self.api.get_finger_order()
|
||||||
joint_state.position = [float(x) for x in pose]
|
joint_state.position = [float(x) for x in pose]
|
||||||
if len(vel) > 1:
|
if len(vel) > 1:
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -46,12 +46,14 @@ def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
|
|||||||
sys.path.insert(0, str(linker_hand_root))
|
sys.path.insert(0, str(linker_hand_root))
|
||||||
try:
|
try:
|
||||||
from core.can.linker_hand_l6_can import LinkerHandL6Can
|
from core.can.linker_hand_l6_can import LinkerHandL6Can
|
||||||
|
from utils.feedback import ReceivedPositionFrames
|
||||||
finally:
|
finally:
|
||||||
sys.path.remove(str(linker_hand_root))
|
sys.path.remove(str(linker_hand_root))
|
||||||
|
|
||||||
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
|
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
|
||||||
hand.can_id = 0x27
|
hand.can_id = 0x27
|
||||||
hand.x01 = [10, 20, 30, 40, 50, 60]
|
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()
|
hand._position_echo_lock = threading.Lock()
|
||||||
command = (255, 2, 253, 253, 253, 253)
|
command = (255, 2, 253, 253, 253, 253)
|
||||||
hand._pending_position_echoes = deque(
|
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._position_echo_timeout_seconds = 0.02
|
||||||
|
|
||||||
hand.process_response(
|
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 hand.x01 == [10, 20, 30, 40, 50, 60]
|
||||||
assert not hand._pending_position_echoes
|
assert not hand._pending_position_echoes
|
||||||
|
|
||||||
measured = (250, 3, 252, 252, 252, 252)
|
measured = (250, 3, 252, 252, 252, 252)
|
||||||
hand.process_response(
|
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.x01 == list(measured)
|
||||||
|
assert hand.get_feedback_snapshot().channel_stamps_ns == (101_000_000_000,) * 6
|
||||||
|
|||||||
@@ -0,0 +1,8 @@
|
|||||||
|
# 标定包协作约定
|
||||||
|
|
||||||
|
- 全程用中文；保留工作区已有修改。
|
||||||
|
- 测试选择遵循 [TESTING.md](TESTING.md)。日常运行受影响用例及必要契约，常规验证目标 60 秒内。
|
||||||
|
- 不默认运行全量 pytest、colcon test 或所有型号整手拟合。公共契约变更与发布节点再扩大范围。
|
||||||
|
- 同一代码版本已通过的测试不无理由重复执行；修复失败后只重跑失败或受修改影响的用例，并记录耗时。
|
||||||
|
- 优先复用不可变输入及已生成产物，在验收边界测试拒绝条件；不为每种错误重复生成整手标定。
|
||||||
|
- 软件回放通过与实机连续通过分开报告；不能为缩短测试或减少暂停放宽精度、可观测性和发布门限。
|
||||||
@@ -1,169 +0,0 @@
|
|||||||
# 统一标定需求代码审查（2026-09-10）
|
|
||||||
|
|
||||||
## 结论与审查范围
|
|
||||||
|
|
||||||
审查当前工作区（包含未提交的统一重构、精简 JSON v2 和本会话启动参数修复），
|
|
||||||
不是只审查 Git HEAD，也没有把历史 PASS 当作当前实机验收。
|
|
||||||
通用采集、拟合、URDF 修正、最终文件验收和发布架构已经存在，但仍有迁移残留和精度验证缺口。
|
|
||||||
本轮修正可由代码及隔离测试确认的问题，没有改变原始 CAD、机械限位、SDK、贴 Tag 配置或相机外参。
|
|
||||||
没有启动 SDK、相机或发送机械手运动指令。
|
|
||||||
|
|
||||||
## 已修复的问题
|
|
||||||
|
|
||||||
| 问题 | 影响 | 修改 |
|
|
||||||
| --- | --- | --- |
|
|
||||||
| `vendor_sdk_config:=` 等空 launch 实参 | O6/L6/G20 可能在节点启动前退出 | runner 省略空参数，使用 ROS CLI 的实际解析器覆盖四型号 |
|
|
||||||
| 在线 IO 使用固定 front/side/top 外参加载器 | 新 Profile 即使静态校验通过，在线仍可能不能使用其他机位 | 直接用通用外参加载器，视图和参考机位来自 Profile |
|
|
||||||
| launch 固定三机位、型号文件名回退和过期参数 | 新型号需修改启动代码；部分开关没有实际作用 | 按 Profile 创建机位；要求受保护 Profile/原始 URDF/Tag 路径；移除旧诊断、速度、scope 和输出目录覆盖参数 |
|
|
||||||
| MVS 无图像时只显示“等待设备” | 操作者无法区分 SDK、相机或检测链路故障 | 统一显示每机位有效内参/检测消息等待项、SDK 条件和内参不匹配原因 |
|
|
||||||
| READY 后设备断流仍保留可开始状态 | 启动请求可能使用已失效的就绪条件 | 开始前可撤销 READY；Start 再次检查完整设备条件 |
|
|
||||||
| 实时 CameraInfo 只比较 P 的部分字段 | K、D、R 改变但 P 未变时，整流图可能与原外参不一致 | 实时校验宽高及 K/D/R/P 指纹；开始后不匹配按原有坐标变化故障停止继续采集 |
|
|
||||||
| 每个稳态采样点清空整方向统计 | 显示有效帧和覆盖率归零，误导进度判断 | 分离方向数据与当前稳态点数据，重扫/换方向才重置方向统计 |
|
|
||||||
| 标题只显示序列号 | 序列号不含型号时无法确认当前型号 | 所有型号统一显示 model、side、serial_number |
|
|
||||||
| 正式 finalizer 仍允许旧格式绕过指令拟合 | 新型号配置旧版本可能没有所需的 SDK 指令→rad 产物 | 正式 Profile 与 finalizer 均只接受统一输出版本 2，取消旧输出分派 |
|
|
||||||
| 完成回调读取精简 JSON 已删除的 `quality` | 文件可能已经发布，但节点在结束时抛 KeyError | 删除没有消费者的 `final_quality` 赋值，增加无 quality 字段的完成回调回归 |
|
|
||||||
| 视觉暂停与控制定时器之间的状态竞争 | 定时器等待锁期间已暂停，取得锁后仍可能继续动作 | 在控制锁内再次检查终止状态 |
|
|
||||||
|
|
||||||
开始前等待相机消息的 2 秒窗口不用于扫描中的实时暂停；空检测数组算视觉链路存活。
|
|
||||||
短时 Tag 丢失、低检测率等仍按方向结束的数据质量策略处理，同速重扫一次后仍不足才暂停。
|
|
||||||
|
|
||||||
清理的在线残留包括无人读取的回调计数、命令频率缓存/方法、型号质量标签、旧 tracker reset hook，
|
|
||||||
以及每次反馈回调不必要构造的备用 Adapter。没有删除 SDK 协议接口的抽象方法。
|
|
||||||
|
|
||||||
五个过渡期写出器已移出生产包并保留原文件：
|
|
||||||
`calibration_output/code_review_retired_serializers_umhxlepj/`。
|
|
||||||
包括 `select.py`、`generic_v1.py`、`legacy_v4.py`、`legacy_v6.py`、`native_v7.py`。
|
|
||||||
历史读取器和历史诊断工具仍在；此归档不包含或改写用户的历史标定产物。
|
|
||||||
|
|
||||||
## 视频复核后的补充修复：共用姿态分支筛选
|
|
||||||
|
|
||||||
视频复核发现旧配置把 `pnp_reprojection_tie_px` 和姿态拒绝阈值均设为 `1.5 px`，
|
|
||||||
导致通过质量门槛的 IPPE 候选都参加时间连续性比较，旧镜像分支可能压过明确更好的图像拟合。
|
|
||||||
用现有候选姿态复现时，旧分支 `0.24 px` 会压过另一分支 `0.05 px`。
|
|
||||||
|
|
||||||
公共模块统一默认近似同误差阈值为 `0.03 px`；ROS 与离线采集继承该值。
|
|
||||||
五份旧 YAML 的重复覆盖已移除，四产品的 `calibration_config_sha256` 已同步更新。
|
|
||||||
新增校验拒绝非有限、负值或不小于图像拒绝阈值的分支容差，防止旧的 `1.5/1.5` 配置再次生效。
|
|
||||||
保持实际近似同误差时的连续性选择、原有倾角/图像质量限制和方向结束重扫策略。
|
|
||||||
因此前文“配置保持不变”仅描述首轮结构审查；这次补充修复修改了上述算法配置及其哈希。
|
|
||||||
|
|
||||||
覆盖五份配置和四型号默认采集的九个复现用例，在修复前全部失败，修复后全部通过。
|
|
||||||
第一组姿态、配置、采集与加载实际参数文件的四型号隔离 ROS host 检查共 51 项通过。
|
|
||||||
第二组公共采集/拟合与发布、O12 姿态解析、来源保护、四型号配置及启动检查共 141 项通过，
|
|
||||||
耗时 114.53 秒。两组为本次修改的定向回归，没有宣称本次又执行过全包测试。
|
|
||||||
本次 `colcon build --packages-select linkerhand_calibration --symlink-install` 通过；
|
|
||||||
安装环境读取四产品配置、校验受保护哈希及公共默认值均通过，`git diff --check` 通过。
|
|
||||||
这不代表所有平面双解已消除，也不替代下述相机时序和实机重复性验证。
|
|
||||||
|
|
||||||
## 仍不能由本轮软件审查保证的事项
|
|
||||||
|
|
||||||
### P1：真实采集时间与传输积压
|
|
||||||
|
|
||||||
`hikrobot_camera.py::_publish_frame` 使用主机取到帧时的 ROS 时间戳，未将设备曝光时间映射到统一时钟。
|
|
||||||
SDK 帧结构有设备时间戳和帧号，但设备时钟单位、偏移、漂移与主机时钟同步还没有在当前硬件验证。
|
|
||||||
`runtime/ros/io.py::_state_callback` 对无时间戳反馈也采用接收时间。
|
|
||||||
因此，USB/驱动缓存可能把旧图像与新反馈配对；50 ms 的消息时间戳检查和“只取最新帧”不能排除该问题。
|
|
||||||
本会话已经实测到三台相机共享 480 Mbps Hub 上行，且正面/上方取帧超时。
|
|
||||||
|
|
||||||
后续应在恢复 USB 3.x 链路后，核验设备时间戳、帧号、曝光与反馈时序，并用独立运动数据检查配对误差。
|
|
||||||
不能猜测设备时钟单位、直接给时间戳减一个常数，或放宽质量阈值作为修复。
|
|
||||||
|
|
||||||
### P1：重复性和实机/仿真验收尚未完成
|
|
||||||
|
|
||||||
前三轮训练、第四轮独立验证、冻结 Tag 安装和双文件重读校验已接入。
|
|
||||||
但同手多次独立重采、重新摆放和重新贴 Tag、不同保持姿态/多关节组合动作的实际验收仍缺证据。
|
|
||||||
L6/O6 部分关节由小指参数迁移，不属于所有关节独立实测。
|
|
||||||
发布报告仍应如实保留 `arbitrary_multiaxis_validated: false`。
|
|
||||||
|
|
||||||
外部仿真要加载本次修正 URDF，用同一 SDK 指令话题及配套 JSON 驱动；用实机反馈直接驱动仿真，
|
|
||||||
只能显示反馈对应姿态，不能证明指令映射正确。桥本身是消息转换器，不是独立精度测量工具。
|
|
||||||
|
|
||||||
### P2：数学模块和旧配置仍有维护负担
|
|
||||||
|
|
||||||
`core/fitting/spatial.py` 约 3,656 行，包含轴线拟合、基座位姿、零位优化、可观测性和验证，
|
|
||||||
其中 `solve_urdf_zero_offsets` 从约第 1,921 行开始，嵌套函数及共享局部状态较多。
|
|
||||||
它是当前生产算法，不是可直接删除的废代码。后续拆分应按这些数学职责进行，
|
|
||||||
用固定输入的参数、残差、失败原因和产物回归验证等价性，不应复制成各型号独立算法。
|
|
||||||
|
|
||||||
当前约 58 个兼容模块仍用于旧导入路径、历史回放/迁移工具和旧回归；不能仅因目录名称或静态引用少就删除。
|
|
||||||
几行的公开兼容导出不是第二套拟合算法。旧型号诊断、旧 URDF plan 和部分旧 YAML 参数仍需明确退休范围后继续收缩。
|
|
||||||
受保护的历史配置在本轮保持不变；旧 YAML 中一些已不被在线节点读取的参数，不应当作当前算法的生效开关。
|
|
||||||
|
|
||||||
原始 G20/O12 的 mimic/限位冲突和 CAD 保留策略仍需机械资料确认；当前通用 URDF 修正
|
|
||||||
主要处理零位、限位和线性 mimic，不估计连杆长度、轴位置、mesh 或惯量。
|
|
||||||
|
|
||||||
## 按需求核对
|
|
||||||
|
|
||||||
| 需求 | 当前判断 |
|
|
||||||
| --- | --- |
|
|
||||||
| 多型号共用拟合和 URDF 修正 | 已实现统一生产链；SDK 新协议仍需 Adapter 和 ROS 启动绑定 |
|
|
||||||
| 新型号只关心 SDK、贴 Tag、避让 | 基本成立，但必须声明关节绑定、可观测性、迁移/保留策略和可修改字段，不能从未知安装的单轴 Tag 自动猜出 CAD 零位 |
|
|
||||||
| 统一 SDK 指令→rad JSON 与修正 URDF | 正式仅生成统一 v2 指令表；u8 为 256 项，rad 为显式节点；被动表由标准 mimic 推导并验收，详细证据保留在报告 |
|
|
||||||
| 开始前可移动、开始后固定 | 每次 Start 丢弃预览参考，正式锁定；可见固定基准有漂移监测；遮挡对象是否移动不能实时保证 |
|
|
||||||
| 正确、可重复 | 有独立验证和来源保护，但仍受采集时序与未完成实机验收限制 |
|
|
||||||
| 少暂停且原因明确 | 统一策略保留；开始前明确缺失设备，不用短时视觉丢失实时打断扫描 |
|
|
||||||
| 统一进度且明确型号 | 已修正型号、设备等待原因及方向统计显示 |
|
|
||||||
| 同源指令实机/仿真验证 | 有统一转换桥；实际动态/组合动作对比尚未完成 |
|
|
||||||
|
|
||||||
## 本轮验证
|
|
||||||
|
|
||||||
- 审查中完整标定包回归：**544 通过、1 跳过，383.25 秒**。跳过项要求显式提供
|
|
||||||
`O12_REPLAY_RAW` / `O12_REPLAY_REFERENCE`，没有用合成数据替代历史实测。
|
|
||||||
- 随后补充的启动清理、单一正式输出及归档改动：启动/runner/正式 Profile 拟合/精简产物定向组
|
|
||||||
**74 通过，108.78 秒**，包含四型号与虚拟型号的实际公共拟合和发布路径。
|
|
||||||
- 最后完成回调、暂停竞争、ROS host、等待诊断、实时内参保护：**25 通过，0.98 秒**。
|
|
||||||
与上一组有重叠，不相加冒充一次全量结果；没有宣称最后所有变更又跑过一次全量。
|
|
||||||
- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。
|
|
||||||
安装入口读取四产品的受保护配置、ROS launch 实参解析均通过；167 个生产/兼容 Python 文件 AST 通过；
|
|
||||||
`git diff --check` 通过。
|
|
||||||
- 新 launch 测试实际构造四型号及改名机位的 launch actions，但不执行这些动作。
|
|
||||||
隔离 ROS 节点/标准加载器测试不启动 SDK 或相机，不代表实机验收。
|
|
||||||
|
|
||||||
## 数学核心与 ROS 组合重构的验收记录
|
|
||||||
|
|
||||||
本轮以包含公共 `0.03 px` 姿态分支修复的工作区为基线，只拆分职责、明确数据和并发边界。
|
|
||||||
没有删除基线中仍存在的历史入口，没有修改配置、拟合公式、优化初值/顺序、残差权重或验收阈值。
|
|
||||||
|
|
||||||
- `core/fitting/spatial.py` 保留 45 个原定义符号的显式导出；实际计算在 `spatial_solver/`。
|
|
||||||
`solve.py` 的入口顺序为输入准备、训练求解、统计、独立验证及结果装配。
|
|
||||||
数据通过 `ZeroProblem`、`TrainingProblem`、`TrainingFit` 和各验收结果类型传递。
|
|
||||||
训练接口只接收训练观测，训练几何中的掌部姿态按训练轮筛选。
|
|
||||||
- `core/geometry/pnp.py` 保留 14 个原定义符号；实现拆为 IPPE、单 Tag 跟踪、刚性组和轨迹选择。
|
|
||||||
跟踪默认值集中在 `tag_pose/parameters.py`，ROS 参数只负责加载和 deg/rad 转换。
|
|
||||||
- `UnifiedCalibrationNode` 直接继承 ROS `Node`，组合消息 IO 与 `CalibrationCoordinator`。
|
|
||||||
协调器不加载 ROS 库，复用 `SessionExecution`、采集、运动、固定基准和断点组件。
|
|
||||||
运行参数、观测输入和快照使用明确类型；阶段只有 `CalibrationSession.phase` 一个来源。
|
|
||||||
- `FinalizationController` 组合原 worker 与发布器，协调器接收阶段事件并授权提交。
|
|
||||||
状态锁先于 worker 锁；PnP 在状态锁外计算，提交时检查会话版本、采集/运动对象及稳态边界。
|
|
||||||
暂停、中止与提交互斥；失败不触发自动恢复运动，成功只提交一次。
|
|
||||||
SDK 绑定显式接收反馈新鲜度、时钟、健康订阅和发布接口。
|
|
||||||
|
|
||||||
固定输入记录在本次工作区快照 `/tmp/calibration_structure_baseline_30qf3ozo/`。
|
|
||||||
持久化的对比报告及压缩前后输出位于 `calibration_output/structure_refactor_20260910/`，
|
|
||||||
其中 `comparison.json` 记录输入摘要、差异、产物摘要和拒绝原因，`protected_inputs.json` 保存输入哈希。
|
|
||||||
这些记录是合成数据的结构等价证据，不是实机精度验收。
|
|
||||||
|
|
||||||
| 固定输入 | 比较浮点值数量 | 最大绝对差异 | JSON/拟合结果/阶段事件及 URDF 文本 |
|
|
||||||
| --- | ---: | ---: | --- |
|
|
||||||
| G20 | 46,046 | 0 | 一致 |
|
|
||||||
| L6 | 15,304 | 0 | 一致 |
|
|
||||||
| O6 | 15,304 | 0 | 一致 |
|
|
||||||
| O12 | 14,494 | 0 | 一致 |
|
|
||||||
| 虚拟型号（任务重排） | 15,304 | 0 | 一致 |
|
|
||||||
|
|
||||||
对五组输入分别移除第四轮或注入第四轮 Tag 姿态滑移，10 个拒绝原因和阶段事件与基线一致。
|
|
||||||
167 个配置、Profile、原始 URDF/资源文件的哈希不变。
|
|
||||||
另一个虚拟型号同时重排 SDK 通道、机位名称和扫描任务，经过真实公共采集、finalizer、
|
|
||||||
最终 JSON/URDF 验收和发布，再检查同一 SDK 指令经过模拟传输、查表和标准 mimic 图的一致性。
|
|
||||||
|
|
||||||
验证结果：
|
|
||||||
|
|
||||||
- 整个标定包：581 passed，1 skipped，337.43 s。
|
|
||||||
- SDK 绑定时钟补充后，协调器、四型号 ROS 构造和安全策略回归：27 passed。
|
|
||||||
- 跳过项是 `test_o12_recorded_replay.py`：未提供 `O12_REPLAY_RAW` 与独立参考模型；不计作实机通过。
|
|
||||||
- 改动模块通过 Pyflakes，`git diff --check` 通过。
|
|
||||||
- `colcon build --packages-select linkerhand_calibration --symlink-install` 成功。
|
|
||||||
- 安装后的 12 个 console entry 可导入；两个产品 CLI 的 `--help` 正常。
|
|
||||||
两个 ROS 节点入口均通过四型号受保护 Profile 的转交检查；安装环境下四型号 ROS 构造/Start 回归 4 passed。
|
|
||||||
|
|
||||||
本轮不处理相机采集时刻同步，也不替代实机多次标定和同话题实机/仿真运动对照。
|
|
||||||
@@ -0,0 +1,71 @@
|
|||||||
|
# O30 右手标定稳定性与产物正确性审查
|
||||||
|
|
||||||
|
本次只分析并修改软件，没有启动 SDK、相机或机械手运动。实机指令下发后反馈不变的问题按硬件问题排除。原始日志、断点、Profile、保护输入哈希及运动顺序均未改写。
|
||||||
|
|
||||||
|
## 结论
|
||||||
|
|
||||||
|
流程确有软件层面的稳定性缺陷，不能把历次暂停全部归因于标签或硬件。本次修复准备观测准入、短暂缺帧的恢复分类，并统一在线与离线的完整采集验收入口。已有的姿态可观测性、独立留出验证及 JSON/URDF 回读要求继续保留。
|
||||||
|
|
||||||
|
“少暂停”与“结果正确”需要分别处理：短暂坏帧应在采集层过滤或使用有次数上限的重试；真正无法区分的三维姿态、移动的固定参考、改变的标签安装或不完整数据，应明确拒绝生成通过产物。相同机位重复同一动作并不一定能提供区分姿态所需的新信息。
|
||||||
|
|
||||||
|
## 已定位并修改的问题
|
||||||
|
|
||||||
|
| 问题 | 原有行为及影响 | 本次修改 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| 无效观测覆盖有效准备帧 | 每个指令区间保存最后一帧；后来的空候选或不合法候选可以覆盖先前有效帧，随后整个运动求解被拒绝 | 在写入区间缓存前，复用求解器已有的候选质量检查；图像求解所需的根参考必须已冻结。只有完整有效帧才能替换缓存，原始坏帧仍保存在日志中 |
|
||||||
|
| 缺帧错误未进入重试策略 | 底层返回 `insufficient_image_frames` 或 `motion_candidates_missing_or_invalid`，已有恢复策略却只识别另一部分错误名称 | 将这些暂态错误纳入同一个恢复策略，每个尚未建立零位的任务/关节组最多重新执行一次准备动作；持续失败仍暂停 |
|
||||||
|
| 在线与离线的收尾检查边界不一致 | 离线入口检查完整扫描单元，在线直接进入最终拟合；若恢复数据只有旧通过标记或最新尝试不完整，缺少同一个入口检查 | 抽出 `runtime/artifacts/capture_validation.py`，最终生成入口统一检查配置身份、固定参考、相机内参、每个单元最新尝试及实际样本质量；两种产品入口均要求采集证据 |
|
||||||
|
| 大日志的重复扫描和额外内存 | 每个单元都遍历完整日志；离线计算日志哈希时额外一次性读取整个文件 | 按单元、任务各建一次索引，复用原有质量规则；日志哈希改为流式计算 |
|
||||||
|
|
||||||
|
候选准入只使用预先规定的检测质量条件，不根据拟合后的误差选择保留帧；保留的两种候选均进入原有比较，不填补不存在的另一种候选，也不改变最终精度阈值。准备记录增加 `preparation_observation_rejections`，可查到被拒绝观测的原因与数量。
|
||||||
|
|
||||||
|
若一个最新尝试同时存在通过与失败标记，拒绝通过；若只有较早尝试完成、较新尝试未完成，也拒绝通过。失败原因写入 `capture_validation.json`，并在拟合前返回，不能更新 `latest_passed`。
|
||||||
|
|
||||||
|
## 真实日志验证
|
||||||
|
|
||||||
|
### 无名指 MCP 准备阶段
|
||||||
|
|
||||||
|
来源：`calibration_output/O30_RIGHT_001/20260919_204235/raw_samples.jsonl`，侧面机位、运动版本 159。
|
||||||
|
|
||||||
|
- 原报告选择了 105 帧，其中 8 个区间的最终帧存在空候选，整体报 `motion_candidates_missing_or_invalid`。
|
||||||
|
- 从原日志抽取该段 430 条侧面观测，保存为 `test/fixtures/o30_ring_preparation_admission.json.gz`；不修改其角点、指令、反馈或候选。
|
||||||
|
- 新准入规则拒绝 14 条不合格候选观测，保留 97 个有效运动区间，求解不再因整段混入空候选而失败。
|
||||||
|
- 这段历史数据还存在候选族不完整和图像重投影失败，最终仍未授权模型。该验证证明准入缺陷已消除，不能用来宣称这段实机标定已经通过。
|
||||||
|
|
||||||
|
### 已保存的 96 个单元
|
||||||
|
|
||||||
|
来源：`calibration_output/O30_RIGHT_001/20260919_214048/raw_samples.jsonl`。
|
||||||
|
|
||||||
|
- 保存的会话具有唯一启动记录、唯一固定参考和三个相机模型；与当前产品的保护输入哈希及采集策略兼容。
|
||||||
|
- 离线提取日志中实际用于覆盖率和稳态质量检查的字段，执行相同单元验收：前 96 个单元通过，第 97 个 `middle_dip_side / cycle=0 / increasing` 因未采集而拒绝；没有将历史断点视为完整产物。
|
||||||
|
- 该检查不拟合历史图像，不修改断点；不是完整运动来源与最终图像验收的替代品。
|
||||||
|
- 正式断点启动仍使用 `passed` 恢复，直接复用通过单元。整套数据的完整检查放在最终生成阶段，不加入恢复启动路径。
|
||||||
|
|
||||||
|
## 目前合理且继续保留的边界
|
||||||
|
|
||||||
|
1. 几何求解与当前零位图像采集分别计时。已有实现会在参考复用后开启独立采集窗口，避免求解或加载耗时吃掉零位采样时间；此次未重复修改这一已修复逻辑。
|
||||||
|
2. 已冻结零位不能被局部重试替换；已成功安装部分机位模型、保持关节发生移动或固定参考冲突时，不自动重走准备动作。
|
||||||
|
3. 三维候选族无法区分时仍不授权。之前对正面、侧面共享标签、测得的父关节几何及其不确定度的修正继续保留；没有把父几何当作绝对真值。
|
||||||
|
4. 运动顺序仍是拇指、四指屈伸、四指侧摆。恢复不回到整手起点；本次没有增加未声明的辅助动作。
|
||||||
|
|
||||||
|
## JSON 与修正 URDF 的正确性检查
|
||||||
|
|
||||||
|
正式产物必须依次通过：
|
||||||
|
|
||||||
|
1. 保护输入、相机和固定参考身份检查；所有应采集单元的最新尝试完成，样本覆盖率、稳态节点和训练/验证输入范围合格。
|
||||||
|
2. 零位、运动模型、跨会话来源及共享标签证据一致；训练数据与独立验证图像不重叠。
|
||||||
|
3. 方向分别拟合的指令映射和零位通过独立留出检查，不能用训练拟合误差替代验收。
|
||||||
|
4. 先写标定 JSON，再从落盘 JSON 重建修正 URDF，回读核对两份文件对应关系。
|
||||||
|
5. 检查授权修改范围、运动学、角度和最终文件对应的独立像素重投影；标准 ROS URDF 加载检查通过后才允许发布。
|
||||||
|
6. 发布控制器复核候选文件哈希和取消状态，最后更新通过指针。采集完整性检查通过不等于整套产物已通过。
|
||||||
|
|
||||||
|
O30 当前契约有 20 个独立主动关节；末节 DIP/IP 的五个 CAD 零位假设必须在产物中明确保留其来源，不能写成五个已经独立实测的绝对零位。本次不改变这一契约。要声称这些绝对零位也经过实测，还需要相应独立观测。
|
||||||
|
|
||||||
|
## 软件验证范围
|
||||||
|
|
||||||
|
- 新增回归覆盖坏帧覆盖、全部坏帧、未冻结根参考、保留两种候选、真实无名指准备数据，以及在线/离线收尾对失败单元、未完成新尝试、配置和内参变化的拒绝。
|
||||||
|
- 恢复回归覆盖实际底层缺帧错误、一次重试后成功和持续失败后暂停；继续检查已通过单元复用、异步求解和零位窗口。
|
||||||
|
- 产物回归覆盖 O30 20 通道非线性映射、独立角点验证、JSON 重建 URDF、授权字段约束、进程收尾与发布取消。
|
||||||
|
- 共 198 项不同的相关回归测试通过，其中新增 18 项用例/参数组合。首批 28 项通过，综合批次 181 项通过（504.86 秒），去除重叠后为 198 项；最终收尾检查修改后又单独复跑 20 项，全部通过。Python 编译与 `git diff --check` 通过。
|
||||||
|
- 软件回归及合成数据验收不代表实机精度已达标；当前仍没有完成整手最终产物验收。
|
||||||
|
- 安装环境解析到当前源码目录，新增的共享验收模块可直接导入；没有启动实机程序验证。
|
||||||
@@ -0,0 +1,111 @@
|
|||||||
|
# O30 自动生成链路修复与实采验证
|
||||||
|
|
||||||
|
2026-09-23。本次实际修改生产代码并只读回放两批实采数据，未驱动硬件。
|
||||||
|
采集与离线求解已解耦，行程端点约束及 JSON→URDF 导出链已修复；当前生成的是
|
||||||
|
**未通过实测精度认证的候选**，不能作为正式正确版本发布。
|
||||||
|
修复前分析见 [O30_CURRENT_FLOW_REVIEW.md](O30_CURRENT_FLOW_REVIEW.md)。
|
||||||
|
剩余误差已按机位、标签和指令进一步定位，见
|
||||||
|
[O30_REMAINING_ISSUES.md](O30_REMAINING_ISSUES.md)。
|
||||||
|
|
||||||
|
## 1. 收敛后的流程
|
||||||
|
|
||||||
|
1. 启动前保存本次有效 Product、Profile、CAD URDF/mesh、内外参及标签配置。
|
||||||
|
默认配置同步已确认的四指 SDK 方向、中节标签绑定和新外参；历史会话仍按自身
|
||||||
|
快照及外参回放，不能将新外参套在旧图像上。
|
||||||
|
2. 保留既有路线、速度、2＋1 分区、108 方向、一次局部补采及停滞重试保护。
|
||||||
|
采集完成先归位封存，关闭 SDK、相机、监控及 ROS 控制上下文，再写采集结果。
|
||||||
|
完整返回 0、缺项返回 3；均不进入拟合，也不因离线失败重采。
|
||||||
|
3. 使用会话 `capture_product.yaml` 和 `raw_samples.jsonl` 单独离线拟合。
|
||||||
|
原始文件与配置、mesh 均受清单哈希保护。验证轮 3 不参与参数优化或候选选择。
|
||||||
|
4. 先校验草稿 JSON，再读回 JSON 生成 URDF；共用一个导出函数，检查实际输出文件
|
||||||
|
FK 与标准加载。独立报告 JSON 双向曲线和纯 URDF 上下限线性驱动的精度。
|
||||||
|
5. 草稿允许保留失败证据用于诊断，精度失败不会更新 `latest_passed`。
|
||||||
|
|
||||||
|
普通单批离线入口及参数见 [README.md](README.md#低精度离线修正草稿o30)。
|
||||||
|
本次跨批验证脚本保存在仓库根目录下
|
||||||
|
`calibration_output/software_fix_20260923_auto_urdf_v2/replay.py`。
|
||||||
|
它调用包内拟合、范围证据、导出和验收函数，不另写一套 URDF 生成逻辑。
|
||||||
|
脚本对输入和实现哈希有缓存约束；代码变化后不能直接复用旧参数冒充重新验证。
|
||||||
|
|
||||||
|
## 2. 行程问题的修复
|
||||||
|
|
||||||
|
换向处通常只保存到达端点的方向标签；离开端点的另一个方向没有独立端点观测。
|
||||||
|
旧模型仍给两个方向分别分配端点自由参数，缺少直接约束的端点可能保留初值，
|
||||||
|
或由邻近指令外推，并进入最终 URDF 极值。这会使文件可加载而行程依据不足。
|
||||||
|
|
||||||
|
新模型对这类端点施加明确的换向连续性约束：有观测的到达端点同时决定未观测的
|
||||||
|
离开端点。如果两方向均有端点观测，则保留独立参数；若连来源端点都缺失，拒绝
|
||||||
|
导出该行程。该假设不等同于两次独立测量，使用
|
||||||
|
`observed_turnaround_continuity_v1` 明确记录。
|
||||||
|
|
||||||
|
优化只移除声明为从属的参数，其他秩亏仍拒绝；图像 Jacobian、角度导数和范围
|
||||||
|
不确定度均沿同一依赖传播，避免只修改导出数字。证据记录训练样本、轮次和方向，
|
||||||
|
JSON 回读检查端点连续性及来源。测试确认任意扰动从属参数不改变残差或行程。
|
||||||
|
|
||||||
|
普通 URDF 的上下限只表示可动范围，不能编码 SDK 指令对应的非线性、双向曲线。
|
||||||
|
因此增加 `draft_urdf_linear_holdout.json`，专门检查
|
||||||
|
`q = lower + (upper - lower) × u / 255`（按 SDK 方向处理）的实际误差。
|
||||||
|
曲线验证通过不能被当成该线性驱动方式通过；零位只应用一次，非零指令基准也不
|
||||||
|
强行当作线性驱动的零角度。
|
||||||
|
|
||||||
|
## 3. 两批真实数据的结果
|
||||||
|
|
||||||
|
| 数据 | 原始方向完成量 | 训练/验证帧数 | 用途 |
|
||||||
|
| --- | ---: | ---: | --- |
|
||||||
|
| 旧外参 `20260923_142201` | 107/108 | 4769 / 2681 | 重新拟合全手参考 |
|
||||||
|
| 新外参 `20260923_183138` | 24/24 | 922 / 516 | 固定旧四指参考后更新拇指 |
|
||||||
|
|
||||||
|
仅取封存通过的最后尝试及有效稳态帧，不拼接失败尝试。旧数据缺第 1 训练轮
|
||||||
|
`fingers_mcp_roll_front / increasing / linked_increasing`，不能由验证轮补齐。
|
||||||
|
旧四指侧摆保留此前确认的竖直定义基准，四项在 JSON 标为用户定义；五个末端
|
||||||
|
零位保留 CAD。不能将这九项说成此次新独立测量的零位。
|
||||||
|
跨批旧参考仍是条件参考，其系统误差没有完整传播，不能据此正式认证新拇指。
|
||||||
|
|
||||||
|
两个训练阶段均在原有预算内收敛，先冻结参数再验证。原始数据哈希保持不变。
|
||||||
|
最终候选位于：
|
||||||
|
|
||||||
|
- `calibration_output/software_fix_20260923_auto_urdf_v2/artifacts/o30_right_draft.urdf`
|
||||||
|
- `calibration_output/software_fix_20260923_auto_urdf_v2/artifacts/draft_calibration.json`
|
||||||
|
|
||||||
|
同目录 mesh 自包含；JSON 重建 URDF 字节一致，SHA256 为
|
||||||
|
`ecb36313311292cb1ebe37d8069ba18234ac1dd6e5ccda39902d10e74ed862ac`。
|
||||||
|
最终文件 FK 最大差 `6.66e-16`，ROS 标准加载及 MuJoCo 20 关节加载/前向计算通过。
|
||||||
|
根目录另有早期诊断文件，本次最终路径以 `result.json` 的 `artifacts/` 路径为准。
|
||||||
|
|
||||||
|
以下角度误差来自冻结模型的独立验证轮，单位为度；测量本身未通过全部图像及
|
||||||
|
不确定度门限，所以它们用于定位问题，不是物理真值精度证书。
|
||||||
|
|
||||||
|
| 拇指关节 | 生成上限 | 曲线最大角度误差 | 纯 URDF 线性最大误差 |
|
||||||
|
| --- | ---: | ---: | ---: |
|
||||||
|
| CMC roll | 52.916 | 0.837 | 0.910 |
|
||||||
|
| CMC yaw | 110.843 | 1.549 | 3.483 |
|
||||||
|
| MCP | 105.135 | 0.748 | 1.325 |
|
||||||
|
| IP | 85.484 | 1.066 | 2.625 |
|
||||||
|
|
||||||
|
当前不能发布的具体原因：
|
||||||
|
|
||||||
|
- 新拇指曲线角度子项通过，但独立角度测量最大重投影误差仍为 7.194 px，最大
|
||||||
|
3σ 不确定度约 1.733°，未通过原门限，不能只报告角度子项通过。
|
||||||
|
- 纯 URDF 线性驱动下 yaw 最大误差 3.483°；IP 的平均/95 分位误差为
|
||||||
|
1.557°/2.571°，这两个关节的角度子项失败。
|
||||||
|
- 旧四指曲线测量最大重投影误差 16.468 px、最大 3σ 约 8.713°；线性驱动验证
|
||||||
|
的独立角度求解未收敛，保留 `raw_joint_holdout_angle_measurement_failed`。
|
||||||
|
- 旧采集不完整，且跨批条件参考尚不满足正式认证。
|
||||||
|
|
||||||
|
下一步应先用已封存图像定位失败机位/标签的重投影残差和参考不确定度，验证
|
||||||
|
标签安装、相机几何及观测条件。只有数据证明需要时，再决定补采范围或更改模型。
|
||||||
|
需要保持 SDK 字节轨迹时，运行端应使用经验证的 JSON 曲线得到弧度并驱动 URDF；
|
||||||
|
若产品要求单靠上下限线性驱动，则必须以该独立报告为准，不能只扩大上限消除表象。
|
||||||
|
|
||||||
|
## 4. 软件回归与证据
|
||||||
|
|
||||||
|
采集/局部/重装组 43 项、离线导出/端点组 43 项、更新审计组 3 项，以及真实
|
||||||
|
coordinator 的完整 108 方向虚拟时钟集成 1 项通过。最新配置及 mesh 封存修改后，
|
||||||
|
对应 4 项边界检查再次通过；它们与前述用例重叠，不重复累计。
|
||||||
|
完整调度用例耗时约 97 秒，其余组分别约 17、34、0.3 和 2.4 秒。
|
||||||
|
软件调度回归不代表本轮实机连续采集通过。
|
||||||
|
|
||||||
|
产物目录的 `verification.json`、`mujoco_acceptance.json`、`curve_holdout.json`、
|
||||||
|
`linear_holdout.json`、`training_frozen.json`、`result.json` 及 `test_evidence/`
|
||||||
|
分别保存来源核验、加载、精度、冻结输入和软件测试记录。
|
||||||
|
本次未放宽精度/可观测性门限，未替换仿真默认模型，未发布正式版本。
|
||||||
@@ -0,0 +1,180 @@
|
|||||||
|
# O30 当前采集与自动生成链路复核
|
||||||
|
|
||||||
|
本文为修复前的只读复核记录；后续实际改动、两批数据重拟合及验收结论见
|
||||||
|
[O30_AUTOMATIC_URDF_FIX.md](O30_AUTOMATIC_URDF_FIX.md)。
|
||||||
|
|
||||||
|
2026-09-23。目标是完全由程序从采集数据生成正确、可复现的 JSON 和修正 URDF。
|
||||||
|
本次评估程序生成版，不用人工调整版作为拟合结果或精度依据。
|
||||||
|
|
||||||
|
结论：轻量采集、离线求解的方向合理，但目前还不能把“采完”或“草稿能加载”视为
|
||||||
|
“自动生成正确 URDF”。优先收敛采集边界和配置，再修复行程证据约束，最后用冻结
|
||||||
|
数据验证实际驱动方式。当前没有证据支持靠增大拇指上限或增加优化迭代解决全部偏差。
|
||||||
|
|
||||||
|
本轮只读复核代码和已有真实数据，运行针对性软件测试；未运动硬件、未重新拟合、
|
||||||
|
未修改生产代码/配置或替换模型。已有工作区改动和被删除的历史测试均保留。
|
||||||
|
|
||||||
|
## 1. 当前流程与合理之处
|
||||||
|
|
||||||
|
实际入口：`runtime/runner.py`；默认 `raw_joint_2_plus_1` / `raw_joint_images_v2`。
|
||||||
|
|
||||||
|
1. 启动检查、固定参考锁定、归基准；17 个任务覆盖 20 个关节。
|
||||||
|
2. 每任务训练轮 0/1、独立验证轮 3，共 108 个方向单元。
|
||||||
|
3. 移动标签在线只记录角点；保持反馈、同步、固定参考及硬件保护。质量不足的方向
|
||||||
|
立即补一次，不拼接失败尝试；恢复保留预算。硬件停滞重试与图像质量补采分别处理。
|
||||||
|
4. 归位、封存原始数据，状态为 `CAPTURE_COMPLETE`，随后关闭 SDK/相机/监控。
|
||||||
|
5. **启动器实际上还会自动调用草稿拟合**；显式 `--offline-raw` 则可单独离线运行。
|
||||||
|
|
||||||
|
值得保留：在线不依赖移动标签几何消歧；端点按已发送指令核验，不要求反馈恰好
|
||||||
|
达到 0/255；离线只取同步、有效稳态帧；验证轮不用于训练；原始证据封存；正式
|
||||||
|
发布保持来源、候选不确定度、独立验证及 JSON→URDF 回读门禁。
|
||||||
|
|
||||||
|
拟合固定 CAD 拓扑、连杆和关节轴，联合估计掌坐标、标签安装、关节零位和双向
|
||||||
|
分段指令曲线。15 个零位可估，五个末端零位沿用 CAD 假设；显式竖直侧摆草稿
|
||||||
|
另把四个侧摆零位设为用户定义基准，因此该草稿实际只估 11 个零位。
|
||||||
|
这些假设必须继续写进产物，不能改称全部零位都由视觉独立测得。
|
||||||
|
|
||||||
|
## 2. 优先处理的问题
|
||||||
|
|
||||||
|
### P0：采集完成仍绑定草稿求解结果
|
||||||
|
|
||||||
|
`runtime/runner.py:271` 在关闭控制栈后调用 `finish_captured_draft()`；
|
||||||
|
`runtime/artifacts/raw_joint_draft.py:352` 只有“数据完整且完整目标草稿生成”才返回 0。
|
||||||
|
因此即使采集完整，离线不收敛也会使整个命令返回 3。关闭控制栈的顺序是正确的，
|
||||||
|
但采集任务的耗时和成功语义仍受拟合影响，README 的“后续单独离线”也不完全准确。
|
||||||
|
|
||||||
|
建议默认入口结束于归位、封存和采集报告，退出码只描述采集；拟合由显式离线命令
|
||||||
|
执行。若保留一键流程，作为明确选项组合两个阶段，分别记录退出状态；拟合失败
|
||||||
|
不改变采集结论，更不据此安排重采。不要另建采集执行器。
|
||||||
|
|
||||||
|
### P0：默认配置与有效离线配置没有收敛
|
||||||
|
|
||||||
|
对照当前默认 Profile 与生成程序草稿使用的
|
||||||
|
`calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_profile.yaml`：
|
||||||
|
|
||||||
|
| 项目 | 默认 Profile | 草稿拟合 Profile |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| 四指侧摆通道 2–5 的 SDK→URDF 方向 | −1 | +1 |
|
||||||
|
| 正面 ID12–15 安装连杆 | `*_metacarpals` | `*_middle` |
|
||||||
|
| 默认产品外参 | 20260922_101452 | 新拇指批次使用 20260923_181333 |
|
||||||
|
|
||||||
|
ID17 在两者中均已绑定 `thumb_proximal`。这些差异不是哈希损坏：默认配置自己的
|
||||||
|
`--validate-only` 可以通过，但这只证明内部结构和文件身份一致，不证明与现场安装一致。
|
||||||
|
原始角点仍有价值，不能因为默认建模配置错误就全部废弃数据。
|
||||||
|
|
||||||
|
建议把已确认的模型方向/标签连杆修订纳入版本化配置；每次采集保存完整配置包，
|
||||||
|
包括 Product、Profile、源 URDF、内外参与标签尺寸。旧数据按自己的快照回放；
|
||||||
|
模型修订另存，保持原采集身份。外参按实际安装选择，不能无条件把“最新文件”
|
||||||
|
应用到所有历史数据。新旧相机批次应各用各的外参。
|
||||||
|
|
||||||
|
### P1：未观测的方向端点可以改变“实测行程”
|
||||||
|
|
||||||
|
这是本次用当前代码重新复现的确定问题，不是照片推测。
|
||||||
|
读取新拇指真实训练帧及已保存参数，核验原始数据 SHA256 与封存清单，不重新求解：
|
||||||
|
|
||||||
|
| 关节 | 程序 URDF 上限 ° | 超出已观测正程端点 ° | 未观测回程参数加 5° 后上限变化 |
|
||||||
|
| --- | ---: | ---: | ---: |
|
||||||
|
| CMC roll | 52.9662 | 0.0520 | +5° |
|
||||||
|
| CMC yaw | 110.9948 | 0.1495 | +5° |
|
||||||
|
| MCP | 106.0734 | 0.9447 | +5° |
|
||||||
|
| IP | 85.4810 | 0.0018 | +5° |
|
||||||
|
|
||||||
|
四项的训练 Jacobian 对应列范数都是 0；修改后训练残差逐元素完全不变。
|
||||||
|
255 端点有正程方向历史的图像，没有独立 decreasing/255 图像。回程起点沿用
|
||||||
|
上一实际推进方向是正确记录，不能把它伪标成独立回程观测。
|
||||||
|
|
||||||
|
原因链：`UrdfCommandImages` 为每方向分配完整曲线 → `JointMapping.bounds_rad`
|
||||||
|
取两条完整曲线包络 → `prepare_raw_motion_ranges()` 将它直接写入范围证据。
|
||||||
|
当前范围证据检查了基准、轮次及图像身份，没有约束每个极值的方向有效域。
|
||||||
|
增量更新工具虽然冻结无观测参数，却仍保留其初值，并未阻止初值进入导出范围。
|
||||||
|
|
||||||
|
本批模型调用全参数协方差检查会报 `urdf_images_geometry_rank_deficient`；上述
|
||||||
|
零列已足以造成秩亏。该检查包含固定旧四指的参数，不能当成一次完整正式拟合
|
||||||
|
运行结果，但说明“方向单元采全”不保证全部自由参数可辨识。
|
||||||
|
|
||||||
|
建议统一处理模型、曲线和导出的有效域：每方向节点记录训练支持、可辨识性和
|
||||||
|
来源；未观测自由参数不进入实测范围。若使用共享端点/连续性约束，必须显式
|
||||||
|
标为模型假设并检查数据是否支持。JSON 读取、范围证据、URDF 导出及协方差应
|
||||||
|
使用同一份契约，不能只在最后裁剪 limit，也不能靠伪逆把未知量变成零不确定度。
|
||||||
|
|
||||||
|
必要回归：扰动无观测参数不得改变认证范围；训练/验证不串用；方向切换端点身份
|
||||||
|
正确；缺证据时明确报告范围不足；JSON 回读重建保持同一有效域。
|
||||||
|
|
||||||
|
### P1：JSON 曲线精度不等于纯 URDF 线性驱动精度
|
||||||
|
|
||||||
|
审查程序生成文件：`offline_combined_new_thumb_draft/o30_right_draft.urdf`，
|
||||||
|
SHA256：`9d6e71276ffcc2108cf0776cdee08a23803de9f048b1a42b7ca76e98b40e48a2`。
|
||||||
|
复核保存的独立第三轮报告，最大角度误差如下；本轮没有用第三轮重新拟合：
|
||||||
|
|
||||||
|
| 关节 | JSON 双向曲线 ° | 纯 URDF 线性驱动 ° |
|
||||||
|
| --- | ---: | ---: |
|
||||||
|
| roll | 0.837 | 0.919 |
|
||||||
|
| yaw | 1.538 | 3.516 |
|
||||||
|
| MCP | 0.749 | 2.285 |
|
||||||
|
| IP | 1.070 | 2.621 |
|
||||||
|
|
||||||
|
曲线模式的四关节角度子项通过，但整体像素/不确定度仍失败；两个模式的整体
|
||||||
|
报告均为 `passed=false`。这些角度是条件图像模型的估计，不是独立编码器真值。
|
||||||
|
|
||||||
|
当前模型的修正关系是 `T_origin_corrected = T_origin_CAD · R_axis(delta)`，
|
||||||
|
JSON 给出相对修正零位的 `q_output=f(command, direction)`。文件回读 FK 一致
|
||||||
|
证明写入关系一致，不证明 delta 或 f 是真实物理值。
|
||||||
|
|
||||||
|
当前纯 URDF 驱动使用 `q=lower+(upper-lower)*u/255`。上下限能表达角度范围，
|
||||||
|
不能同时保存中间点非线性和方向差异。此前冻结同一模型的训练诊断中，yaw 在
|
||||||
|
SDK80 处线性值比正程曲线大约 2.90°，在 SDK181 处大约 1.32°；不是全区间都
|
||||||
|
“行程偏小”。当前 yaw 的未观测端点影响约 0.15°，也不足以解释全部偏差。
|
||||||
|
|
||||||
|
目标应明确为程序同时生成可信几何 URDF 和指令映射。如果使用纯 URDF 线性
|
||||||
|
驱动，就必须用该驱动方式独立验收零位、两端、中段及组合姿态，并如实报告无法
|
||||||
|
用一条直线达到的精度；不能把扩大物理行程作为非线性补偿。
|
||||||
|
|
||||||
|
### P1：实际重复性与组合姿态仍未达到自动认证条件
|
||||||
|
|
||||||
|
已有现场记录中，同基准指令返回后，正面固定 ID0 角点 RMS 位移约 0.092 px，
|
||||||
|
拇指 ID1/ID2 分别约 7.39/16.23 px，反馈接近。证据说明存在需要解释的重复性
|
||||||
|
差异，不能仅凭一次对照唯一归因于硬件回差、标签松动或零位错误。
|
||||||
|
当前双向曲线都强制基准值为 0，单个固定零位不能自动解释不一致的基准观测。
|
||||||
|
|
||||||
|
建议先用已有各轮基准帧做相同路径/方向/其他通道状态下的重复性统计，单独报告
|
||||||
|
跨路径差异。保持既有原始帧，不为降低残差删掉不利观测，也不立即增加在线几何
|
||||||
|
暂停。若固定条件下仍无法重复，增加优化迭代不能获得可信的确定性映射。
|
||||||
|
|
||||||
|
新拇指与旧四指组合依赖冻结的旧四指及标签安装参数。它是条件草稿；旧参考误差
|
||||||
|
未完整传播，旧整手仍有 1/108 方向缺项。新拇指 24/24 不能补齐这些认证缺口。
|
||||||
|
组合生成还依赖输出目录中的专用脚本，没有形成统一的跨批次正式入口。
|
||||||
|
|
||||||
|
## 3. 建议实施顺序及验收边界
|
||||||
|
|
||||||
|
1. **先稳定采集交付**：收敛配置与快照，默认只采集并封存；保留原路线、速度、
|
||||||
|
2+1 分区、一次补采和硬件保护。采集报告明确已完成/缺项/中断、是否安全归位。
|
||||||
|
2. **用已有数据修离线契约**：先解决方向有效域与未观测参数，再用相同冻结输入
|
||||||
|
比较旧/新结果。统一共享模型和导出模块，避免另建拇指拟合器或手调常量。
|
||||||
|
3. **形成单一可复现离线入口**：训练模型选择 → 冻结 → 独立轮验证 → JSON 落盘 →
|
||||||
|
重建 URDF → 实际驱动方式的文件/FK/角度验收。跨批次模型显式携带外参及参考
|
||||||
|
不确定度。失败另存报告，不覆盖已发布模型、不自动重采。
|
||||||
|
4. **最后评估实机精度**：先区分基准、端点、中间映射、组合几何的误差，再决定
|
||||||
|
是否缺少必要观测。只有数据能支持轴线/安装修正时才扩展模型，不能靠任意放开
|
||||||
|
CAD 轴吸收外参和重复性误差。现有单关节验证不能认证任意多轴组合。
|
||||||
|
|
||||||
|
采集稳定、求解成功、文件正确、实机精度通过应有独立状态。满足前三项仍不能
|
||||||
|
自动承诺第四项；本轮没有把软件通过表述为现场已能自动生成准确 URDF。
|
||||||
|
|
||||||
|
## 4. 本轮验证与证据
|
||||||
|
|
||||||
|
- 采集、预算、断点、重装隔离、局部范围和增量更新：46 passed，16.93 秒；
|
||||||
|
按测试约定未重复运行耗时完整整手虚拟时钟用例。
|
||||||
|
- 草稿、离线初始化、训练隔离及文件生成相关：39 passed，24.41 秒。
|
||||||
|
- 默认正式 CLI `--validate-only` 通过；它不能检测与现场安装的配置漂移。
|
||||||
|
- 初次测试命令覆盖了 ROS 的 PYTHONPATH，收集失败；修正环境后运行上述分组。
|
||||||
|
一次 `python -m runtime.runner` 未调用 main、没有实际校验，改为直接调用 CLI main 后确认通过。
|
||||||
|
- 真实冻结数据的端点敏感性检查已完成；既有 85 项测试通过没有覆盖并消除该漏洞。
|
||||||
|
|
||||||
|
本轮机器可读证据：
|
||||||
|
|
||||||
|
- [端点审计](../../calibration_output/software_review_20260923_current_flow/endpoint_audit.json)
|
||||||
|
- [配置差异、程序原稿与角度误差](../../calibration_output/software_review_20260923_current_flow/review_summary.json)
|
||||||
|
- [采集相关测试](../../calibration_output/software_review_20260923_current_flow/capture_tests.json)
|
||||||
|
- [离线相关测试](../../calibration_output/software_review_20260923_current_flow/offline_tests.json)
|
||||||
|
|
||||||
|
既有现场重复性证据：
|
||||||
|
`calibration_output/thumb_recapture_20260923_175829/live_comparison_20260923/repeatability.json`。
|
||||||
@@ -0,0 +1,148 @@
|
|||||||
|
# O30 右手离线修正逻辑复核（2026-09-23）
|
||||||
|
|
||||||
|
本轮使用封存会话 `splay_recapture_20260923_132346/O30_RIGHT_001/20260923_142201`。
|
||||||
|
原始数据未改写，没有 SDK 运动；训练轮 0/1 共 4769 帧，独立验证轮 3 共 2681 帧。
|
||||||
|
采集仍为 107/108 个方向完整。独立验证未参与参数拟合、候选选择或绑定判断。
|
||||||
|
|
||||||
|
## 已修正的问题
|
||||||
|
|
||||||
|
1. **标签所属连杆错误。** ID12–ID15 原配置为 `*_metacarpals`，程序因此认为 MCP/PIP 动作不会带动这些标签。
|
||||||
|
训练数据中四标签随 MCP 移动约 93–105 像素、随 PIP 移动约 37–49 像素，DIP 动作时仅约 0.2–0.3 像素。
|
||||||
|
小指对应 MCP/PIP 观测中掌部参考角点仅变化约 0.08/0.19 像素。
|
||||||
|
用户确认四标签都固定在中节或随中节运动的支架上，最终离线绑定为 `pinky_middle`、`ring_middle`、
|
||||||
|
`middle_middle`、`index_middle`。保持角色名称、ID、原始角点和 SDK 指令不变。
|
||||||
|
原始采集配置保留；本批求解必须使用下述新的离线配置，不能继续使用旧绑定配置。
|
||||||
|
2. **PnP 候选排序被误当成轨迹身份。** 实际 ID17 在 SDK 64→96 期间交换候选排序，固定取第 0/1 项会产生超过 40° 的虚假跳变。
|
||||||
|
草稿与正式求解的初始化现在复用候选连续关联，保留两个候选族，缺失的候选不复制成第二份证据。
|
||||||
|
初始化不用于授权唯一分支；原有在线严格连续性检查保持不变。最终拟合仍直接使用二维角点。
|
||||||
|
3. **扫描起点被上一段回程方向排除。** 初始化现在按正式扫描/分段归属纳入起点，四指补段只纳入实际运动的关节。
|
||||||
|
前向迟滞模型仍使用原始逐通道方向历史，不改写历史来伪造上行或下行。
|
||||||
|
4. **欠约束零位可能被漏判。** 当 Jacobian 行数小于列数时，保留完整右零空间，防止精简 SVD 遗漏不可观测方向。
|
||||||
|
5. **行程证据报告不准确。** 行程和基准样本只引用主视角有效可见标签及实际训练轮次，排除验证轮和无标签图像。
|
||||||
|
单训练轮草稿使用明确的规划选项；正式范围验收仍拒绝单轮证据。
|
||||||
|
6. **生成与验收混淆。** 终端分别显示全手参数生成、独立验证和草稿发布状态；结果清单记录模型绑定修正。
|
||||||
|
|
||||||
|
## 最终本批结果
|
||||||
|
|
||||||
|
目录：`calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_draft/`。
|
||||||
|
|
||||||
|
- `o30_right_draft.urdf` 与 `draft_calibration.json` 必须配套使用，SDK 指令不能直接作为 URDF 弧度。
|
||||||
|
- 11 个拟合零位、4 个用户定义的竖直侧摆零位、5 个保留 CAD 的末端零位；20 个拟合运动范围。
|
||||||
|
- 训练坐标 RMS：旧竖直草稿 5.5647 px → 本版 1.4867 px。
|
||||||
|
- JSON 与最终 URDF FK 最大差 5.55e-16；标准加载、原始文件哈希检查通过。
|
||||||
|
- 18/20 个关节的独立角度检查通过，包括四指全部关节及 `thumb_cmc_yaw`。
|
||||||
|
- `thumb_cmc_yaw` 独立角度 MAE 0.658°、P95 1.950°、最大 2.180°。
|
||||||
|
- `thumb_cmc_roll` 最大 10.306°、`thumb_mcp` 最大 4.262°，二者角度检查未通过。
|
||||||
|
- 全局最大测量重投影误差 16.47 px，最大角度置信界约 8.71°；全手精度仍未通过。
|
||||||
|
角度子项通过不等于零位绝对精度或任意多关节联动已认证。
|
||||||
|
- 没有更新 `latest_passed`，没有替换用户正在运行的 MuJoCo 模型。
|
||||||
|
|
||||||
|
`offline_motion_logic_review` 和 `offline_corrected_tag_links_draft` 是绑定核实过程中的对照产物，
|
||||||
|
已分别留下被最终中节绑定取代的说明，不能将其中的支座/近端绑定当成最终确认。
|
||||||
|
最终确认及像素依据见同级 `offline_confirmed_middle_tags.json`、`tag_motion_dependencies.json`。
|
||||||
|
|
||||||
|
## 只读离线重放
|
||||||
|
|
||||||
|
在项目根目录运行；输出目录必须是尚不存在的新目录：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
source /opt/ros/jazzy/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
OPENBLAS_NUM_THREADS=1 OMP_NUM_THREADS=1 ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_product.yaml \
|
||||||
|
--workspace /home/lxp/projects/linkerhand_retarget_ros2 \
|
||||||
|
--offline-raw calibration_output/splay_recapture_20260923_132346/O30_RIGHT_001/20260923_142201/raw_samples.jsonl \
|
||||||
|
--offline-draft --draft-upright-splay-zero \
|
||||||
|
--offline-output calibration_output/splay_recapture_20260923_132346/offline_confirmed_middle_tags_replay
|
||||||
|
```
|
||||||
|
|
||||||
|
## 回归范围
|
||||||
|
|
||||||
|
新增 7 项集中回归均通过：真实候选换序、起点方向/四指补段、候选缺失及严格连续性门限、
|
||||||
|
欠约束零位、两个单训练轮草稿与正式拒绝边界、确认中节绑定的原始配置隔离及关节依赖。
|
||||||
|
其余受影响的草稿初始化、重复观测目标等价、JSON 导出、独立验证失败记录、CLI 和关闭控制链测试分组通过。
|
||||||
|
首次测试有 4 项因 shell 未加载 ROS 缺少 rclpy，2 项因新增夹具字段名错误失败；修正后对应项通过。
|
||||||
|
未恢复被外部删除的历史测试，也没有把离线回放当作实机验收。
|
||||||
|
|
||||||
|
|
||||||
|
## 新版仿真仍有拇指偏差的复核
|
||||||
|
|
||||||
|
只读订阅确认当前拇指 SDK 指令仍为 roll=55、yaw=181、MCP=122、IP=0；
|
||||||
|
实机反馈约为 56、176、120、4。仿真与新草稿文件哈希相同，使用配套指令映射。
|
||||||
|
新旧映射在当前 yaw 指令下输出分别为 79.164°、79.207°；新旧拇指末端 FK
|
||||||
|
仅差 0.219 mm 和 0.271°，因此上一轮修改并没有实质解决这个拇指姿态差异。
|
||||||
|
|
||||||
|
当前联合求解器固定 CAD 的关节轴线和拓扑，只拟合掌姿态、标签安装、零位和指令曲线。
|
||||||
|
训练轮中 MCP 轴的连续 PnP 假设与该模型轴存在偏差：正面 ID1 最接近的候选约 10.2°，
|
||||||
|
同机位末端标签约 9.94°，顶机位 ID17 约 13.09°。这些是几何诊断线索，不能排除相机
|
||||||
|
外参和候选歧义的影响，也不能直接将其作为允许写入 URDF 的轴修正。
|
||||||
|
静止标签的微小旋转不作为轴证据；原始候选与运动量完整保存在诊断 JSON 中。
|
||||||
|
|
||||||
|
原采集主要为单关节扫描，MCP 扫描将 yaw 保持在 SDK 80；当前 roll/yaw/MCP 的
|
||||||
|
55/181/122 组合不属于已直接观测的组合姿态。独立轮次中的角度子项通过并不认证
|
||||||
|
绝对关节轴/零位或任意组合运动。后续应优先验证拇指几何约束及组合姿态，不能用降低
|
||||||
|
误差阈值、照片手调 yaw 或再次导出同一参数代替修正。
|
||||||
|
|
||||||
|
诊断：`offline_confirmed_middle_tags_draft/thumb_current_pose_model_audit.json`。
|
||||||
|
本次仅诊断，未改求解参数、仿真模型或实机姿态。
|
||||||
|
|
||||||
|
|
||||||
|
## yaw 行程专项复核
|
||||||
|
|
||||||
|
本轮保持 CAD 转轴及全部拟合参数不变，只读训练角点、URDF/JSON 和当前 ROS 指令。
|
||||||
|
两个训练轮的正反扫描均有 SDK 0/255 对应的顶机位 ID17 有效图像，每个端点 3 帧。
|
||||||
|
SDK=255 的实际方向历史为 increasing（反向扫描起点也沿用此前的上行历史），没有把扫描
|
||||||
|
名称当成实际指令方向，更没有要求反馈必须等于 255 才认可发送端点。
|
||||||
|
|
||||||
|
当前输出曲线：SDK 80→32.97°、128→54.11°、181→79.16°、223→98.26°、255→113.39°。
|
||||||
|
URDF 上限为 113.3957°，高于原 CAD 约 107.25° 的名义范围。单独加载 MuJoCo 后对
|
||||||
|
181/223/255 与三种方向状态逐项执行映射和 mj_forward，qpos 与映射完全一致，未发生限位截断。
|
||||||
|
当前实机控制话题仍是 yaw=181，反馈约 176；这不能当成实机已到全行程终点。
|
||||||
|
|
||||||
|
保留两个连续 IPPE 假设、分别检查两训练轮正反向后，原始角点估计的相对总转角为
|
||||||
|
114.54°–115.15°，181 处约 80.00°–80.99°。与当前曲线相比，存在约 1°–2° 的偏小线索，
|
||||||
|
但这是姿态候选诊断，不能直接当成独立物理真值或据此放大行程。
|
||||||
|
当前零位修正为 −5.13°；它影响绝对朝向，不等于减少 5.13° 的相对行程。
|
||||||
|
明显组合姿态差异尚不能只由这约 1°–2° 的相对行程差解释。
|
||||||
|
|
||||||
|
原始诊断与完整逐停点数据：`offline_confirmed_middle_tags_draft/yaw_range_review.json`。
|
||||||
|
仿真核验：`linker_hand_mujoco_ros2/reports/yaw_mujoco_range_check.json`。
|
||||||
|
未发送实机动作，未改轴、放大曲线或替换任何模型。
|
||||||
|
|
||||||
|
|
||||||
|
## 当前中间指令的现场图像诊断
|
||||||
|
|
||||||
|
用户明确为同一中间指令下仿真偏小。为避免只对单关节历史数据推断组合姿态，
|
||||||
|
在现有 55/181/122/0 拇指指令保持不变时，短时读取三相机图像；没有机械手控制发布。
|
||||||
|
侧面可见 ID1/ID2，各 8 帧；原始文件单独存入 `current_thumb_pose_readonly`，不加入训练。
|
||||||
|
固定当前几何/零位/安装的图像角度诊断：仅调 yaw 最佳约 +2.16°；四关节共同解释时
|
||||||
|
约 roll +4.44°、yaw +0.82°、MCP −0.73°、IP +0.41°。残差与不确定度仍较大，
|
||||||
|
yaw 条件 3σ 约 5.96°，未包括共享几何不确定度，不能据此写入新修正。
|
||||||
|
这支持继续区分中段映射、绝对零位及组合姿态误差，不能直接扩大 URDF 上限。
|
||||||
|
详见 `current_thumb_pose_readonly/README.md` 与 `thumb_pose_measurement.json`。
|
||||||
|
|
||||||
|
## 纯 URDF 模式下分开复核零位与组合姿态
|
||||||
|
|
||||||
|
后续仿真按用户要求默认仅使用 URDF 线性映射。对已保存 55/181/122/0 组合图像重新诊断：
|
||||||
|
只调 yaw 为 +1.581°，同时拟合四拇指角度时 yaw 为 −0.503°；不能沿用旧曲线模式的 +2.160°。
|
||||||
|
训练两轮的条件零位复核为 yaw −5.331° / −4.907°，并未独立证明绝对零位正确。
|
||||||
|
|
||||||
|
发现最终原始记录存在相同输入冲突：训练轮 0 的四指侧摆，13:47:45 上行末端与 13:53:29
|
||||||
|
下行起点，控制指令、方向历史、相机内参及拇指反馈相同，掌部 ID0 中心只差 0.041 px，
|
||||||
|
拇指 ID1/ID2 中心分别差 6.264/15.468 px。7 帧与原始 JSONL 逐字段核对一致。
|
||||||
|
这些辅助拇指观测进入了零位求解；重复观测聚合虽保留原目标，却不能解决输入本身的不一致。
|
||||||
|
诊断性暂不使用四指侧摆任务辅助拇指观测后，yaw 零位 −5.130°→−5.018°，
|
||||||
|
指令 181 的拟合角度仅 79.164°→79.185°，因此此问题不足以解释全部 yaw 差异。
|
||||||
|
|
||||||
|
细节见采集目录 `current_thumb_pose_readonly/ZERO_AND_COMBINATION_REVIEW.md`；对应三个 JSON
|
||||||
|
保留条件、证据和敏感性结果。未改 CAD 转轴、原始数据、仿真 URDF、生产求解器或正式发布状态，
|
||||||
|
未使用第三轮拟合，也未发送实机动作。
|
||||||
|
|
||||||
|
### 对故障归因的修正
|
||||||
|
|
||||||
|
“观测有差异”不等于“离线实现有 bug”。再次核查发现，重复观测压缩完整保留组内平方误差；
|
||||||
|
7 帧实测数据的完整目标与压缩目标差值小于 1e-11 px²。现有草稿如实记录独立验收失败并禁止正式发布。
|
||||||
|
在现有确定性模型下采用最小二乘折中是合理实现，不能只凭不同采集段图像不一致就要求删除观测或修改拟合。
|
||||||
|
前面对“必须修正离线数据使用逻辑”的表述过强，应以此处结论为准：软件缺陷尚未证实；
|
||||||
|
相机视角、线性驱动近似、实际回位差异和固定几何模型的适用范围仍是待区分解释。
|
||||||
|
详见 `current_thumb_pose_readonly/hypothesis_reassessment.json`。本轮仅复核证据及更正说明，未修改生产代码或模型。
|
||||||
@@ -0,0 +1,208 @@
|
|||||||
|
# O30 轻量标定实现与验收边界
|
||||||
|
|
||||||
|
2026-09-23。O30 正式入口为 `raw_joint_2_plus_1` / `raw_joint_images_v2`。
|
||||||
|
采集结束与 URDF 验收是独立结果，软件验证不代表实机通过。
|
||||||
|
|
||||||
|
## 正式采集流程
|
||||||
|
|
||||||
|
启动检查 → 17 个任务、20 个关节连续两轮训练及一轮验证 → 当前失败方向立即补一次 → 安全归位 → 封存数据并关闭控制栈。
|
||||||
|
|
||||||
|
- 保留原运动路线、避让、四指侧摆分段、训练及验证停点和速度。108 个正式方向单元，训练轮身份 0、1，独立验证轮身份 3。
|
||||||
|
- 移动标签只保存角点、身份、检测质量、相机参数、指令、反馈及时间。在线不运行移动标签 PnP、分支确认、父模型或辅助消歧；固定基准保护保留。每个任务只要求原指定主机位覆盖。
|
||||||
|
- 停点由稳态效果统一有界等待。中间指令步不再要求额外固定反馈位移；整个方向使用真实同向反馈推进保护，4 秒累计实际运动无推进即停止，计划内静止采样不消耗运动预算，切换停点不重置预算。
|
||||||
|
- 按用户最新要求，端点以实际发送的 SDK 指令为准，不要求反馈等于 0/255。有效稳态图中同步记录的已发送端点指令也用于端点核验，不重复要求扫描阶段另有一张端点图；稳态图不混入运动覆盖、反馈行程或分箱统计。没有实际发到的目标值不能作为端点证据。
|
||||||
|
- 反馈失联、硬件故障、控制权冲突、越界、固定参考移动仍停止；避让和归位到位要求不变。按用户补充要求，停滞后等待 3 秒、从当前指令位置继续同一方向一次；再次停滞停止，不循环重发。
|
||||||
|
- 当前方向质量不足立即按原准备路径回到起点再采一次。仍不足记录缺项、继续下一方向；不队尾补采、不循环回访、不合并两个失败尝试。重启保留已消耗预算。
|
||||||
|
- 结束状态为 `CAPTURE_COMPLETE`，不是标定通过。`raw_capture_manifest.json` 独立记录流程结束、覆盖完整、尝试次数、缺项、配置和证据文件哈希。完整返回码为 0，缺项返回码为 3；两者都关闭控制栈。
|
||||||
|
- 在线不启动最终拟合。后续 `--offline-raw` 只读封存数据，单独保存输出，缺项或拟合失败都不触发运动。
|
||||||
|
- 离线可重新计算已完成方向的采集质量，并在 `acquisition_reassessment` 保存原失败原因和新检查结果；不改写原始记录、不拼接尝试，不接受中断或未完成的方向。断点恢复仍按原记录消耗补采次数。
|
||||||
|
- 新会话计划为 `capture_plan_v7_raw_joint`，立即补采语义与 v2 角点绑定。旧 v1 数据保持离线读取，禁止混入新会话续采。断点仍要求安装条件可核验。
|
||||||
|
|
||||||
|
## 历史实机卡点
|
||||||
|
|
||||||
|
`20260922_233830` 只有拇指旋转的 6/108 单元通过，拇指 MCP 指令 32、反馈 0 三次失败。
|
||||||
|
此前拇指旋转指令 223/255、反馈 189/195 也曾停滞。本次针对逐停点额外推进要求进行结构调整，
|
||||||
|
不能由这些历史日志单独推断真实死区、阻挡或反馈异常；当前实机结果须另行记录。
|
||||||
|
|
||||||
|
## 离线求解与发布
|
||||||
|
|
||||||
|
- 未认证草稿按任务接受至少一轮完整训练，并使用另一轮已通过采集质量检查的完整方向。
|
||||||
|
例如侧摆第二轮缺一方向时，第一轮完整仍可保留四指关节链；初始化取完整训练轮。
|
||||||
|
完整方向仍按原始质量门限复核，不拼接两次失败尝试，第三轮不补训练缺项。
|
||||||
|
逐任务覆盖、缺项和算法身份写入独立输出；采集完整性与正式发布规则不变。
|
||||||
|
- 草稿可显式复用同批数据的局部训练结果作初值，核对原始文件/配置身份与训练选择记录，
|
||||||
|
共享参数仍全部重新拟合；第三轮不用于初始化或选择。重复预测做加权等价合并，
|
||||||
|
保留组内散差和原始逐图误差，以减少内存和重复计算。草稿数值停止容差单独记录。
|
||||||
|
- `UrdfCommandImages` 复用共享图像优化入口，直接沿原 CAD 拓扑计算四指及非平行拇指。一个掌变换、每个实体标签一个共享安装；不引入各阶段独立六自由度变换。
|
||||||
|
- 五根轴的方向与空间布局初始化掌坐标，子轴补充非平行约束；所有允许零位和双向单调指令映射在原始图像目标中联合求解。首次观测及行程端点不作为物理零位。
|
||||||
|
- 最多 64 个初始化，每个初始化的安装预优化与联合优化合计最多 200 次求值。正式后台进程沿用 600 秒上限；预算耗尽保留失败原因，不自动增加动作。
|
||||||
|
- 训练候选先检查原重投影门限，再对不同数值解族做有分辨率余量的配对图像损失检验（0.03 px、Holm 家族错误率 0.01）。同一保持姿态和重复初始化不重复计为独立证据。所有未被训练证据排除的候选均保留；发布代表按物理输出的最坏误差选择，不按最低像素分数选零位。
|
||||||
|
- 完整相关协方差和候选间差异传播到零位、绝对指令角、轴方向、轴位置、FK 位置及旋转。保留原角度/空间门限；标签安装不唯一不单独否决。审计的 FK 范围为训练网格和单关节姿态，不宣称任意多轴组合已验收。
|
||||||
|
- 第三轮冻结全部共享几何、安装、零位和指令映射。仅为独立测量估计每张图自己的角度；按图像块批量运算，各图之间不共享测量变量。第三轮不能参与训练、候选选择或参考修正。
|
||||||
|
- 沿用现有文件名和 `latest_passed`。仅授权修改 `origin.rpy` 与限位；拇指 IP、四指 DIP 保留 CAD 零位。检查 JSON 重建、实际文件 FK、授权字段及标准 robot_state_publisher 加载后，才允许原子发布。
|
||||||
|
|
||||||
|
## 使用与结果
|
||||||
|
|
||||||
|
首次新协议必须显式新采集，旧会话及原 66 个通过单元只保留回归用途：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--no-resume --camera-optical-observations /绝对路径/optical_observations.json
|
||||||
|
```
|
||||||
|
|
||||||
|
采集结束后，使用同一批只读数据离线修正；默认不发布：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--offline-raw /绝对路径/会话/raw_samples.jsonl \
|
||||||
|
--offline-output /绝对路径/新的离线结果目录
|
||||||
|
```
|
||||||
|
|
||||||
|
`--publish-offline` 仅在全部原精度与文件检查通过后才允许更新 `latest_passed`。
|
||||||
|
原始索引、压缩证据、导入证据和 `raw_capture_manifest.json` 须一起保留；不得编辑封存数据。
|
||||||
|
|
||||||
|
后续恢复省略 `--no-resume`，仍需提供当前安装条件下的光学观测。若安装参考不可核验，按拒绝原因处理，不能改哈希绕过。
|
||||||
|
|
||||||
|
2026-09-22 用户明确要求复用已完成的相机标定：可将上述光学观测参数换为
|
||||||
|
`--reuse-camera-calibration`。此入口仍核对已有相机标定质量、受保护文件和实时
|
||||||
|
CameraInfo；日志及发布审计记录 `current_optical_verification: not_performed`，
|
||||||
|
不宣称本次重新做过光学核验。第三轮独立关节精度、候选不确定度与 URDF 发布检查不变。
|
||||||
|
|
||||||
|
现有 `three_camera_extrinsics.launch.py` 增加 `verification_extrinsics_file`：指定受保护外参文件、另设 JSON `output_file`，沿用棋盘格采集及保存操作。该模式不重拟合相机参数，保存同步多姿态角点，并验证冻结内外参；每个机位至少 15 组，跨机位同步不超过 50 ms，同时检查图像/倾角覆盖及原重投影门限。正常外参标定模式不变。核验模式的角点 JSON 用于上述启动参数。
|
||||||
|
|
||||||
|
关键文件：
|
||||||
|
|
||||||
|
- `raw_joint_result.json`：分别记录原始采集完整、拟合完成、精度通过；包含缺失单元、失败原因和禁止自动几何重采标志。
|
||||||
|
- `raw_joint_training.json` / `raw_joint_training_selection.json`：初始化预算、训练排除依据、全部候选审计。
|
||||||
|
- `raw_joint_candidates.npz`：保留候选参数和完整协方差。
|
||||||
|
- `raw_joint_holdout.json`：每个保留候选的冻结验证。
|
||||||
|
- `calibration_report.json` / `release_manifest.json`：协议、原始证据、安装参考、光学观测及候选集合身份；最终发布证据。
|
||||||
|
|
||||||
|
## 历史软件验证（2026-09-22）
|
||||||
|
|
||||||
|
- 全手运动效果测试覆盖 17 个任务、三轮正式往返、原避让和归位；实际 coordinator 虚拟时钟覆盖小指 18 个方向、全部已发送指令，无在线求解。
|
||||||
|
- 当时的有界队尾补采、跨恢复预算、首单元中断发现、协议/参考拒绝、固定参考保护、光学缺失时禁止启动均有测试。当前 v2 已改为当前方向立即补一次。
|
||||||
|
- ID5、ID9、ID12 的真实历史双候选角点可作为采集数据；ID10 真实缺测不能变成完整扫描。
|
||||||
|
- 独立 XML FK 真值：64 个初始化全部收敛，约 193 秒；32 个超过原图像门限，16 个被训练配对证据排除，剩余 16 个全部通过候选集合不确定度及第三轮验收。该批次生成合成 JSON/URDF，并通过标准 ROS 加载和最终文件 FK 回读。
|
||||||
|
- 合成产物位于 `calibration_output/software_review_20260922_raw_joint/known_truth/`，仅有软件测试指针 `synthetic_only_passed`；没有更新实机 `latest_passed`。
|
||||||
|
- 该软件产物保留生成时的检验审计；当前更严格的 0.01 家族错误率复核另存 `current_training_selection.json`，保留候选集合完全相同，未重复运行优化器或改写原验收文件。
|
||||||
|
- 当日没有重新启动电机；这些合成结果不能证明实际机械手的准确性。当前相机安装的光学核验和实机姿态对照仍须单独验收。
|
||||||
|
|
||||||
|
## 本次实施验证（2026-09-23）
|
||||||
|
|
||||||
|
软件完整 coordinator 模拟：108 方向均通过、没有补采，安全归位后封存且不再发送指令；
|
||||||
|
旧合成报告的 v1/v2 协议重建产生相同 URDF，原候选不确定度拒绝条件仍有效。
|
||||||
|
采集、立即补采、封存和只读离线检查详见 `TESTING.md`。
|
||||||
|
|
||||||
|
实机 `20260923_104837` 去程通过，拇指旋转回程指令降到 162 而反馈仍为 253，
|
||||||
|
累计实际运动无推进 4.005 秒后保护停止。角点记录亦未显示明显回程运动。
|
||||||
|
控制栈已退出，未重复强推、未自动归位；本次仅完成 1/108 方向，未生成通过的 URDF。
|
||||||
|
随后用户明确允许此类硬件停滞重试一次，已采用等待 3 秒后继续一次的策略；
|
||||||
|
新会话 `20260923_110651` 使用更新后的计划身份，不导入上述旧策略会话。
|
||||||
|
|
||||||
|
该新会话实际完成 35/108 个方向，原在线记录 29 个通过、6 个端点覆盖不足，
|
||||||
|
在小指 PIP 第三轮回程收到 `o30_sdk_identity_mismatch` 后停止。此时反馈约 29.81 Hz、
|
||||||
|
无活动硬件故障；通用“通信失联”提示不能证明物理断线。原日志没有保存异常身份报文，
|
||||||
|
无法确定具体不匹配字段；现已补充保留异常身份字段并单独显示身份错误，停止条件不变。
|
||||||
|
未触发硬件停滞重试，控制栈已退出，未完成归位和整手封存，未再次启动运动。
|
||||||
|
|
||||||
|
按最终端点规则对原始数据只读复核，35 个完整执行方向全部通过，原 6 个端点误判均消除；
|
||||||
|
还缺 73 个方向，禁止发布。复核报告位于
|
||||||
|
`calibration_output/software_review_20260923_lightweight/interrupted_capture_review.json`，
|
||||||
|
记录源数据哈希、算法身份、逐方向重算证据；源文件哈希未变化。
|
||||||
|
本次实机尚未完成全手流程和 URDF 精度验收。
|
||||||
|
|
||||||
|
同批数据随后做了真实拇指四关节局部图像拟合，发现 ID17 的安装连杆配置错误。
|
||||||
|
用户确认 ID17 位于随 MCP 弯曲的近端指节，仅顶机位可见；现改为 `thumb_proximal`，
|
||||||
|
同步产品配置哈希，保留机位、标签尺寸、任务和路线。原始记录与旧配置身份不改写。
|
||||||
|
仅训练数据的绑定对照使最大标签重投影 RMS 从 87.575 px 降到 8.047 px，
|
||||||
|
仍未通过原 1.5 px 门限，不能称为 URDF 修正成功或证明两轮足够。
|
||||||
|
具体结果与修改前配置快照见
|
||||||
|
`calibration_output/software_review_20260923_lightweight/thumb_fit_audit/README.md`。
|
||||||
|
本轮没有实机运动，没有使用第三轮选择或调整新绑定候选，也未发布 URDF。
|
||||||
|
|
||||||
|
## 用户允许先生成低精度修正文件（2026-09-23）
|
||||||
|
|
||||||
|
新增 `--offline-draft`，与正式精度认证分开。复用 `UrdfCommandImages` 的图像残差、
|
||||||
|
参数约束及优化器，仅为部分已采集关节增加轴线初始化；训练选解后冻结，第三轮仅报告。
|
||||||
|
能估计的零位与行程一起导出。单根手指的根零位与掌坐标存在等价变换，固定其 CAD
|
||||||
|
零位；其余估计零位须不落入图像雅可比的数值零空间。原已声明末端 CAD 零位仍明确列出。
|
||||||
|
不以收敛或多个初值一致冒充实际精度认证，草稿不更新 `latest_passed`。
|
||||||
|
|
||||||
|
实际旧数据生成于 `calibration_output/O30_RIGHT_001/draft_20260923_110651_v2/`：
|
||||||
|
拇指侧摆零位 -2.761403°，MCP 零位 -1.639888°；四关节行程分别为旋转 52.171947°、
|
||||||
|
侧摆 115.878573°、MCP 104.415960°、IP 82.065095°。其余缺测关节保持原 CAD。
|
||||||
|
完整结构 URDF 已通过 JSON 读回重建、最终 FK（最大差 4.44e-16）和标准 ROS 加载。
|
||||||
|
训练最大标签误差仍约 8.047 px，独立验证未通过原精度要求，报告如实保留。
|
||||||
|
原始数据哈希未变，没有实机运动。拇指旋转与 IP 的绝对零位未标为实测修正。
|
||||||
|
|
||||||
|
第一次导出发现有效稳态窗口中也存在无同步指令的原始帧，范围证据生成误读了这些帧。
|
||||||
|
已让正式/草稿共用范围证据函数，并统一通过既有 `fitting_frames` 选择同步完整记录；
|
||||||
|
保留全部原始日志，不把缺失指令填成零。失败 v1 结果保留，成功产物另存 v2。
|
||||||
|
|
||||||
|
## 全手草稿目标与采集后自动导出（历史，已由独立离线入口取代）
|
||||||
|
|
||||||
|
按用户要求，完整目标明确为 15 个非末端零位与全部 20 个运动范围；仅五指末端零位
|
||||||
|
保留 CAD。`correction_scope` 列出目标、实际估计与缺失关节，`full_target_generated`
|
||||||
|
只在目标均覆盖且文件校验通过后成立。旧局部拇指草稿不等于全手完成。
|
||||||
|
|
||||||
|
正常 CAPTURE_COMPLETE 后，启动器关闭 SDK/相机进程、销毁监控节点与自建 ROS 上下文，
|
||||||
|
刷新日志，然后才运行离线草稿。缺数据或求解失败都只保存结果，不进入补采或归位运动。
|
||||||
|
每次启动保存受保护 Profile 原始字节快照；后续仅修正安装连杆时可自动匹配旧会话身份。
|
||||||
|
|
||||||
|
上述自动拟合行为已在后续收敛中移除：默认采集现在归位、封存、关闭控制后直接退出，
|
||||||
|
仅写 `capture_result.json`；使用会话 `capture_product.yaml` 和 `--offline-raw` 单独拟合。
|
||||||
|
运动路线、2+1 分区、一次质量补采及硬件保护保持。配置快照一起进入封存清单。
|
||||||
|
|
||||||
|
独立 XML FK 图像软件检查使用当前 ID17 近端指节绑定、原正式采样网格和机位：
|
||||||
|
单个有界初始化收敛（12 次联合迭代），15 个零位参数均可观测，雅可比秩 463/463；
|
||||||
|
已知真值的最大零位差 0.001054°。复用这次拟合输出完整 URDF，确认 15 个 origin.rpy
|
||||||
|
实际改变、五个末端 origin 不变、20 个运动范围写入，最终 FK 最大差 5.55e-16、标准
|
||||||
|
ROS 加载通过。报告位于 `calibration_output/software_review_20260923_full_draft/`。
|
||||||
|
这是必要的全手软件检查，不是实机精度结论；没有以合成结果代替完整实机流程。
|
||||||
|
# 2026-09-23 ID10 调整与断点保留
|
||||||
|
|
||||||
|
实机 `20260923_120425` 保存 78 个通过方向（拇指、小指、无名指、中指），食指 MCP
|
||||||
|
六方向及 PIP 首方向两次都因 ID10 缺图失败，后续方向由操作者中止。原始日志、角点、
|
||||||
|
配置和哈希记录完整保留；这是中断会话，未宣称全手结束或归位。
|
||||||
|
|
||||||
|
操作者最初确认 ID10 仍在食指中节，后补充说明主要调整的是遮挡 ID10 的 ID15，且
|
||||||
|
ID15 仍在原来的侧摆支座；两者均未更换连杆。新增离线准备草稿断点入口，保留
|
||||||
|
连续通过前缀，并按 CAD 拓扑隔离受影响手指的历史观测；新数据采用新安装参数，
|
||||||
|
其他方向的补采预算不变。普通恢复及正式发布门限保持原义；该调整流程的未复核
|
||||||
|
安装明确标为草稿，不通过正式发布入口。
|
||||||
|
|
||||||
|
第一次启动恢复固定基准漂移不超过 0.325 px，但尚未测量的食指正面 ID15 归位位置
|
||||||
|
相对历史偏移 5.945 px，保护拒绝且未开始正式扫描。修正为整条受影响手指的历史
|
||||||
|
观测隔离，避免借用旧食指辅助观测绑定新安装，同时不放宽其他标签与固定基准门限。
|
||||||
|
|
||||||
|
## 2026-09-23 四指侧摆重采的恢复检查
|
||||||
|
|
||||||
|
操作者授权仅重采末项四指侧摆，在独立输出目录保存前 96 个方向的完整前缀，
|
||||||
|
旧侧摆全部记录保留在原会话，正式剩余计划为 12 个方向。两次实际启动在扫描前
|
||||||
|
被活动标签基准检查拒绝：`132507` 的 ID4/ID5 差异为 6.416/12.295 px，
|
||||||
|
`132938` 为 5.182/9.673 px；固定基准最大漂移分别为 0.264/0.219 px。
|
||||||
|
操作者确认小指标签与支架未调整。相同 SDK 指令及稳定反馈不能独立证明恢复了
|
||||||
|
相同物理姿态，因此在线不再将活动标签像素差直接判定为安装改变。
|
||||||
|
|
||||||
|
原始采集恢复仅将活动标签差异记录为 `requires_offline_review`，继续导入原证据
|
||||||
|
与尝试次数；不修改原始角点、旧基准或质量结论。固定基准和受保护输入检查仍为
|
||||||
|
阻断条件，失联、硬件故障及停滞保护不变。离线正式验收保留严格安装核验，并拒绝
|
||||||
|
带未复核安装记录的数据；草稿允许分析此类数据，但不得宣称精度或安装一致性通过。
|
||||||
|
本次只运行软件检查及已保存参考的只读回放，机械手未运动，后续由操作者启动采集。
|
||||||
|
|
||||||
|
## 2026-09-23 中指补段和四指联动的指令核对
|
||||||
|
|
||||||
|
`135113` 的中指向上补段（13:54:05.985–13:54:12.319）保存 625 条实际发送
|
||||||
|
记录，四指向量由 `[0,0,0,0]` 到 `[0,80,0,0]`；没有中指补段越过 80 的记录。
|
||||||
|
下一轮联动才继续增加中指指令。联动指令为 `[64,124,64,64]` 时，实际反馈为
|
||||||
|
`[59,120,0,55]`：无名指未动，而其他手指继续推进。操作者确认存在接触挤压，
|
||||||
|
因此补段范围正确不能证明同步联动的实体避让安全，现有 4 秒保护也不是同步到位保证。
|
||||||
|
|
||||||
|
状态增加子段名称及各通道声明范围，命令/反馈标明当前通道；实际下发日志增加
|
||||||
|
子段、方向、尝试次数和通道身份。这些只是显示及诊断改动，不改变运动路线、
|
||||||
|
速度或保护预算，不宣称解决接触问题。三轮/归位/立即补采相关检查 3 passed
|
||||||
|
（1.48 秒），另核对三轮全部 12 个方向目标及子段显示。未启动机械手，未操作
|
||||||
|
操作者运行的范围检查 SDK。实际指令审计保存在该会话的 `middle_lower_command_audit.json`。
|
||||||
@@ -0,0 +1,83 @@
|
|||||||
|
# O30 剩余精度问题：封存数据定位结果
|
||||||
|
|
||||||
|
2026-09-24。只读复核旧外参全手 `20260923_142201` 和新外参拇指
|
||||||
|
`20260923_183138`，使用各自封存配置和本轮已冻结的参数。诊断脚本与 JSON 报告位于
|
||||||
|
`calibration_output/software_fix_20260923_auto_urdf_v2/`。没有修改原始证据、候选 JSON/URDF、
|
||||||
|
采集路线或精度门限；没有硬件运动。本轮新增的只是只读诊断脚本和这份报告。
|
||||||
|
|
||||||
|
## 已确认的三个独立问题
|
||||||
|
|
||||||
|
**字节到关节角不能仅靠 URDF 上下限线性换算。**逐字节比较程序生成的 JSON 曲线
|
||||||
|
与同一 URDF 的线性插值：拇指 yaw 上行在字节 96 的差为 3.167°，IP 上行在字节 128
|
||||||
|
的差为 2.273°；两端差均约为 0。这解释了为什么扩大 URDF 上限不能修正中间行程。
|
||||||
|
物理几何仍由 URDF 提供，仿真/ROS 的字节输入必须先经认证 JSON 的对应方向曲线
|
||||||
|
映射为关节弧度。当前 JSON 精度未认证，不能通过改变加载标志直接用于正式控制。
|
||||||
|
逐字节结果见 `driver_gap.json`。
|
||||||
|
|
||||||
|
**新拇指 roll 的正面图像有重复出现的空间残差。**冻结拟合在训练轮的 thumb IP
|
||||||
|
标签最大前向误差 7.238 px，验证轮独立测角后最大 7.194 px。训练轮字节 0 的
|
||||||
|
thumb IP 标签误差中位数约 2.093 px，字节 255 约 7.114 px；MCP 与 IP 标签在
|
||||||
|
高角度的误差方向接近。高误差在训练和验证轮均出现，继续增加同一模型的优化
|
||||||
|
迭代没有足够依据。旧全手的最坏观测另在顶部 thumb yaw 标签、thumb MCP 任务中，
|
||||||
|
最大 16.468 px，不应混作新拇指 roll 的误差。
|
||||||
|
|
||||||
|
受限的几何假设检验：冻结其他参数后，仅用新批次训练轮拟合一个相对字节 0
|
||||||
|
的 CMC roll 转轴平移，得到局部 y≈+0.052 mm、z≈−1.969 mm。用该值回放独立
|
||||||
|
验证轮，未做每图角度修正的 thumb IP 最大前向误差从 7.672 px 降到 5.261 px，
|
||||||
|
MCP 从 6.793 px 降到 3.057 px；依旧未通过 1.5 px 图像门限。
|
||||||
|
这只能说明“共同上游几何变化”可解释一部分误差，不能区分真实转轴位置、
|
||||||
|
标签随运动的位移或受力形变，更不能据此修改 URDF 的 `origin.xyz`。
|
||||||
|
试探代码及结果见 `probe_thumb_pivot.py` / `thumb_pivot_probe.json`。
|
||||||
|
|
||||||
|
**旧全手参考在新批次的辅助食指端点存在偏差。**新拇指 roll 任务明确令食指侧摆
|
||||||
|
通道保持字节 0，其他拇指任务为 255。该食指标签在验证轮字节 0 的误差中位数
|
||||||
|
5.192 px，在 255 时为 1.158 px。只用新批次训练图像试探一个 +0.974° 的
|
||||||
|
食指字节 0 临时角度修正，验证轮中位误差降到 1.954 px，仍高于门限。
|
||||||
|
这支持旧参考的端点角度有偏差，但不支持用单个辅助保持姿态改写整条四指曲线。
|
||||||
|
旧全手原始采集仍少一个训练方向（107/108），旧参考系统误差也没有完整传入
|
||||||
|
新拇指不确定度。试探代码及结果见 `probe_reference.py` / `reference_probe.json`。
|
||||||
|
|
||||||
|
## 观测条件与下一次采集
|
||||||
|
|
||||||
|
新批次拇指 roll 的三轮稳态窗口中，顶部相机分别保存了 32、29、37 帧，
|
||||||
|
**全部只有固定 `top_base` 标签**；随拇指运动的顶部 `thumb_cmc_yaw` 标签没有
|
||||||
|
一帧有效观测。正面相机同时看到了 thumb MCP/IP 标签。这足以发现投影误差，
|
||||||
|
却不足以从独立视角确认 CMC roll 转轴的三维位置。
|
||||||
|
逐帧来源计数见 `thumb_roll_visibility.json`。
|
||||||
|
|
||||||
|
下一批采集前的顺序：
|
||||||
|
|
||||||
|
1. 在当前拇指 roll 全程的 0、各中间节点和 255，确认顶部或另一独立视角持续
|
||||||
|
看到**随拇指运动的标签**，并且正面仍可看到 MCP/IP 与固定参考。先用静态
|
||||||
|
预览确认可见性，避免整手运动后才发现动态标签缺失。ID17 当前固定在
|
||||||
|
`thumb_proximal`；若能刚性固定在 yaw 子连杆 `thumb_metacarpals_base2` 并保持
|
||||||
|
全行程可见，可减少 MCP 运动对 roll/yaw 观测的混合，但必须更新标签连杆绑定。
|
||||||
|
移动相机需要重测外参；只移动标签需要重估其安装位姿。两者均需建立新批次，
|
||||||
|
不能沿用旧标签安装证据。
|
||||||
|
侧面机位当前仅声明四指运动标签；若用于 yaw，需要侧面实际可见的拇指运动
|
||||||
|
标签与固定参考，并接入 Profile 与离线求解。若侧面与顶部共用物理 ID17，
|
||||||
|
还需让求解器保存各机位观测并共用同一个标签安装参数，不能将两个机位当成
|
||||||
|
两个独立安装。
|
||||||
|
2. 对拇指 roll 的端点分别核对两轮方向、反馈稳定、标签角点和刚性；检查字节 255
|
||||||
|
是否存在遮挡、标签与手指碰撞或安装件受力。当前同一误差在训练/验证重复出现，
|
||||||
|
应优先排查物理条件和 CAD 转轴，再决定是否扩大几何拟合参数。
|
||||||
|
3. 若目标是**正式全手 URDF**，在同一有效外参和安装条件下采完整 108 方向的
|
||||||
|
全手 2＋1 数据。旧批次缺失方向不能用新外参的几帧直接拼成已完成旧批次。
|
||||||
|
新拇指 24/24 可作诊断和拟合初值，但不替代同批全手独立认证。
|
||||||
|
4. 用新批数据先冻结训练解，再独立验收图像、零位、端点和整段曲线；另行验证
|
||||||
|
JSON 曲线驱动的 URDF。若确有多视角证据支持转轴位置偏差，再扩展共同几何
|
||||||
|
模型和 URDF `origin.xyz` 的授权字段，并验证 JSON 回读和最终文件 FK。
|
||||||
|
当前求解器只导出零位角与限位，增加图像本身不会自动修改转轴位置。
|
||||||
|
|
||||||
|
新批次 yaw 字节 16 下降方向的单张正面图像出现 3σ≈1.733° 的测角不确定度。
|
||||||
|
相同保持指令与反馈下，顶部也有三张动态标签图像。将该保持窗口的正面与顶部
|
||||||
|
观测作为同一角度的**诊断**时，3σ≈0.230°；时间跨度约 0.242 s。
|
||||||
|
这表明当前逐张单视角测角方式可能高估该稳态窗口的不确定度，但重复图像存在
|
||||||
|
共同几何误差，不能简单按独立噪声缩小正式置信区间。联合多视角验收须显式
|
||||||
|
处理曝光同步与相关误差；即便解决这项不确定度，前述 7 px 图像残差仍会拒绝发布。
|
||||||
|
数据见 `multiview_angle_probe.json`。
|
||||||
|
|
||||||
|
当前程序生成候选继续保持 `accuracy_passed=false`、`publication_allowed=false`；
|
||||||
|
仿真默认和 `latest_passed` 均未切换。保存的报告保留了每个受影响标签、任务、
|
||||||
|
方向和样本时间戳，见 `old_residual_diagnostics.json` 与
|
||||||
|
`new_residual_diagnostics.json`，便于新批次做同项对照。
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,119 @@
|
|||||||
|
# O30 标定稳定性评估（2026-09-20）
|
||||||
|
|
||||||
|
当前结论：已有精度拒绝和产物发布边界应保留，但现有逐关节准备、立即冻结、
|
||||||
|
再向下游传递的观测组织方式，尚不足以支撑稳定的一次整手标定。
|
||||||
|
不能将局部软件回放通过或某个候选精度达标等同于整手稳定。
|
||||||
|
|
||||||
|
后续修复进展：已实现准备契约隔离、发布入口预算核验、按漂移调整相机刷新间隔、
|
||||||
|
启动锚点年龄扣除及辅助观测继承预算前置检查。详见 `O30_RIGHT_CALIBRATION.md`
|
||||||
|
末节。下文保留评估时的事实和整体目标；不能将这些局部结构修复视为所有重构或
|
||||||
|
整手实机稳定验收已完成。当前独立范围标定占用设备，尚无新版实机验收结果。
|
||||||
|
|
||||||
|
## 最新实机证据
|
||||||
|
|
||||||
|
会话 `calibration_output/O30_RIGHT_001/20260920_175156`，复用 12 个扫描单元后，
|
||||||
|
在小指 DIP 准备阶段以 `OBS-POSE-AMBIGUOUS-117` 暂停。
|
||||||
|
完整证据为该目录的 `assistant_resume_review.json`、`node_status.json`、
|
||||||
|
`raw_samples.jsonl` 第 117841 行及三个 `camera_timing_*.jsonl`。
|
||||||
|
|
||||||
|
| 项目 | 实测结果 | 含义 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| DIP 准备图像 | 228 帧，114 训练 / 114 留出 | 增加采样已实际生效 |
|
||||||
|
| 候选 0 轴向不确定度界 | 2.915°，门限 3° | 合格但裕量很小 |
|
||||||
|
| 候选 1 轴向不确定度界 | 3.035° | 不能发布，但仍能解释图像 |
|
||||||
|
| 两候选留出 RMS | 0.350 / 0.395 px | 像素拟合接近，不足以直接选三维解 |
|
||||||
|
| 校正后的比较 p 值 | 0.09375，要求 ≤ 0.01 | 歧义拒绝符合现有契约 |
|
||||||
|
| 继承 PIP 轴向不确定度界 | 2.274° | 占用下游大部分精度预算 |
|
||||||
|
| 旁观辅助观测 | 两次 `observer_image_validation_failed` | 本次没有提供可用的额外约束 |
|
||||||
|
| SDK | 约 29.8 Hz，无报告故障 | 没有证据把直接停因归为 SDK 故障 |
|
||||||
|
|
||||||
|
`axis_uncertainty_95_rad` 是现有字段名；实现使用局部线性化协方差的三倍标准差界，
|
||||||
|
并保守加入继承项。它不是包含内参、标签系统偏差在内的实测绝对精度保证。
|
||||||
|
|
||||||
|
## 为什么此前修复后仍会停
|
||||||
|
|
||||||
|
1. **改动没有更新整条依赖链。** 密集采样用于新的 DIP 准备；恢复则保留已冻结的
|
||||||
|
旧 PIP 模型。参考复核仅确认当前安装/姿态可以复用，不会把旧模型重新训练成新模型。
|
||||||
|
因而恢复成功和下游可观测并不是同一个条件。
|
||||||
|
2. **单视角平面标签仍有不同三维解释。** 增加同一运动弧上的帧数可以改善随机误差，
|
||||||
|
不保证增加区分两个候选所需的信息。本次两候选都收敛，不能靠增加求解次数解决。
|
||||||
|
也不能删掉略超精度门限、但仍可解释图像的备选候选。
|
||||||
|
3. **辅助观测路径没有预先核算可行性。** `fit_observer_pose` 将父模型不确定度加入
|
||||||
|
辅助相对姿态的界，再要求旋转不超过 1°。旧 PIP 已有约 2.27°，沿用这个来源时，
|
||||||
|
即使像素验收过关也无法满足该界。本次实际首先失败于像素验收，不能把两者混为一项。
|
||||||
|
4. **准备策略的组合复杂度超过已完成的实机覆盖。** 当前存在共享 Tag 交接、父几何
|
||||||
|
传递、旁观辅助、侧摆辅助及进入姿态联合准备等路径，部分组合显式不支持。
|
||||||
|
各路径局部回放通过，不构成它们在整手遮挡、回访和断点恢复中的组合保证。
|
||||||
|
|
||||||
|
现有基准锁定、证据哈希、训练/留出隔离、禁止失败发布和有界恢复是合理的。
|
||||||
|
需要调整的是观测规划、冻结范围、恢复依赖和验收顺序，而不是放宽拒绝条件。
|
||||||
|
|
||||||
|
### 最新像素的父模型敏感性实验
|
||||||
|
|
||||||
|
重新估计历史 PIP 准备模型，并仅在隔离的深拷贝中重算最新一轮 228 帧 DIP 的父姿态，
|
||||||
|
按相同门限求解：PIP 继承轴向界由约 2.274° 降为 1.574°，DIP 候选 0 的轴向界
|
||||||
|
为 2.213°，校正 p 值为 0.002227，离线解析成功。相较原实机的 2.915° / 0.09375，
|
||||||
|
这支持“旧父模型是本次主要限制，必须连同依赖链更新”的判断。
|
||||||
|
|
||||||
|
该实验使用历史 PIP 图像和最新 DIP 图像，没有实机重采，也没有重新完成当前父姿态
|
||||||
|
准入。脚本显式使用离线准入标记，未改变原始记录，不允许发布。
|
||||||
|
不能据此声称实机下次必过，更不能把重新计算的父模型注入旧正式产物。
|
||||||
|
结果和复现脚本存于 `software_review_stability_architecture/latest_parent_counterfactual.json`
|
||||||
|
及同目录 `parent_counterfactual.py`。脚本建立来源注册表时曾两次缺少依赖而被正式入口
|
||||||
|
拒绝，补齐历史 MCP/PIP 来源后才完成实验；拒绝记录保存在该目录的验证摘要中。
|
||||||
|
|
||||||
|
## 独立存在的时钟问题
|
||||||
|
|
||||||
|
约 374 秒内，front / side / top 分别出现 33 / 31 / 27 次时钟失效并重同步；
|
||||||
|
曝光时间序列的最大图像间隔分别约 372 / 338 / 371 ms。
|
||||||
|
这不是此次歧义判定的直接错误码，但会削弱采集连续性，必须单独消除。
|
||||||
|
|
||||||
|
三相机的主机时间与设备计时差在该轮累计同向变化约 201–203 ms，提示应优先检查
|
||||||
|
共用的主机时间基础。另做 30 秒只读探测，墙钟相对 `CLOCK_MONOTONIC_RAW` 变化
|
||||||
|
约 -13.9 ms；主机启用了时间同步。这些现象不足以证明历史失效就是 NTP 导致，
|
||||||
|
也不能据此直接关时间同步或扩大同步容差。
|
||||||
|
|
||||||
|
旧日志仅保存成功的锁存和失效原因，没有保存触发拒绝的锁存事务，缺少定位证据。
|
||||||
|
本次补充了两个独立时钟的事务边界、被拒绝的原始锁存和旧锚点，拒绝时立即落盘。
|
||||||
|
曝光时间映射、2 ms 锁存界、时钟连续性界和图像新鲜度界均未修改。
|
||||||
|
对应相机时序测试 12 项通过，耗时 0.33 秒。这是诊断能力修复，不是时钟故障已解决。
|
||||||
|
|
||||||
|
## 应采用的流程和实施顺序
|
||||||
|
|
||||||
|
1. **先验收基础设施。** 固定本次控制进程的 ROS 发现范围；确保同一设备只有一个
|
||||||
|
控制者。三相机进行覆盖预期标定时长的连续采集检查，以原始双时钟证据定位并修复
|
||||||
|
同步异常。ROS 图隔离不能替代物理设备独占检查。
|
||||||
|
2. **正式训练前完成整条手指的观测准备。** 每条依赖链先检查可见性、运动覆盖、
|
||||||
|
候选可区分性和完整精度预算，再整体接受准备模型。父模型应有经过数据论证的
|
||||||
|
下游预算，不能只按自身 3° 门限提前冻结。
|
||||||
|
3. **独立信息不足时改变预先声明的观测设计。** 优先评估保持其他关节约束的不同
|
||||||
|
上游姿态，必要时采用经过验证的跨视角共同观测。额外动作需纳入运动计划、
|
||||||
|
原始证据和独立留出；不得观察留出结果后反复挑方案直到通过。
|
||||||
|
三台相机存在并不代表当前求解已使用多视角。本轮预检中光学内参和相对外参的
|
||||||
|
实测状态均为 `unverified`，跨视角方案必须先补足对应验证。
|
||||||
|
4. **按依赖链管理准备版本和断点。** 准备策略升级或父模型需要更新时，在新会话中
|
||||||
|
明确重建受影响的父子准备模型及其派生数据；保留无依赖的数据和历史原件。
|
||||||
|
不得将新父模型与旧角度、旧零位或旧发布证据直接拼接。完全冻结后的训练版本
|
||||||
|
不能在正式留出失败后局部重训并继续沿用同一留出验收。
|
||||||
|
5. **统一有限恢复。** 对短暂缺帧执行有界、可回放的局部恢复；基准变化、设备故障、
|
||||||
|
真正不可观测或证据损坏仍应终止/暂停。恢复必须由具体证据触发，同一信息不足的
|
||||||
|
运动不能无限重走。启动索引和重求解保持在实时控制回调之外。
|
||||||
|
6. **最后验证完整交付。** 连续完成整手训练与独立验收，验证 JSON 的证据、数值、
|
||||||
|
单位和关节覆盖，从该 JSON 重建修正 URDF 并回读核验，最后才更新发布指针。
|
||||||
|
末节 CAD 零位保留假设必须继续在产物中明确，不能称所有零位均已独立实测。
|
||||||
|
|
||||||
|
以上是结构调整的实施要求，并非这些重构已经完成。禁止用新增重试、降低门限或
|
||||||
|
跳过候选比较来替代观测设计。
|
||||||
|
|
||||||
|
## 稳定性的验收定义
|
||||||
|
|
||||||
|
- 故障回放覆盖断帧、时钟跳变、SDK 失联、参考移动、候选歧义和恢复版本不兼容；
|
||||||
|
暂态在规定预算内恢复，其余明确拒绝，均无错误发布。
|
||||||
|
- 固定代码、配置和现场布置后，至少连续三次完整实机运行无需人工介入，均完成独立
|
||||||
|
验收并生成可回读的 JSON/URDF，同时报告关节结果重复性。三次是工程验收建议，
|
||||||
|
不是数学上的成功率证明，也未改写当前产品 `required_independent_passes: 1`。
|
||||||
|
- 单独验证正常中断后的断点恢复与一次完整运行结果一致；不能仅验证复用单元数量。
|
||||||
|
- 保留每次失败和软件版本；不能只汇总成功运行或把反事实重放计为实机通过。
|
||||||
|
|
||||||
|
当前尚未达到上述验收。本轮没有发布标定 JSON 或修正 URDF，不能宣称流程稳定或
|
||||||
|
保证下一次一次通过。
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -1,12 +1,112 @@
|
|||||||
# LinkerHand 多型号统一标定
|
# LinkerHand 多型号统一标定
|
||||||
|
|
||||||
G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF
|
G20、L6、O6、O12、O30 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF
|
||||||
验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。
|
验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。
|
||||||
|
|
||||||
本包校准关节零位、有效行程及标准线性 mimic；保留 CAD 连杆尺寸、轴位置、mesh、惯量。
|
本包按观测关节冻结实机 baseline 零位，输出实测曲线及明确声明的复制曲线组成的 JSON v3，
|
||||||
JSON 描述 SDK 输入与修正 URDF 角度的转换，不能补偿错误的 URDF。
|
以及保留线性 mimic 联动的修正 URDF；报告区分独立实测、迁移和 CAD 零位假设。
|
||||||
软件仿真通过不等于实机精度通过；当前原始 G20/O12 存在需要确认的 mimic/限位冲突。
|
保留 CAD 连杆尺寸、轴位置、mesh、惯量。完整运动由 JSON 的全部关节角度配合修正 URDF 计算。
|
||||||
详见 [执行与验收记录](CALIBRATION_REFACTOR_PROGRESS.md)。
|
软件仿真通过不等于实机精度通过；当前原始 G20/L6/O12 存在需要确认的 mimic/限位冲突。
|
||||||
|
本轮实现与验证见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
|
||||||
|
|
||||||
|
O30 右手的构建、相机外参重标和完整操作说明见 [O30 右手标定](O30_RIGHT_CALIBRATION.md)。
|
||||||
|
O30 当前使用反馈判断运动、稳定及零位恢复；允许指令与反馈存在固定偏差。最终 JSON 仍为指令→视觉角度，反馈映射只作诊断。
|
||||||
|
测试的分层、选择范围与耗时记录见 [测试说明](TESTING.md)。
|
||||||
|
|
||||||
|
## O30 当前默认：轻量采集，离线修正
|
||||||
|
|
||||||
|
默认 `raw_joint_2_plus_1` / `raw_joint_images_v2`：移动标签仅保存二维角点，
|
||||||
|
每任务两轮训练加一轮验证；当前失败方向立即补采一次，仍失败记录缺项并继续。
|
||||||
|
采完归位、封存数据、关闭控制栈，终态 `CAPTURE_COMPLETE` 不等于 URDF 通过。
|
||||||
|
后续使用 `--offline-raw` 读取同一批数据修正，失败不再触发运动。
|
||||||
|
启动、离线命令和协议边界见 [O30 轻量标定说明](O30_RAW_PROTOCOL_IMPLEMENTATION.md)。
|
||||||
|
以下分阶段和自适应章节为历史显式试验入口，不是 O30 当前默认流程。
|
||||||
|
|
||||||
|
## O30 分阶段训练试验
|
||||||
|
|
||||||
|
新增显式选项 `--training-policy staged_2_to_3`，该历史策略以 `fixed` 为对照。Tag 数量、编号、尺寸、位置、
|
||||||
|
相机布局、完整网格、运动速度和验收门限不变。新策略按原任务顺序执行全手轮次 0、全手轮次 1，
|
||||||
|
进行训练检查；必要时仅集中补一次轮次 2，随后冻结整手模型，最后采集独立验证轮次 3。
|
||||||
|
局部零位补采包含同轮几何依赖，分段任务整体补采，共享掌部问题扩大到全手。补采后仍不合格即失败。
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 仅校验配置；不连接、启动硬件
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand --config \
|
||||||
|
src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--training-policy staged_2_to_3 --validate-only
|
||||||
|
```
|
||||||
|
|
||||||
|
准备阶段会保留已有上游运动中可见的相关末节 Tag 原始角点及合法候选；缺失旁观 Tag 不阻塞
|
||||||
|
上游任务。末节保持指令、反馈、参考和独立图像检查全部合格后，测得的相对位姿才能约束后续准备。
|
||||||
|
上游图像及几何不确定度进入验收；原有图像无法区分的姿态仍拒绝。旧日志不会补造缺失角点。
|
||||||
|
|
||||||
|
回访复核原零位后复用原模型。训练进程、任务索引和局部拟合在会话内复用；冻结文件
|
||||||
|
`frozen_training.json` 绑定配置、源 URDF、计划、输入证据和求解版本。在线最终验收不重新训练；
|
||||||
|
离线审计仍复算决定。新策略断点仅复用完整任务访问，重试额度跨断点保留，冻结后不回退到训练。
|
||||||
|
同一会话的 `raw_samples.jsonl` 和 `frozen_training.json` 应一起保留，离线回放须指定相同策略。
|
||||||
|
|
||||||
|
两轮全部通过时，扫描单元从 144 减到 108、正式稳态停点从 1390 减到 1052；这不是总耗时降幅。
|
||||||
|
阶段重排增加回访和参考复核，实机对照须计入这些时间。`stage_timing.json` 区分阶段耗时、开始到
|
||||||
|
终态耗时及开始前断点加载耗时，终态后等待退出不计入开始到终态指标。独立验证失败不得回炉训练。
|
||||||
|
JSON→URDF 重建、文件与像素验收、成对发布、失败不更新通过指针，以及五个末节 CAD 零位声明均保留。
|
||||||
|
|
||||||
|
软件检查与实机验收分开记录。完整实机通过、人工干预不增加且同条件总耗时至少降低 10% 后，
|
||||||
|
才能评估切换默认策略；本次软件实现不自动启动硬件。实现与验收说明见
|
||||||
|
[分阶段标定实施记录](STAGED_CALIBRATION_IMPLEMENTATION.md)。
|
||||||
|
|
||||||
|
## O30 自适应训练试验
|
||||||
|
|
||||||
|
该历史策略以 `fixed` 为对照：三轮训练加一轮独立验证。新会话可显式选择 `adaptive_2_to_3`：两轮训练后
|
||||||
|
检查覆盖、稳态网格、双向重复性和可计算的几何零位质量；证据不足补第三轮，第三轮仍不合格
|
||||||
|
就失败。每个方向的缺样恢复仍最多同速重扫一次，准备缺样仍最多局部恢复一次。
|
||||||
|
速度、双向运动顺序、节点、准备路径及最终精度门限均保留。
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 只检查配置，不连接硬件
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand --config \
|
||||||
|
src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--training-policy adaptive_2_to_3 --validate-only
|
||||||
|
|
||||||
|
# 完成现场准备后，显式开始全新试验会话
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand --config \
|
||||||
|
src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--training-policy adaptive_2_to_3 --no-resume
|
||||||
|
```
|
||||||
|
|
||||||
|
旧 `adaptive_2_to_3` 顺序中，O30 掌部共享几何依赖最后采集的四指侧摆。前面的任务无法可靠提前计算零位置信区间，
|
||||||
|
因此保守保留第三轮；最后一个任务满足全部门限时才减轮。本版完整计划的稳态停点最多从
|
||||||
|
1390 降到 1340（约 3.6%），不承诺总耗时同比下降。所有任务都省一轮时的 1052 个停点只是
|
||||||
|
理论下界，该旧顺序下不能实现。上面的分阶段策略在两轮全手证据齐全后统一判断，不受此逐任务调度限制。
|
||||||
|
|
||||||
|
采集计划 `capture_plan_v1` 绑定任务、运动分段、准备路径、网格与实际训练/验证身份。
|
||||||
|
轮次 ID 保持不变：减轮后依次采集 0、1、3，界面显示第 1、2、3 轮；ID 3 始终为独立验证。
|
||||||
|
`training_decision` 保存增补/冻结/失败决定、输入与计划哈希、冻结局部模型哈希及统计。
|
||||||
|
几何依赖未齐时明确记录 `deferred`，不能将局部曲线一致性冒充零位认证。
|
||||||
|
零位仍按真实独立轮次计算 Student t 置信区间，1° 半宽不变；曲线重复性使用既有角度误差门限。
|
||||||
|
重复图像不会增加独立轮数，验证数据不参与训练、增补决策或模型选择。
|
||||||
|
|
||||||
|
最终产物仍依次经过训练冻结、独立验证、落盘 JSON、从 JSON 重建 URDF、图像与运动学回读、
|
||||||
|
成对发布。Tag 安装拟合及运动范围证据也使用实际训练轮集合。五个末节 CAD 零位假设及已验证
|
||||||
|
运动范围的精度声明保留；失败候选不会更新通过指针。
|
||||||
|
|
||||||
|
旧断点沿用固定策略，新旧策略不能拼接。自适应断点只复用连续完整任务；需要重做某个任务时，
|
||||||
|
其后依赖旧训练输入的任务一并重新采集。离线复算自适应会话也须传入同一 `--training-policy`。
|
||||||
|
原始日志与已发布产物不被改写。训练预检在有总时限的独立进程执行，超时或取消会清理子进程。
|
||||||
|
新会话的 `stage_timing.json` 按控制周期边界汇总准备、几何计算、等待、采样、恢复、断点加载及最终验收耗时。
|
||||||
|
|
||||||
|
已有固定三轮训练日志可作只读对照，保留同一独立验证集：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
PYTHONPATH=src/linkerhand_calibration python3 -m linkerhand_calibration.runtime.training_replay \
|
||||||
|
--raw-samples calibration_output/O30_RIGHT_001/20260919_214048/raw_samples.jsonl \
|
||||||
|
--source-urdf src/linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf \
|
||||||
|
--output /tmp/o30-training-comparison.json
|
||||||
|
```
|
||||||
|
|
||||||
|
此工具复用采集索引，仅比较已完成任务的训练与角度验证，不生成发布产物，也不认证全手零位。
|
||||||
|
2026-09-19 21:40 会话的 12 个完整任务在两种训练轮数下均通过原独立角度门限；其共享几何尚未
|
||||||
|
齐全，不能据此批准两轮零位标定。约 2.88 GB 日志索引加载实测约 25 秒，作为后续性能工作的基线；
|
||||||
|
本次没有改动日志存储格式。
|
||||||
|
|
||||||
## 启动
|
## 启动
|
||||||
|
|
||||||
@@ -25,44 +125,316 @@ ros2 run linkerhand_calibration calibrate_hand --config \
|
|||||||
src/linkerhand_calibration/config/o12_right_product.yaml
|
src/linkerhand_calibration/config/o12_right_product.yaml
|
||||||
```
|
```
|
||||||
|
|
||||||
其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`。
|
其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`、`o30_right_product.yaml`。
|
||||||
|
AprilTag 检测参数直接读取产品指定的受保护 Tag YAML，统一启动文件不再覆盖其中的采样分辨率。
|
||||||
`calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。
|
`calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。
|
||||||
`--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。
|
`--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。
|
||||||
|
产品配置的 `resume_mode: passed` 表示操作者确认安装位置未变：只读取已通过的完整任务，
|
||||||
|
不回放历史图像、不重新判定旧采样质量，也不复核已完成关节的物理零位；未完成任务整组重新采集。
|
||||||
|
O30 当前使用此模式。配置/设备身份核对、实时保护和最终产物验收仍执行，恢复策略写入原始日志。
|
||||||
|
默认 `resume_mode: verify` 会逐帧回读校验历史图像；若大断点超过默认 120 秒，可用
|
||||||
|
`--startup-timeout-seconds 600` 显式延长节点初始化及设备就绪等待。此参数不改变运动、反馈新鲜度、
|
||||||
|
堵转保护或精度验收门槛，也不会跳过断点基准和关节零位复核。
|
||||||
|
断点预校验会共享不可变模型的解析结果，并在至少 256 帧时使用最多 4 个独立进程回放图像；
|
||||||
|
工作进程只做离线计算，在实时采集开始前退出。每帧角点、姿态、样本绑定及原始记录的内容哈希仍完整检查，
|
||||||
|
不跨会话缓存校验结论；小断点及收尾进程沿用串行校验。
|
||||||
|
标定节点在启动校验后将长期驻留的对象移出循环垃圾回收扫描，避免大断点触发数秒停顿；
|
||||||
|
实时新增对象仍正常回收，节点退出后恢复原对象的回收管理。反馈超时保护不变。
|
||||||
重新开始时先安全回基准，仍必须确保现场没有障碍物。
|
重新开始时先安全回基准，仍必须确保现场没有障碍物。
|
||||||
|
|
||||||
|
### 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 | 输出格式 |
|
| Profile | SDK 位置通道 | 扫描任务 | Tag | 输出格式 |
|
||||||
| --- | ---: | ---: | ---: | --- |
|
| --- | ---: | ---: | ---: | --- |
|
||||||
| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v2，256 项指令查表 |
|
| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v3，256 项指令查表 |
|
||||||
| L6/right/l6_right_8/v1 | 6 | 3 | 8 | unified v2，256 项指令查表 |
|
| L6/right/l6_right_8/v1 | 6 | 3 | 8 | unified v3，256 项指令查表 |
|
||||||
| O6/right/o6_right_8/v1 | 6 | 3 | 8 | unified v2，256 项指令查表 |
|
| O6/right/o6_right_8/v1 | 6 | 3 | 8 | unified v3，256 项指令查表 |
|
||||||
| O12/right/o12_right_16/v1 | 12 | 11 | 16 | unified v2，rad 输入节点查表 |
|
| O12/right/o12_right_16/v1 | 12 | 11 | 16 | unified v3，rad 输入节点查表 |
|
||||||
|
|
||||||
任务数、主动关节数和实测关节数不必相等。一条运动可观测多个主动/被动关节。
|
任务数、主动关节数和实测关节数不必相等。一条运动可观测多个主动/被动关节。
|
||||||
L6/O6 未贴 Tag 的同机构关节使用显式迁移，不能把迁移称为独立实测。
|
观测策略由 `measurement.transferred_motion_sources` 声明，目标关节指向直接实测的来源关节：
|
||||||
每任务四个往返周期：前三轮训练，第四轮独立验证。
|
|
||||||
每个方向先连续扫描，再单通道访问稳态点。训练每方向默认 9 点，第四轮为交错点加端点；
|
| 型号 | 独立采集 | 复制的运动映射 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| O6/L6 | 拇指、小指 | 食指/中指/无名指的 MCP、DIP 分别复制小指对应关节 |
|
||||||
|
| O12 | 拇指、小指、中指、食指 | 无名指 MCP/PIP/DIP 分别复制小指对应关节 |
|
||||||
|
| G20 / 全关节模式 | 每个关节 | 不声明复制关系 |
|
||||||
|
|
||||||
|
复制保留接收关节自己的 SDK 通道、URDF 名称与 CAD 几何，不要求接收关节另贴 Tag 或采集零位帧。
|
||||||
|
复制参数不等于接收关节独立实测；接收关节仍须有明确的绝对零位来源及合法机械范围。
|
||||||
|
`zero.cad_zero_assumptions` 显式记录“实机 baseline 与原 CAD 末节零位一致”的使用前提，
|
||||||
|
相应 DIP（及拇指末节 IP）δ=0、自身 `origin` 不改。这是假设，不是视觉测得的绝对 CAD 朝向。
|
||||||
|
按用户确认，O12 的 `pinky_pip`、`ring_pip` 和主动 `thumb_mcp` 也采用原始 URDF 零位作为 baseline 假设。
|
||||||
|
运动曲线仍按实际观测／明确复制关系生成，报告标记 `assumed_source_cad_zero`，不称为独立实测零偏。
|
||||||
|
正式入口在驱动硬件前检查运动覆盖及零位来源，缺失依据时直接指出缺项，不采完才发现无法发布。
|
||||||
|
启动检查分别列出 `unmeasured_joints`（未独立观测）、`unresolved_motion_joints`（也未声明复制）
|
||||||
|
及 `missing_baseline_geometry`。旧迁移字段单独存在不会自动授权新的 v3 曲线复制。
|
||||||
|
本次沿用现有 Tag 和避让布局声明延后零位动作，没有把新增动作或缺失 Tag 布局标为实机验证通过。
|
||||||
|
每任务的运动模型采集四个往返，共八趟单程：前三轮训练，第四轮独立双向验证。
|
||||||
|
四个产品配置统一使用 `command_capture_mode: separate`：这八趟均为完整连续扫描，随后另做一组稳态映射训练往返和一组独立验证往返。
|
||||||
|
主扫描不逐点停顿；稳态点直接执行移动、稳定反馈、新图像采样，不重复等待两次到位。
|
||||||
|
稳态训练点由前三轮连续数据生成 `command_sampling_plan`：在原节点网格上寻找最少的
|
||||||
|
保留节点，各关节、各方向的预测曲线插值变化均不超过 0.25°；端点、基准、中间支撑点及
|
||||||
|
显式增加的非线性节点保留。缺少完整训练数据或曲线噪声过大时使用原网格。
|
||||||
|
这份计划只决定停在哪里，每个保留点的静态角度仍由新图像独立测量，不用连续运动角度
|
||||||
|
冒充静态角度。独立验证网格保持原样，不参与节点选择。计划及训练来源哈希随原始记录保存，
|
||||||
|
拟合、离线回放和断点恢复均重新核验；缺点、篡改或最终误差超限仍禁止发布。
|
||||||
|
上一段已经完成到位/稳定检查且下一准备目标完全相同时，可复用一秒内、反馈未变化的
|
||||||
|
完整保持姿态，省去重复准备等待；有实际位移、反馈变化或记录过期时执行原准备流程。
|
||||||
|
原训练网格每方向通常为 9 点，并保留行程内的 baseline；O6 小指额外包含 239，共 10 点。
|
||||||
|
新流程在这份网格上按上述规则精简训练停点。
|
||||||
|
验证使用交错点加端点（O6 小指包含 247），两类数据不混用。
|
||||||
|
日志中的运动轮编号为 0–3，独立指令映射训练和验证编号为 4、5；界面分别显示“稳态指令映射训练／独立验证”。
|
||||||
|
其他采用 `interleaved` 的配置仍在四轮各方向中穿插稳态点，并使用三轮稳态训练、第四轮稳态验证。
|
||||||
|
行进图像用于旋转/几何拟合，停稳后的新图像用于 SDK 指令映射；同一张图像不会复制成两类样本。
|
||||||
每点至少 3 个稳定同步图像，轨迹结束后最多等待 2 秒，不能用精确指令/反馈相等判断稳定。
|
每点至少 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。
|
字节拟合不再改写原始 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` 检查相机配置文件、序列号、内外参指纹和已有外参质量记录，不采集现场图像。
|
||||||
|
正式启动还检查实时 `CameraInfo` 的宽高及 K/D/R/P 是否与受保护外参中的记录一致；
|
||||||
|
参数不匹配、无效或消息过期时不能开始，并显示具体机位和原因。
|
||||||
|
|
||||||
|
**配置匹配不等于现场内外参有效。** 当前只有手上的 Tag，各机位使用不同 Tag，
|
||||||
|
没有声明跨机位共视或已知的 Tag 间几何关系；手掌和贴纸又允许在开始前移动、重贴。
|
||||||
|
因此光学内参与相对外参的现场有效性记录为 `unverified`，不能用重新锁定掌心 Tag、
|
||||||
|
较小的 PnP 残差或未变化的配置哈希代替实测，也不能由 Tag 位置变化断定相机被碰。
|
||||||
|
|
||||||
|
`READY` 只表示设备与消息满足现有开始条件。显示上述能力提示后沿用现有启动行为，
|
||||||
|
不要求各手指 Tag 可见，不增加人工确认或新的暂停条件；`READY` 不表示物理相机参数已验证通过。
|
||||||
|
会话目录中的 `camera_preflight.json` 和 `raw_samples.jsonl` 保存检查结果与依据，
|
||||||
|
开始前仅在结果变化时记录，接受 Start 前再检查并冻结一次；后续状态中的检查结果指这次启动检查。
|
||||||
|
单凭这些记录不能判定是否需要重新标定。`expected_cameras` 是配置期望身份，不是现场实测结果。
|
||||||
|
|
||||||
|
镜头、焦距或成像设置变化后应重新核验内参；相机之间相对位置变化后应重新核验外参。
|
||||||
|
未来同一个物理 Tag 被多个机位共同看到时，可以检查相对外参一致性，但当前尚未实现该能力。
|
||||||
|
本次检查不修改 Profile、SDK 或原始 URDF。
|
||||||
|
|
||||||
## 基准、避让与恢复
|
## 基准、避让与恢复
|
||||||
|
|
||||||
- 开始前可以调整手掌、相机和 Tag；预览数据不会成为正式参考。
|
- 开始前可以调整手掌、相机和 Tag；预览数据不会成为正式参考。
|
||||||
- 开始后清空预览缓存，先安全回基准，再为每机位固定 Tag 收集至少 10 个有效帧。
|
- 开始后清空预览缓存，先安全回基准，再为每机位固定 Tag 收集至少 10 个有效帧。
|
||||||
缺少基准时保持并显示缺少的 ID，不因为等待时间长暂停。
|
缺少基准时保持并显示缺少的 ID，不因为等待时间长暂停。
|
||||||
- 锁定后禁止人工移动手掌、相机、支架、Tag 粘贴位置；程序驱动关节和避让仍是允许的。
|
- 锁定后禁止人工移动手掌、相机、支架、Tag 粘贴位置；程序驱动关节和避让仍是允许的。
|
||||||
|
- 全手 baseline 时只要求固定参考可见，不要求四指运动 Tag 同时可见。
|
||||||
|
每关节在 `motion.joint_zero_references` 指定任务前，按声明的到达姿态和避让分组回到自身 baseline，
|
||||||
|
反馈稳定后收集至少 10 个稳定视觉帧，冻结 `JointZeroReference`。此后方向、轮次、重扫均不重新归零。
|
||||||
|
例如 G20 食指开始前，已完成四轮的其他手指可以保持非零避让；这些完整指令和反馈写入参考。
|
||||||
|
task.start 不代表零位，例如侧摆 baseline=127、扫描起点=255，会分别执行两种姿态。
|
||||||
- 相机相对位置改变时须重新确认/标定外参；仅重新锁定掌心 Tag 不能替代外参标定。
|
- 相机相对位置改变时须重新确认/标定外参；仅重新锁定掌心 Tag 不能替代外参标定。
|
||||||
- 固定 Tag 被避让遮住时可用已锁定参考继续采集；软件不能保证检测完全不可见对象的移动。
|
- 固定 Tag 被避让遮住时可用已锁定参考继续采集；软件不能保证检测完全不可见对象的移动。
|
||||||
运动 Tag 的刚性和几何一致性仍须通过最终独立验证。
|
运动 Tag 的刚性和几何一致性仍须通过最终独立验证。
|
||||||
- 断点版本为 `unified_engine_v4_dual_mapping`，不复用缺少稳态证据的旧 v1/v2/v3。
|
- 采集断点版本为 `unified_engine_v8_all_view_images`；另以 `pose_tracking_policy_version=confirmed_branch_v8_training_model_selection`
|
||||||
新断点检查配置哈希、基准及所复用 Tag 安装关系（5 px、2°、5 mm）。缺失或不一致即放弃复用，
|
记录姿态分支策略。缺少此字段或版本不同的旧断点不能自动或手工恢复到新会话，历史文件保持原样供离线诊断。
|
||||||
自动完整重采，旧会话不修改。恢复时重新执行准备与避让，不直接跳到旧姿态。
|
首先检查配置哈希与整场固定参考；各关节轮到时，在同一完整保持姿态核对原零位与父子 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 朝向。
|
||||||
|
`zero.known_baseline_geometry` 可声明有独立依据的 CAD 角度、误差界与来源；不能填入猜测的 0、旧 mimic
|
||||||
|
或 Tag 安装偏角冒充几何证据。相应 `origin.rpy` 和限位修正仍须在 Profile 中明确授权。
|
||||||
|
|
||||||
O12 避让：小指和无名指到 Profile 的最大弯曲指令；标定食指前，中指 MCP/PIP 到最大弯曲指令，
|
O12 避让：小指和无名指到 Profile 的最大弯曲指令；标定食指前，中指 MCP/PIP 到最大弯曲指令，
|
||||||
中指侧摆为 0 rad。到位采用完成轨迹、正确方向、至少 80% 请求位移和短时稳定，
|
中指侧摆为 0 rad。到位采用完成轨迹、正确方向、至少 80% 请求位移和短时稳定，
|
||||||
不要求反馈数值精确等于命令。SDK 指令最大值与实测 URDF 角度不是同一个量。
|
不要求反馈数值精确等于命令。SDK 指令最大值与实测 URDF 角度不是同一个量。
|
||||||
|
|
||||||
O12 使用 HCAN device 0/channel 0 的已锁定 vendor Python wheel，不检查 `can0`。
|
O12 使用 HCAN device 0/channel 0 的已锁定 vendor Python wheel，不检查 `can0`。
|
||||||
|
连接参数由本包的 `config/o12_sdk.yaml` 保存，并通过产品配置中的 SHA256 校验；
|
||||||
|
不依赖第三方 SDK 子仓库中未提交的示例 YAML 修改。
|
||||||
只读反馈桥可在第一次运动命令之前取得状态，不会发送零位命令来“激活回读”。
|
只读反馈桥可在第一次运动命令之前取得状态，不会发送零位命令来“激活回读”。
|
||||||
POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 错误位过热保护，不伪造温度。
|
POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 错误位过热保护，不伪造温度。
|
||||||
字节 SDK 当前没有独立硬件故障遥测，状态会明确注明这一能力边界。
|
字节 SDK 当前没有独立硬件故障遥测，状态会明确注明这一能力边界。
|
||||||
@@ -70,8 +442,13 @@ POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK
|
|||||||
## 哪些情况暂停
|
## 哪些情况暂停
|
||||||
|
|
||||||
实时仅因人工中止、竞争控制器、活动硬件故障/模式错误、真实失联、反馈超过 1 秒未更新、
|
实时仅因人工中止、竞争控制器、活动硬件故障/模式错误、真实失联、反馈超过 1 秒未更新、
|
||||||
物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。
|
物理越限、明显运动要求下超过 Profile 的无推进等待时间（O30 为 4 秒，其他型号为 2 秒）、
|
||||||
|
已锁固定基准连续 10 帧漂移超过 5 px 而暂停。
|
||||||
开始后相机投影参数变化也会拒绝继续使用混合坐标数据。
|
开始后相机投影参数变化也会拒绝继续使用混合坐标数据。
|
||||||
|
当前关节持续缺失零位观测会暂停并列出关节、机位和所需 Tag；尚未轮到的手指遮挡不阻塞当前任务。
|
||||||
|
零位和稳态点的反馈稳定窗口以最新独立反馈为终点，并保留跨越窗口起点的一条反馈。
|
||||||
|
控制定时器的调度抖动或重复读取同一反馈不会清空稳定观测；真实反馈波动或反馈过期仍会使当前窗口失效。
|
||||||
|
零位仍需至少 10 帧且姿态波动不超过 1°；拟合验收门限不变。
|
||||||
|
|
||||||
短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动
|
短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动
|
||||||
不会实时中断轨迹。无效视觉帧直接丢弃。
|
不会实时中断轨迹。无效视觉帧直接丢弃。
|
||||||
@@ -79,18 +456,153 @@ POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK
|
|||||||
所有型号的独立 Tag 姿态筛选共用 `core/geometry/pnp.py` 中的近似同误差阈值，默认 `0.03 px`。
|
所有型号的独立 Tag 姿态筛选共用 `core/geometry/pnp.py` 中的近似同误差阈值，默认 `0.03 px`。
|
||||||
ROS 和离线采集使用相同默认值，型号 YAML 不重复声明该值。`1.5 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、规定的有效行程覆盖，
|
方向结束才检查 ≥40 个有效同步样本、≥32 分箱、内部空白 ≤行程的 1/16、规定的有效行程覆盖，
|
||||||
以及 Profile 声明的端点/双视角观测。数据不足只同速重扫一次；第二次失败暂停。
|
以及 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、命令/反馈/单位/速度、
|
所有型号显示相同进度：阶段、任务/轮次/方向、分机位 Tag ID、命令/反馈/单位/速度、
|
||||||
有效数据、基准、断点来源，以及中文原因、建议和原始诊断。耗时拟合在后台执行，不阻塞反馈与状态。
|
有效数据、基准、断点来源，以及中文原因、建议和原始诊断。
|
||||||
|
ROS 收尾拟合在独立进程执行：只读取采集结束时已同步的日志前缀，后续图像不进入拟合；
|
||||||
|
主进程继续接收反馈并检查固定基准，阶段事件按原顺序回传，最终发布仍由主进程持锁完成。
|
||||||
|
中止时终止计算子进程，不允许子进程更新发布指针。`finalization_timing.json` 保存实际耗时。
|
||||||
标题明确显示型号、左右手和序列号。开始前等待状态列出缺少或过期的相机内参、检测消息和 SDK 条件；
|
标题明确显示型号、左右手和序列号。开始前等待状态列出缺少或过期的相机内参、检测消息和 SDK 条件；
|
||||||
空 Tag 检测消息可以证明视觉链路工作，不要求开始前已经识别全部 Tag。
|
空 Tag 检测消息可以证明视觉链路工作，不要求开始前已经识别全部 Tag。
|
||||||
收到开始请求时重新检查设备状态；相机有效内参必须与受保护外参中的指纹一致。
|
收到开始请求时重新检查设备状态；实时 CameraInfo 参数必须与受保护外参中的指纹一致。
|
||||||
开始前相机消息的新鲜度窗口为 2 秒，该窗口不用于扫描中的实时暂停。
|
开始前相机消息的新鲜度窗口为 2 秒，该窗口不用于扫描中的实时暂停。
|
||||||
|
|
||||||
## 唯一生产代码链
|
## 唯一生产代码链
|
||||||
@@ -104,7 +616,7 @@ linkerhand_calibration/
|
|||||||
├── profiles/ 加载、静态验证、Tag/CAD 观测关系
|
├── profiles/ 加载、静态验证、Tag/CAD 观测关系
|
||||||
├── core/domain/ Profile、样本、CalibrationResult、状态
|
├── core/domain/ Profile、样本、CalibrationResult、状态
|
||||||
├── core/geometry/ 相机、变换、PnP 兼容导出
|
├── core/geometry/ 相机、变换、PnP 兼容导出
|
||||||
│ └── tag_pose/ IPPE、连续跟踪、刚性组、轨迹分支选择
|
│ └── tag_pose/ IPPE、连续跟踪、冻结前运动证据、刚性组和轨迹选择
|
||||||
├── core/fitting/ 运动曲线、线性耦合、固定安装
|
├── core/fitting/ 运动曲线、线性耦合、固定安装
|
||||||
│ └── spatial_solver/ 空间观测、基座、零位、统计与独立验收
|
│ └── spatial_solver/ 空间观测、基座、零位、统计与独立验收
|
||||||
├── core/urdf/ 授权修正、FK、范围与最终文件验收
|
├── core/urdf/ 授权修正、FK、范围与最终文件验收
|
||||||
@@ -113,13 +625,18 @@ linkerhand_calibration/
|
|||||||
├── runtime/parameters.py 已解析的运行参数；无 ROS 类型
|
├── runtime/parameters.py 已解析的运行参数；无 ROS 类型
|
||||||
├── runtime/inputs.py 相机/检测输入、时钟与发布接口
|
├── runtime/inputs.py 相机/检测输入、时钟与发布接口
|
||||||
├── runtime/cameras.py 受保护相机身份与消息新鲜度
|
├── runtime/cameras.py 受保护相机身份与消息新鲜度
|
||||||
|
├── runtime/camera_preflight.py 启动前配置检查结果与内外参实测能力边界
|
||||||
├── runtime/session.py 唯一在线状态机
|
├── runtime/session.py 唯一在线状态机
|
||||||
├── runtime/execution.py Profile 任务与运动效果调度
|
├── runtime/execution.py Profile 任务与运动效果调度
|
||||||
├── runtime/motion_execution.py 平滑轨迹与位移到位判定
|
├── runtime/motion_execution.py 平滑轨迹与位移到位判定
|
||||||
├── runtime/capture.py 多机位视觉/反馈采集
|
├── runtime/capture.py 多机位视觉/反馈采集
|
||||||
|
├── runtime/branch_initialization.py 最后归零准备段证据与一次性几何确认
|
||||||
|
├── runtime/branch_tracking.py 冻结约束选候选，原子提交同帧姿态与证据
|
||||||
|
├── runtime/motion_provenance.py 读取和收尾核验来源内容、零位与样本关联
|
||||||
├── runtime/scan_quality.py 唯一方向数据门
|
├── runtime/scan_quality.py 唯一方向数据门
|
||||||
├── runtime/reference_lock.py 开始后固定基准
|
├── runtime/reference_lock.py 开始后固定基准
|
||||||
├── runtime/resume.py 唯一断点证据检查
|
├── runtime/resume.py 整场断点证据检查
|
||||||
|
├── runtime/joint_resume.py 逐关节原零位验证后导入旧样本
|
||||||
├── runtime/safety.py 最小实时保护
|
├── runtime/safety.py 最小实时保护
|
||||||
├── runtime/status.py 统一状态与中文显示
|
├── runtime/status.py 统一状态与中文显示
|
||||||
├── runtime/snapshot.py 类型化状态快照与旧状态映射
|
├── runtime/snapshot.py 类型化状态快照与旧状态映射
|
||||||
@@ -140,7 +657,8 @@ linkerhand_calibration/
|
|||||||
|
|
||||||
PnP 的原导入路径继续可用。`tag_pose/parameters.py` 集中定义跟踪默认值，
|
PnP 的原导入路径继续可用。`tag_pose/parameters.py` 集中定义跟踪默认值，
|
||||||
ROS 加载器将角度参数由 deg 转为 rad，离线采集直接使用同一套默认值。
|
ROS 加载器将角度参数由 deg 转为 rad，离线采集直接使用同一套默认值。
|
||||||
公共分支容差仍为 `0.03 px`，重投影质量、倾角、过滤原因和连续跟踪恢复策略保持一致。
|
公共分支容差仍为 `0.03 px`，重投影质量和倾角限制保持一致；首次确认、参考冻结与失联恢复
|
||||||
|
使用同一公共分支策略，ROS 和离线链路均不能绕过它。
|
||||||
|
|
||||||
`CalibrationSession.phase` 是流程状态来源。协调器在状态锁内开始、中止、推进和提交观测；
|
`CalibrationSession.phase` 是流程状态来源。协调器在状态锁内开始、中止、推进和提交观测；
|
||||||
耗时姿态计算在锁外进行，提交时核对会话版本、采集对象、运动对象及稳态采样边界。
|
耗时姿态计算在锁外进行，提交时核对会话版本、采集对象、运动对象及稳态采样边界。
|
||||||
@@ -167,8 +685,8 @@ Profile 声明：
|
|||||||
|
|
||||||
1. Adapter、SDK 通道顺序、单位、正负方向、命令/反馈范围、基准值、独立速度槽布局。
|
1. Adapter、SDK 通道顺序、单位、正负方向、命令/反馈范围、基准值、独立速度槽布局。
|
||||||
2. Tag ID/尺寸/机位/安装 link，以及每次运动的父子观测关系；不明确的 link 不能靠名称猜。
|
2. Tag ID/尺寸/机位/安装 link，以及每次运动的父子观测关系；不明确的 link 不能靠名称猜。
|
||||||
3. 扫描任务、速度、准备/避让/收尾 waypoint；必要时指定不可见 Tag 的恢复验证姿态。
|
3. 扫描任务、速度、准备/避让/收尾 waypoint；每关节首次零位任务、完整保持指令及固定到达姿态。
|
||||||
4. 哪些零位可观测、哪些固定保留 CAD、哪些参数同机构迁移；声明空间观测约束和拟合原语。
|
4. 声明空间可观测约束、已知 baseline 几何或明确接受的 CAD 零位假设；每个运动关节须独立观测或显式指定对应实测来源。
|
||||||
5. 每关节允许修正的 `origin.rpy`、`limit.lower/upper`、`mimic.multiplier/offset`。
|
5. 每关节允许修正的 `origin.rpy`、`limit.lower/upper`、`mimic.multiplier/offset`。
|
||||||
|
|
||||||
这仍需要正确的可观测性设计：单轴加一对未知安装角的 Tag 不能自动辨识绝对 CAD 零位。
|
这仍需要正确的可观测性设计：单轴加一对未知安装角的 Tag 不能自动辨识绝对 CAD 零位。
|
||||||
@@ -177,10 +695,10 @@ Profile 声明：
|
|||||||
|
|
||||||
复用 SDK 不增加 Python；新协议只增加 `runtime/adapters/` 实现及协议工厂注册。
|
复用 SDK 不增加 Python；新协议只增加 `runtime/adapters/` 实现及协议工厂注册。
|
||||||
没有型号节点、runner、状态机、质量/断点/发布器；新数学只能增加通用拟合原语。
|
没有型号节点、runner、状态机、质量/断点/发布器；新数学只能增加通用拟合原语。
|
||||||
新型号使用 `artifacts.output_schema_version: 2` 选择统一指令查表格式；产物自身为
|
新型号使用 `artifacts.output_schema_version: 3`；产物为
|
||||||
`format: unified_calibration_v2, schema_version: 2`。详细双映射保留在独立标定报告中。
|
`format: unified_calibration_v3, schema_version: 3`。详细双映射保留在独立标定报告中。
|
||||||
旧 generic v1、v4/v6/v7 只保留历史兼容工具，不把反馈表改名为指令表来伪造兼容。
|
旧 generic v1、v4/v6/v7 只保留历史兼容工具，不把反馈表改名为指令表来伪造兼容。
|
||||||
正式 Profile 校验及 finalizer 只接受 `output_schema_version: 2`，不能用旧格式跳过指令映射拟合。
|
v1/v2 按原语义读取，不会自动升级为独立实测；保留的 v2 数学入口用于历史回归，不能证明 v3 精度。
|
||||||
配置错字、错误绑定、未知修正字段在启动前失败；修改受保护文件后应审查并更新对应 SHA256。
|
配置错字、错误绑定、未知修正字段在启动前失败；修改受保护文件后应审查并更新对应 SHA256。
|
||||||
|
|
||||||
## 产物与离线验证
|
## 产物与离线验证
|
||||||
@@ -189,16 +707,72 @@ Profile 声明：
|
|||||||
有效发布由 `release_manifest.json` 及产品发布指针共同证明；单独的 `PASS` 摘要或存在 URDF 文件不等于已发布。
|
有效发布由 `release_manifest.json` 及产品发布指针共同证明；单独的 `PASS` 摘要或存在 URDF 文件不等于已发布。
|
||||||
manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和独立 holdout 证据。任一验收失败都不会替换原发布指针。
|
manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和独立 holdout 证据。任一验收失败都不会替换原发布指针。
|
||||||
`measurement_contract.json` 提前列出 Tag/link、零位观测关系、CAD 保留/迁移与原始 mimic 限位冲突。
|
`measurement_contract.json` 提前列出 Tag/link、零位观测关系、CAD 保留/迁移与原始 mimic 限位冲突。
|
||||||
主 JSON 以原始 URDF 关节名为键，每个关节仅包含 `sdk_channel` 和 `angle_rad`；
|
主 JSON 以原始 URDF 关节名为键，单值关节包含 `sdk_channel` 和 `angle_rad`；
|
||||||
O12 等 rad 输入还包含一一对应的 `input_values`（在有效节点间线性插值，不外推）。
|
O12 等 rad 输入还包含一一对应的 `input_values`（在有效节点间线性插值，不外推）。
|
||||||
字节型号固定 256 项：SDK 指令 `v` 对应 `angle_rad[v]`，只能在训练指令实际覆盖 0–255 时导出。
|
字节型号每条曲线固定 256 项：SDK 指令 `v` 对应所选曲线的第 `v` 项，只能在训练指令实际覆盖 0–255 时导出。
|
||||||
输出均是修正 URDF 的 q_output，不再加零偏。顶层保留 `format/schema_version/profile_id/model/side/serial_number/input_unit`。
|
输出均是修正 URDF 的 q_output，不再加零偏。顶层记录身份、单位、完整 `baseline_command` 和共同零位约定。
|
||||||
被动关节的查表由导出 URDF 的标准 mimic 递归生成，绑定同一驱动通道，必须与 URDF 完全一致。
|
独立观测的主动和被动关节直接导出各自视觉曲线，同一驱动通道可绑定多个关节。
|
||||||
单张查表取训练正反向指令曲线的平均值，再用实际导出表重放第四轮视觉数据；回差等因素导致超差时禁止发布，
|
显式复制的关节使用来源关节的同一组节点与角度，不重新拟合、归零或由 mimic 推导；接收通道保持不变。
|
||||||
不通过隐藏方向分支、外推、放宽精度或 JSON 非线性被动联动制造通过结果。
|
默认单值曲线在指定的稳态训练数据中约束 baseline=0，正反向诊断分支共用冻结参考、保留回差。
|
||||||
|
独立稳态验证分别检验两个方向；超差时禁止发布，不隐藏切换分支。
|
||||||
|
|
||||||
|
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 安装或基座。
|
||||||
|
正式图像采集还要求核验第四轮及独立稳态验证的原始 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`、输入域、方向分支、
|
`calibration_report.json` 保存完整的 `command_to_rad` / `feedback_to_rad`、输入域、方向分支、
|
||||||
零位、来源、迁移和训练/holdout 指标。报告中的 `applicability` 记录保持姿态；
|
实测关节的零位原始参考帧、完整指令/反馈/到达方向、来源和训练/holdout 指标。
|
||||||
|
复制关节记录 `independently_measured: false`、`transferred_from_joint` 与来源参考链接，
|
||||||
|
不生成自己的伪零位样本或独立精度指标。绝对零位来源另由 `zero_method`、
|
||||||
|
`cad_zero_assumption` 或 `zero_transferred_from_joint` 表达。报告中的 `applicability` 记录保持姿态；
|
||||||
未采集多姿态证据不能宣称任意多轴工况已验证。主 JSON 用于指令查表，不能误作反馈转换。
|
未采集多姿态证据不能宣称任意多轴工况已验证。主 JSON 用于指令查表，不能误作反馈转换。
|
||||||
旧 unified v1 会话继续只读兼容，不自动覆盖或转换历史产物；新输出单表必须重新通过最终文件验收。
|
旧 unified v1 会话继续只读兼容，不自动覆盖或转换历史产物；新输出单表必须重新通过最终文件验收。
|
||||||
|
|
||||||
@@ -212,9 +786,15 @@ ros2 run linkerhand_calibration calibrated_joint_state_bridge --ros-args \
|
|||||||
|
|
||||||
需要反馈角度显示时显式选择 `input_kind:=feedback`，桥读取配套报告中的反馈映射。
|
需要反馈角度显示时显式选择 `input_kind:=feedback`，桥读取配套报告中的反馈映射。
|
||||||
桥检查三文件哈希、拒绝越域输入，
|
桥检查三文件哈希、拒绝越域输入，
|
||||||
被动角由实际导出 URDF 计算，不加第二次零偏。仿真输出不能与实机反馈话题同名。
|
v3 全部关节角由 JSON 显式计算，不加第二次零偏。完整 FK 使用
|
||||||
|
`independent_mimic_angles=True`，以 JSON 给出的末节角度覆盖线性近似。
|
||||||
|
标准 `robot_state_publisher` 和普通 URDF 网页查看器会执行 mimic，适合检查近似联动；
|
||||||
|
仅把完整 JointState 发给标准 RSP，不能让它停止覆盖被动角。
|
||||||
|
消费实测非线性曲线的仿真程序必须明确采用 JSON 角度，关闭重复的线性被动更新。
|
||||||
|
仿真输出不能与实机反馈话题同名。
|
||||||
MuJoCo 是用户后续使用的外部验收项目，本轮不改动它、不增加 MuJoCo 依赖，自动标定不要求打开 GUI。
|
MuJoCo 是用户后续使用的外部验收项目，本轮不改动它、不增加 MuJoCo 依赖，自动标定不要求打开 GUI。
|
||||||
外部对比须确认加载的模型来自这份 URDF，指令转换使用配套 JSON；不能用实机反馈直接驱动仿真来证明指令映射。
|
外部对比须确认加载的模型来自这份 URDF，指令转换使用配套 JSON；不能用实机反馈直接驱动仿真来证明指令映射。
|
||||||
|
现有 MuJoCo 脚本尚不能视为支持 v3：后续须消费完整关节映射并停止冲突的线性被动更新。
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
# 同一算法离线回放新策略的完整采集；不启动硬件，默认不发布
|
# 同一算法离线回放新策略的完整采集；不启动硬件，默认不发布
|
||||||
@@ -228,9 +808,138 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \
|
|||||||
|
|
||||||
离线回放也检查原始会话策略、SDK 输入域、配置/相机证据、每方向完整数据及最终文件。
|
离线回放也检查原始会话策略、SDK 输入域、配置/相机证据、每方向完整数据及最终文件。
|
||||||
旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出，且不会绕过验证。
|
旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出，且不会绕过验证。
|
||||||
算法和坐标规则见 [CALIBRATION_FLOW.md](CALIBRATION_FLOW.md)，批准的原始方案保持在
|
当前坐标规则、生产职责与验收范围见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
|
||||||
[CALIBRATION_REFACTOR_PLAN.md](CALIBRATION_REFACTOR_PLAN.md)。
|
|
||||||
|
|
||||||
当前代码审查、已修复问题与未完成的精度验收见
|
早期需求审查见 [2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)；
|
||||||
[2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)。相机图像目前采用主机取帧时间；
|
当前修改依据、实机结果与未完成的精度验收见 [流程评估](CALIBRATION_PIPELINE_REVIEW.md)。
|
||||||
USB/驱动积压情况下，图像与反馈的时间戳接近不能证明曝光时刻同步，不能据此宣称实机精度通过。
|
相机和反馈目前均采用下述测量时间；时间配对正确仍须经过独立空间验收。
|
||||||
|
|
||||||
|
|
||||||
|
### 实时调度边界
|
||||||
|
|
||||||
|
公共 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 条件不一致时拒绝发布。
|
||||||
|
这不表示联合求解或外参自校准已经通过验证。
|
||||||
|
|
||||||
|
## 低精度离线修正草稿（O30）
|
||||||
|
|
||||||
|
最新程序修复、两批实采数据验证及未通过的精度项目见
|
||||||
|
[O30_AUTOMATIC_URDF_FIX.md](O30_AUTOMATIC_URDF_FIX.md)。
|
||||||
|
|
||||||
|
当前目标是 **15 个非末端关节零位＋20 个关节运动范围**；拇指 IP 与四指 DIP 的五个
|
||||||
|
末端零位保留 CAD，其运动范围仍由数据拟合。完整全手数据可提供局部拇指缺少的掌坐标
|
||||||
|
约束；只采部分任务时，不能将局部输出当作上述全手目标已完成。
|
||||||
|
|
||||||
|
O30 原始采集正常结束后，启动器完成归位封存、关闭 SDK/相机及控制上下文后退出。
|
||||||
|
`capture_result.json` 只报告采集结果，完整返回 0、缺项返回 3，不自动拟合。
|
||||||
|
每个新会话保存可供离线使用的 `capture_product.yaml`、Profile、源 URDF、内外参和
|
||||||
|
标签配置快照，并随原始证据封存。离线失败不改变采集状态，也不会再次运动。
|
||||||
|
|
||||||
|
也可手动从已完成任务生成完整结构的修正 URDF，再优化精度：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config /绝对路径/会话/capture_product.yaml \
|
||||||
|
--offline-raw /绝对路径/会话/raw_samples.jsonl \
|
||||||
|
--offline-draft --offline-output /绝对路径/新的草稿目录
|
||||||
|
```
|
||||||
|
|
||||||
|
草稿复用现有图像拟合器、零位约束和 URDF 写入器。训练数据估计可观测的零位及
|
||||||
|
双向指令曲线，验证数据仅报告误差；超过原精度门限仍可生成文件。
|
||||||
|
未独立观测的反向换向端点采用有来源的连续性约束，不保留可任意改变行程的自由参数；
|
||||||
|
约束写入 `motion_range_evidence.command_support`。缺少端点依据时拒绝生成行程。
|
||||||
|
`draft_urdf_linear_holdout.json` 单独报告纯 URDF 上下限线性驱动的误差；曲线精度
|
||||||
|
不能替代线性驱动验收，未通过精度验收的草稿不更新正式发布指针。
|
||||||
|
每个任务至少有一轮完整训练即可参与草稿求解，另一训练轮已通过采集质量检查的
|
||||||
|
完整方向也保留；不拼接失败尝试，不用第三轮补训练缺项。初始化使用该任务已完整的
|
||||||
|
训练轮，逐任务覆盖与选择理由写入 `draft_result.json` 的 `training_selection`。
|
||||||
|
父关节观测约束仍保留；草稿可覆盖全手与原始采集完整是两个独立结果。
|
||||||
|
缺测关节保留原 CAD，单根手指无法观测的根零位及已声明的 CAD 末端零位单独列出。
|
||||||
|
不从数值不可观测的自由度导出任意零位。JSON 读回、字段授权、运动范围、最终 FK、
|
||||||
|
标准 URDF 加载和来源哈希检查仍必须通过。
|
||||||
|
|
||||||
|
离线模型纠错可使用单独的 Profile/Product 文件，只允许修改标签连杆绑定和
|
||||||
|
`sdk_to_joint_direction`；原始会话仍按保存的 `capture_profile.yaml` 校验。
|
||||||
|
基准 SDK 指令、通道、采样路线和速度不允许借此更改。新旧方向及配置哈希写入
|
||||||
|
`draft_result.json`，不重写原始记录，也不影响在线控制。
|
||||||
|
求解前以训练轮的双向运动和两种平面姿态候选检查 CAD 平行根轴的方向一致性；
|
||||||
|
所有候选均反向时拒绝拟合，记录 `draft_sdk_to_urdf_direction_conflict`。
|
||||||
|
不能靠零位补偿错误的 SDK/URDF 方向。文件能加载及参数数量齐全，也不等于物理零位正确。
|
||||||
|
|
||||||
|
现场明确确认四指在采集基准指令下竖直时，离线命令可增加
|
||||||
|
`--draft-upright-splay-zero`。此选项先检查源 CAD 的四指零位为掌坐标竖直，
|
||||||
|
再从求解变量中移除四个侧摆零位，重新拟合其他零位与全部指令曲线；不是导出后改姿态。
|
||||||
|
JSON 将这四项标为 `operator_defined`，分别统计 11 个拟合零位、4 个用户定义零位及
|
||||||
|
5 个 CAD 末端零位，不能把四个定义值当作独立测量。原始基准指令、采集配置和记录不变；
|
||||||
|
该选项不能进入在线采集或正式发布。
|
||||||
|
|
||||||
|
同批数据已有局部草稿时，可增加 `--draft-initialization-from /绝对路径/已有草稿目录`。
|
||||||
|
入口核对原始数据及配置身份，只读取训练选出的起始姿态和曲线；所有待求参数仍在
|
||||||
|
全手训练图像中联合调整。不同批次、使用验证选择的结果不能作为该初值。
|
||||||
|
其文件哈希和用途写入新报告，不覆盖已有草稿。
|
||||||
|
|
||||||
|
全手草稿按相同标签、相同祖先关节指令/方向及相机参数合并重复预测，使用样本数权重
|
||||||
|
及组内散差保持原始最小二乘目标、梯度与信息矩阵；误差报告仍逐张原始图像计算。
|
||||||
|
草稿数值停止 `ftol=1e-5`，正式求解保持 `1e-9`；这不改变任何精度、可观测性或
|
||||||
|
发布门限，草稿无论数值收敛与否都不能自动更新 `latest_passed`。
|
||||||
|
草稿最多保留四个收敛初值的候选并按训练误差选择，报告已计算初值数量及是否穷尽；
|
||||||
|
不以这个有限搜索声称已排除全部几何解。正式候选搜索与发布验收保持原规则。
|
||||||
|
|
||||||
|
输出 `o30_right_draft.urdf`、`draft_calibration.json`、`draft_result.json` 及训练/验证报告。
|
||||||
|
草稿 JSON 使用独立格式，不作为正式控制映射或精度证书自动加载；URDF 可标准加载。
|
||||||
|
该入口允许读取中断会话的完整方向，不启动 SDK、不补采、不更新 `latest_passed`。
|
||||||
|
`--offline-draft` 不能与 `--publish-offline` 同用，正式精度验收入口保持不变。
|
||||||
|
|
||||||
|
若旧数据的标签连杆配置有误，提供 `--capture-profile /绝对路径/原Profile.yaml`；
|
||||||
|
新会话已保存 `capture_profile.yaml` 时会自动使用该快照。
|
||||||
|
先核对原 Profile 与会话哈希，再仅允许当前 Profile 修正标签安装连杆。
|
||||||
|
标签身份、尺寸、运动策略、相机与 CAD 来源不允许借此改变，原始记录保持只读。
|
||||||
|
|
||||||
|
采集中途重新安装移动标签时，不能直接沿用该标签原来的安装参数。O30 草稿流程可先
|
||||||
|
离线建立新断点，再按普通断点恢复：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
|
--config src/linkerhand_calibration/config/o30_right_product.yaml \
|
||||||
|
--prepare-remount-from /绝对路径/原会话 --remounted-tag-id 10
|
||||||
|
```
|
||||||
|
|
||||||
|
此步骤不启动相机或 SDK。只允许保留调整标签之前连续通过的任务前缀；原会话不修改，
|
||||||
|
新断点按 CAD 拓扑排除受影响手指的全部旧标签角点，从首个受影响任务重新采集，
|
||||||
|
其他方向的补采次数保持原值。未开始新扫描的准备断点可扩大排除范围；已开始的尝试
|
||||||
|
不能借此重新获得预算。
|
||||||
|
原会话保存一次性后继指针，后续重启使用普通恢复，不能再次从旧安装重置尝试次数。
|
||||||
|
原始角点续采时，活动标签在同一 SDK 基准指令下的像素/反馈差异及不可见情况
|
||||||
|
列为“待离线复核”，不将回位姿态差异直接判成标签重装，不据此暂停采集。
|
||||||
|
固定基准、配置身份及运动保护仍须通过。前后安装参考、差异和原补采预算均保留；
|
||||||
|
未核实差异只允许进入未认证修正草稿，正式发布仍拒绝未复核的安装关系。
|
||||||
|
标签必须仍在原配置连杆上；若移到了其他指节，先修正连杆绑定。
|
||||||
|
|||||||
@@ -0,0 +1,92 @@
|
|||||||
|
# 固定 Tag 的分阶段标定实施记录
|
||||||
|
|
||||||
|
默认 `fixed` 保留。新策略通过 `--training-policy staged_2_to_3` 显式启用；当前满足其
|
||||||
|
“实测指令发布、完整交错网格”能力约束的内置配置为 O30。通用实现不按手型号或相机名称分支。
|
||||||
|
本次没有启动硬件、修改 Tag/相机配置或更新已发布产物。
|
||||||
|
|
||||||
|
## 执行和证据
|
||||||
|
|
||||||
|
- `capture_plan_v2` 描述各阶段、原任务顺序及实际训练轮。引擎按轮次排列现有扫描单元，
|
||||||
|
继续使用原基准、避让、进入/退出路径。首轮数据直接用于正式训练。
|
||||||
|
- 两轮全手数据齐全后检查曲线重复性、空间几何、零位置信度和 Tag 安装。训练问题使用
|
||||||
|
参数、原因、范围和任务依赖描述；局部零位缺陷补齐同轮依赖，共享根部几何缺陷扩大到全手。
|
||||||
|
- 第三轮只允许一个集中补采批次，轮次固定为 2。仍不合格即失败。独立验证始终是轮次 3。
|
||||||
|
- 每次回访保存新零位复核记录，原零位、安装模型与证据 ID 不被替换。新/旧分支编号均有
|
||||||
|
独立授权，历史扫描不会被后来一次复核覆盖。训练不足、持续姿态歧义和参考变化不靠重复验证解决。
|
||||||
|
- 上游准备阶段按物理 Tag 关系保存旁观角点、候选及原指令/反馈身份，沿用有界运动分箱。
|
||||||
|
末节保持基准指令和反馈稳定才可采用。候选使用固定时间划分、原像素门限、角度分层的独立
|
||||||
|
误差比较及 Holm 控制；不能用最低误差强选不可区分的候选。
|
||||||
|
- 相对位姿估计传播同图父 Tag 的像素不确定度，并保守累加父模型几何不确定度；后续末节
|
||||||
|
准备仍通过原运动、图像、可观测性和候选区分门限。回读核对模型身份、原始角点、固定取样、
|
||||||
|
保持通道和测量结果。短暂内部恢复仅沿既有带日志的参考延续规则复用来源。
|
||||||
|
旁观证据与已有共享 Tag 桥接并存时，策略标识同时保留两者的来源和约束方式。
|
||||||
|
|
||||||
|
## 计算和冻结
|
||||||
|
|
||||||
|
训练工作进程在会话内复用。传输、计算和返回均受同一超时/取消约束。任务索引缓存已选
|
||||||
|
训练快照，增补只使受影响任务的局部模型失效，随后重算全局空间解。
|
||||||
|
|
||||||
|
`staged_training_decision` 记录输入与计划哈希、结构化问题、实际独立轮数、置信区间和
|
||||||
|
增补/冻结/失败决定。`frozen_training.json` 使用显式 JSON 数值类型，包含双向映射、空间解、
|
||||||
|
实际应用零偏、统计、掌部姿态和 Tag 安装；绑定完整配置、源 URDF、训练记录、零位和求解版本。
|
||||||
|
修改求解语义时必须升级求解版本，不能用缓存替代证据。
|
||||||
|
|
||||||
|
在线最终验收加载已冻结结果，不再次运行曲线和空间训练优化。产物顺序仍为：独立角度验证、
|
||||||
|
写 JSON、从落盘 JSON 重建 URDF、文件/限位/方向/通道/单位/像素检查、成对发布。
|
||||||
|
离线审计复算训练决定并比对冻结结果。模型或证据改变时拒绝发布。五个末节绝对零位仍为
|
||||||
|
明确的 CAD 假设，精度声明限于经过独立验证的运动范围和姿态。
|
||||||
|
|
||||||
|
## 断点和恢复
|
||||||
|
|
||||||
|
新旧计划不得混用。旧策略保持原恢复语义。新策略仅复用连续完整任务访问；部分方向不算
|
||||||
|
访问完成。固定参考通过后，先导入原模型和原零位，再逐次复核当前零位，最后才跳过原访问。
|
||||||
|
集中补采与冻结决定按日志顺序恢复；已冻结而缺失原训练前缀或冻结文件时拒绝回退训练。
|
||||||
|
恢复父模型时一并恢复其已绑定的有界旁观原始帧，缺失角点不补造；当前零位仍须重新复核。
|
||||||
|
扫描时间必须晚于所绑定访问的零位复核；导入历史扫描保留当时的复核证据，不能伪造新采集时间。
|
||||||
|
准备缺样额度与同速重扫额度跨断点保留；已经耗尽的恢复不能通过重新加载断点获得新额度。
|
||||||
|
|
||||||
|
## 验证与局限
|
||||||
|
|
||||||
|
### 共享 Tag 改变进入姿态时的联合准备（2026-09-20）
|
||||||
|
|
||||||
|
拇指 CMC roll 与 MCP 使用同一个 Tag，但两任务间 yaw 基准不同，不能直接把前一个零位
|
||||||
|
当作 MCP 零位。本次保留既有 yaw 进入动作，把其原始图像与 MCP 自身准备图像联合求解：
|
||||||
|
前端绑定已经实测的 roll 零位，后端用保持指令及稳定反馈确认与 MCP 的共同姿态。
|
||||||
|
SDK 指令只标识动作和保持状态，不充当相机测得的角度。没有新增运动或修改 Tag、速度、路径。
|
||||||
|
|
||||||
|
采样分箱及训练/留出划分事先固定，完整枚举两段动作的候选组合；训练决定模型，留出只可
|
||||||
|
否决。维持原像素、几何不确定度和候选区分门限。目标几何的不确定度计算恢复全部自由度，
|
||||||
|
避免把固定来源零位误当作额外精度。辅助进入数据缺失时，在拟合前记录原因并使用原准备入口；
|
||||||
|
进入数据违反参考/安装/保持通道约束，或联合拟合及留出失败时，不能退回另一种求解碰运气。
|
||||||
|
|
||||||
|
`preparation_transition_v1` 绑定来源零位、来源模型、实际运动依赖、完整原始帧哈希和固定
|
||||||
|
选样身份。日志回读重新核对源 URDF 依赖、来源零位图像，并复算联合模型、核对冻结模型哈希。
|
||||||
|
取消和跨会话会清除未完成的进入证据。该机制沿用现有准备工作进程及状态机，普通冻结图像模型
|
||||||
|
和最终 JSON→URDF 发布契约不变。
|
||||||
|
|
||||||
|
真实会话 `20260920_125306` 的 MCP 原始图像经运行时采集入口回放后通过：原方法仍因候选
|
||||||
|
不可区分而拒绝；联合模型的最大训练/留出帧 RMS 分别约 1.117/1.083 px（原门限 1.5 px）。
|
||||||
|
四个初始化组合中两个收敛到等价几何，另两个违反原像素门限。原中指 DIP 不充分证据回归仍拒绝。
|
||||||
|
这证明该次 MCP 停点的软件问题得到改善，不代表整手物理精度、总耗时或一次连续通过已经验收。
|
||||||
|
本次优化未启动硬件，也未生成或发布新的整手 JSON/URDF；默认策略继续为 `fixed`。
|
||||||
|
软件验证与原始回放报告保存在
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_preparation_transition/`。
|
||||||
|
|
||||||
|
测试按快速单元、回放、产物集成、历史兼容分层；命令见 `TESTING.md`。整手输入和产物由
|
||||||
|
共享夹具提供，篡改、缺字段、取消等直接进入对应拒绝边界。修正了一个拿不完整采集数据
|
||||||
|
调用最终化、却期待后置运动证据错误的旧用例，改为直接检验其负责的运动验收边界。
|
||||||
|
|
||||||
|
已覆盖：两轮冻结、一次局部/全手补采、补采失败、真实依赖的混合轮数、冻结后不再训练、
|
||||||
|
留出隔离、回访复核、有界恢复、完整访问恢复、模型篡改拒绝、取消发布和 O30 JSON→URDF 一致性。
|
||||||
|
真实中指 DIP 旧数据缺少旁观证据时仍拒绝。新增合成独立证据能区分候选时才通过。
|
||||||
|
各次测试的源码指纹、耗时、失败及修复后复查关系保存于工作区
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_staged_training/software_validation.json`。
|
||||||
|
|
||||||
|
O30 产物集成使用原正式采集证据入口及独立 FK 合成图像，复用兼容的几何运动授权。
|
||||||
|
它覆盖文件链路与证据契约；在线图像准备算法另由原真实 DIP 回放和新增旁观图像用例检查。
|
||||||
|
该合成产物用例不是整手实机图像模型选择、连续运动或物理精度证明。
|
||||||
|
|
||||||
|
实机工作仍需在全新会话完成：所有尝试均计入，比较总耗时、完整通过、人工干预和失败原因。
|
||||||
|
记录开始到终态、准备、稳定等待、采样、恢复、训练、文件验收及开始前断点加载开销；比较时
|
||||||
|
不能漏掉加载开销或只比较扫描时间。达到正确性门限、人工干预未增加且总耗时改善至少 10%
|
||||||
|
才考虑切换默认。大日志存储/全量解码重构未与本次采集规则一起进行。
|
||||||
@@ -0,0 +1,782 @@
|
|||||||
|
# 标定测试的执行范围
|
||||||
|
|
||||||
|
2026-09-24 O30 剩余误差诊断：只读两批封存数据及已冻结参数，旧/新独立验证图像
|
||||||
|
分别 2681/516 帧；按机位/标签/任务/指令拆分残差，受限试探仅用训练图像选参数、
|
||||||
|
验证轮报告结果。顶部相机在新拇指 roll 三轮只见固定标签；跨批食指端点偏差及
|
||||||
|
拇指 roll 高角度重复性残差已定位。JSON/URDF 逐字节差、来源哈希和所有试探结果
|
||||||
|
记录于 [O30_REMAINING_ISSUES.md](O30_REMAINING_ISSUES.md)。未修改生产拟合、采集、
|
||||||
|
门限或已生成候选；诊断结果不计作正式精度测试通过，也未驱动硬件。
|
||||||
|
|
||||||
|
2026-09-23 自动 URDF 修复：采集/局部/重装组 43 passed（17.06 秒），离线草稿及
|
||||||
|
换向端点组 43 passed（34.18 秒），更新审计 3 passed（0.33 秒）。完整真实
|
||||||
|
coordinator 的虚拟时钟 108 方向集成 1 passed（97.21 秒），用于核对完整调度和
|
||||||
|
训练/验证隔离，未启动硬件。最后补齐快照 mesh 与封存路径检查后，仅复查受影响
|
||||||
|
4 项，4 passed（2.38 秒）；此组与前面重叠，不重复累计。
|
||||||
|
早期失败包括旧夹具仍使用已纠正的标签/方向配置、近端插值掩盖未观测换向端点；
|
||||||
|
分别修正历史场景声明、改用有来源的端点连续性约束后通过，没有降低验收门限。
|
||||||
|
旧外参全手与新外参拇指均按自身配置回放，训练重新收敛，JSON 重建 URDF 字节
|
||||||
|
一致，FK 最大差 6.66e-16，ROS/MuJoCo 加载通过；独立精度仍未通过，未发布。
|
||||||
|
证据、物理精度限制及候选路径见 [O30_AUTOMATIC_URDF_FIX.md](O30_AUTOMATIC_URDF_FIX.md)。
|
||||||
|
|
||||||
|
2026-09-23 离线逻辑复核：复用连续候选关联替代逐帧 PnP 排序，扫描起点按扫描身份纳入；
|
||||||
|
补全欠约束 SVD 零空间，范围证据记录实际可见训练样本和轮次，单轮仅允许未认证草稿。
|
||||||
|
新增 7 项集中回归通过；其余受影响项分组通过。首次 24 passed/4 failed/2 errors（22.21 秒）
|
||||||
|
中的失败分别为未 source ROS 和新增夹具字段名错误；修正后对应及 CLI 10 passed（2.13 秒）。
|
||||||
|
范围边界组 5 passed（1.20 秒），中节绑定/历史数据隔离组 2 passed（1.12 秒）。
|
||||||
|
用户确认 ID12–ID15 随中节运动后，新离线配置将四标签绑定至 `*_middle`；封存数据回放生成
|
||||||
|
`offline_confirmed_middle_tags_draft`，训练坐标 RMS 1.4867 px，最终 FK 差 5.55e-16。
|
||||||
|
四指角度子项通过，但拇指 roll/MCP 及全局图像/不确定度未通过；未发布，未控制实机。
|
||||||
|
详细问题、证据和复现命令见 [O30_OFFLINE_LOGIC_REVIEW.md](O30_OFFLINE_LOGIC_REVIEW.md)。
|
||||||
|
|
||||||
|
2026-09-23 四指侧摆竖直基准：新增显式离线选项 `--draft-upright-splay-zero`，
|
||||||
|
四项作为用户定义基准移出自由零位参数，全部行程与其余零位重新拟合。
|
||||||
|
新增/CLI 用例 6 passed（13.04 秒），相关范围、缺项、导出及生命周期用例
|
||||||
|
18 passed（16.88 秒）。`offline_upright_zero_draft` 最终 URDF 的四指正面侧偏均为
|
||||||
|
0°；JSON/FK 最大差 8.89e-16，标准加载与来源哈希检查通过。
|
||||||
|
报告区分 11 个拟合零位、4 个用户定义零位、5 个 CAD 末端零位和 20 个拟合范围。
|
||||||
|
本次独立验证已算出角度误差，但整手精度仍未通过，未更新 latest_passed。
|
||||||
|
原始数据保持不变，未进行硬件运动；上一版侧摆偏移 −6.6° 至 −9.4° 的草稿已附拒绝说明。
|
||||||
|
|
||||||
|
2026-09-23 物理零位复核：旧 `offline_full_hand_draft` 因四指 SDK/URDF 方向反转而
|
||||||
|
错误估计约 −20° MCP 零位，已附拒绝说明，不能将其文件检查通过理解为零位正确。
|
||||||
|
独立离线配置纠正方向后，`offline_corrected_zero_draft_v2` 的四指 MCP 为
|
||||||
|
+0.385°、−0.306°、−0.690°、−1.475°，没有强制置零；JSON/URDF FK 最大差
|
||||||
|
7.78e-16、标准加载及原始数据哈希检查通过。独立验证求解器仍报 IndexError，
|
||||||
|
保留 traceback，精度/发布均为 False；没有硬件动作。
|
||||||
|
本次 `test_raw_draft.py` 27 passed（30.15 秒），新增实采角点方向回归、离线模型
|
||||||
|
修订边界及验证求解异常不冒充精度通过的检查。原始数据仍为 107/108 单元。
|
||||||
|
|
||||||
|
全手草稿候选搜索收敛为最多四个收敛结果，明确记录非穷尽；相关数值不可观测拒绝、
|
||||||
|
候选预算和训练初值契约 3 passed、21 deselected（0.90 秒）。
|
||||||
|
实机封存数据 `20260923_142201` 已生成全手草稿于
|
||||||
|
`calibration_output/splay_recapture_20260923_132346/offline_full_hand_draft/`：
|
||||||
|
15 个非末端零位、20 个范围，五个末端零位保留 CAD；原始采集仍为 107/108。
|
||||||
|
复用已计算且收敛的同目标训练候选，重算原始像素误差确认一致，再运行正常草稿验收；
|
||||||
|
缓存文件、脚本和来源哈希见 `numerical_fit_reuse.json`。JSON/最终 URDF FK 最大差
|
||||||
|
6.66e-16，标准加载通过。独立验证角度求解失败，四个零位接近 ±20° 模型边界；
|
||||||
|
精度和发布均未通过，`latest_passed` 未更新。没有硬件动作。
|
||||||
|
|
||||||
|
2026-09-23 同批数据全手离线求解支持：最终 `test_raw_draft.py` 23 passed（14.47 秒）。
|
||||||
|
新增重复观测合并的原始目标、梯度和信息矩阵等价检查，原始图像误差仍逐帧报告；
|
||||||
|
训练初值拒绝不同数据批次或使用验证选择的结果，相关 CLI 不能进入在线运动。
|
||||||
|
首次等价测试的近零梯度逐元素断言受浮点消减影响（最大绝对差 4.62e-7、全梯度相对差
|
||||||
|
3.32e-13），改为完整梯度范数误差小于 1e-11 后通过，未改模型或采集精度门限。
|
||||||
|
试验过的自适应参数缩放未在合成全手收敛检查中通过，已撤回；最终沿用原参数缩放。
|
||||||
|
仅未认证草稿的数值停止容差为 1e-5，正式求解默认值及发布检查不变。
|
||||||
|
|
||||||
|
2026-09-23 离线草稿支持单轮完整训练：`test_raw_draft.py` 20 passed（8.42 秒）。
|
||||||
|
覆盖任一训练轮缺四指联动上行时仍保留全手求解范围、初始化选择另一完整训练轮、
|
||||||
|
两轮互补缺项或完整验证轮不能冒充完整训练轮，以及原有不可观测零位拒绝、JSON
|
||||||
|
重建和只读离线生命周期契约。未改变正式采集完整性或发布门限；未恢复历史测试。
|
||||||
|
实机数据的实际离线求解另行记录，不把上述软件测试当作精度验收。
|
||||||
|
|
||||||
|
2026-09-23 原始续采的活动标签基准检查：12 passed、30 deselected（1.37 秒）。
|
||||||
|
覆盖活动标签像素/反馈差异记录后继续导入、缺少基准记录的离线复核标记、固定基准
|
||||||
|
移动仍拒绝、非有限反馈拒绝、正式发布拒绝未复核安装、草稿仍检查来源完整性，
|
||||||
|
以及现有标签重装/次数持久化契约。真实 coordinator 检查确认没有因活动标签
|
||||||
|
基准差异暂停，且将未复核状态写入日志。未启动 SDK 或相机。
|
||||||
|
两份实机停止记录 `132507` / `132938` 的固定参考与 96 个方向断点只读复核位于
|
||||||
|
`calibration_output/splay_recapture_20260923_132346/software_review_resume_reference.json`。
|
||||||
|
不重判旧采样质量、不修改角点；下一任务为四指侧摆第 1 轮正向联动。
|
||||||
|
这只证明恢复检查和任务位置正确，不代表后续实机运动或 URDF 精度验收通过。
|
||||||
|
|
||||||
|
2026-09-22 正式 2＋1 局部失败调度收敛：新增 test_taskwise_recovery.py，覆盖
|
||||||
|
完整依赖任务组延后、训练/验证顺序不变、已完成前缀不变、一次回访预算、真实
|
||||||
|
coordinator 的带视角错误处理、多视角参考变化不被掩盖、失败后归位且不进入拟合、
|
||||||
|
无可复用零位时的恢复日志及预算持久化、未解决状态禁止发布。
|
||||||
|
|
||||||
|
首组 30 项通过、1 项失败（13.93 秒）；失败为既有观察器迁移测试仍断言
|
||||||
|
rigid_pair_v1，当前配置实际为 rigid_pair_candidates_v2，修正过时断言。
|
||||||
|
随后调度/恢复/旧 staged 契约/执行组 44 项通过、1 项未选（6.20 秒）；passed
|
||||||
|
断点/分阶段训练/真实图像共享姿态交接组 59 项通过（22.11 秒）。集中任务准入
|
||||||
|
判断后，受影响恢复组及完整合成 JSON→URDF 验收 18 项通过（14.10 秒）。各组
|
||||||
|
有重叠，不累计为全量回归。最后一组包含最终文件重建一致性、独立图像验收、
|
||||||
|
CAD 冻结零位与授权字段保护。
|
||||||
|
|
||||||
|
正式 CLI --validate-only 通过，确认 capture_plan_v5、17 任务、训练 0/1、验证 3，
|
||||||
|
受保护输入一致。git diff --check 通过。本轮无实机运动，未改求解器/精度门限，
|
||||||
|
不代表全手稳定性验收；旧 v4/v15 断点与新调度隔离。
|
||||||
|
|
||||||
|
2026-09-22 用户指定无名指侧摆基准由 85 改为 127：同步 Profile、准备向量、
|
||||||
|
ROS 配置、产品指纹及当前夹具。Profile/局部任务组首次 14 项通过、1 项旧基准
|
||||||
|
断言失败（21.25 秒）；修正该断言后单项通过（0.55 秒）。正式 CLI 配置校验及
|
||||||
|
diff 检查通过。全手新会话 194750 在启动前被已有控制/反馈/状态发布者保护拒绝，
|
||||||
|
未启动本次 SDK、未发送运动，不算实机标定通过；旧基准断点不复用。
|
||||||
|
|
||||||
|
2026-09-22 固定 190634 的绝对零位可观测性检查：新增
|
||||||
|
`test_zero_evidence_coverage.py`，覆盖原零位输入拒绝不变、训练/验证缺失列表
|
||||||
|
分离、明确缺失掌定位轴线，以及真实 O30 URDF 下的 MCP 零位/未知掌坐标精确
|
||||||
|
等价变换；已知掌坐标时同一零位变化可检测。三项通过，0.45 秒；既有
|
||||||
|
`test_spatial_solve_order.py` 真实空间拟合回放一项通过，0.86 秒。
|
||||||
|
仅增强输入覆盖报错，不改变零位/精度/运动策略。本次无实机运动，未发布产物；
|
||||||
|
不能把完整小指屈伸数据误当作全手绝对掌坐标参考，详情见稳定性报告最新节。
|
||||||
|
|
||||||
|
2026-09-22 DIP 父姿态/安装/角度不确定度：复用 `JointPairImages`，新增独立图像
|
||||||
|
局部角度协方差，保留共享相关项。原组 8 项通过，0.89 秒；新增“不确定度不可用
|
||||||
|
不得删除竞争模型”后受影响 3 项通过，0.44 秒。临时密度参数边界 3 项通过，
|
||||||
|
0.43 秒；密度对照未显示稳定收益，参数及其专属测试随后撤除。补齐重复图像和
|
||||||
|
角色身份检查，最终该模块 9 项通过，0.95 秒。各组有重复，不累计为全量回归。
|
||||||
|
|
||||||
|
真实 190634 前两轮默认预算 16 初值全部收敛，不确定度结果仍为非发布诊断；
|
||||||
|
72 帧/阶段对照中 2 初值独立图像求解失败，保留负面证据，不放宽优化器/候选
|
||||||
|
门限、不触发额外运动。第三轮在本轮未用于训练或选择，正式零位和产物仍未验收。
|
||||||
|
|
||||||
|
2026-09-22 复用 190634 离线曲线检查：补充原图像模型的纯前向名义像素误差
|
||||||
|
`image_motion_prediction_errors`，禁止验证时重拟合角度，检查训练/映射来源重叠、
|
||||||
|
有限角度、模型不变与错误预测可见。受影响组首次 3 项通过，0.47 秒；核对正式
|
||||||
|
验收语义后撤除对名义总像素误差额外施加 1.5 px 的判决，仅返回误差，再测该项
|
||||||
|
1 项通过，0.44 秒。正式物理验收规则及测量精度门限不变。
|
||||||
|
真实 MCP/PIP 冻结指令曲线及 DIP 两个保留候选的第三轮条件角度检查均通过；
|
||||||
|
DIP 安装候选仍未区分，不等于物理零位或 JSON/URDF 发布通过。细节及一次
|
||||||
|
缺少旁观标签的读取失败记录在稳定性报告，本轮未运动机械手。
|
||||||
|
|
||||||
|
2026-09-22 取消辅助侧摆并回到原交接方法：撤除本轮显式辅助开关，整指实验
|
||||||
|
拒绝任何辅助计划。采集/实际指令三轮/入口组首次 36 项通过、2 项因未 source
|
||||||
|
工作区而找不到 ROS package 失败，17.96 秒；补齐环境后仅重跑失败两项，
|
||||||
|
2 项通过、0.60 秒，没有为测试修改生产路径。
|
||||||
|
双侧交接回归首组六项因历史夹具与现 Profile 的姿态/标签尺寸不符而初始化失败；
|
||||||
|
修复测试夹具后首次写错 dataclass 字段，未进入求解；改用 baseline_u8 后六项
|
||||||
|
通过、1.86 秒，其余受同夹具影响的 23 项通过、4.86 秒。历史 113/16.5 mm
|
||||||
|
仅用于旧证据回放，生产仍为 85/16 mm。包含交接后模型选择、旧零位不变、
|
||||||
|
坏图像双侧拒绝、来源哈希和读回防篡改；不代表当前实机发布已通过。
|
||||||
|
当前 182949 真实数据原 MCP/PIP 及交接回放见稳定性报告：第三轮几何检查通过，
|
||||||
|
但未完成冻结指令映射、物理零位和最终产物验收。本轮未再次驱动机械手。
|
||||||
|
|
||||||
|
2026-09-22 用户指定一次辅助侧摆对照：复用已有 ObserverPreparation，整指实验
|
||||||
|
只允许 DIP 前一个有界动作，不继承全手或扩幅预算。真实 coordinator 虚拟测试
|
||||||
|
`test_chain_capture_runtime.py -k 'complete_chain and True'` 1 项通过、15.94 秒，
|
||||||
|
确认只有一次 52→115→52 辅助段，MCP/PIP/DIP 实际全部指令仍各三次往返，
|
||||||
|
不生成零位或产物。单动作/全手拒绝/多动作拒绝三个预算边界通过、0.83 秒。
|
||||||
|
实机 182949 在 MCP/PIP 十二方向完成及一次辅助段完成后，因早期父参考图像差异
|
||||||
|
暂停，DIP 未开始。离线有无辅助的双标签安装估计均未通过候选验收；未放宽门限，
|
||||||
|
未发布。具体证据见稳定性报告，不能将软件调度通过写成实机全流程通过。
|
||||||
|
|
||||||
|
2026-09-22 小指实际三轮采集：`test_chain_capture.py`、
|
||||||
|
`test_chain_capture_runtime.py`、`test_diagnostic_presets_cli.py` 共 33 项通过，
|
||||||
|
17.23 秒。实际 coordinator 虚拟时钟统计全部发送指令，包含准备/收尾，断言三个
|
||||||
|
关节恰好三次完整往返；保存三份第一轮训练回程参考，禁止验证轮、错误方向/
|
||||||
|
轮次、缺帧、标签缺失或不稳定反馈替换来源。DIP 标签持续缺失仍按原超时暂停，
|
||||||
|
原始证据不生成授权零位或角度、不发布。新增旧正式准备不受影响与同一参考只写
|
||||||
|
一次的两个边界通过，0.57 秒。本轮未驱动硬件，也未声称正式离线产物链已接通。
|
||||||
|
|
||||||
|
2026-09-22 正式入口收敛：O30 默认改为 `taskwise_2_plus_1`，不默认声明辅助动作，
|
||||||
|
产品指纹同步；历史合成夹具显式生成 fixed 数据全集，再由正式夹具选择验收分区。
|
||||||
|
`test_taskwise_two_round.py`、`test_o30_profile.py`、诊断 CLI 与交错运动受影响组
|
||||||
|
41 项通过，18.57 秒；含一次正式合成 20 关节 JSON→URDF 及独立图像验收，
|
||||||
|
新增 CAD 零位不变和仅允许字段变化断言。启动参数/分区/恢复/旧 staged 兼容组
|
||||||
|
41 项通过、6 项未选，18.30 秒；未运行不相关全型号拟合。旧固定策略测试显式
|
||||||
|
选 fixed，不通过改变生产策略保留过时默认。以上是软件测试，不是实机正式通过。
|
||||||
|
新增 2＋1 实际 launch 参数传递用例 1 项通过，0.84 秒；CLI `--validate-only`
|
||||||
|
确认默认 17 任务、训练 0/1、验证 3，受保护指纹一致，未启动硬件。检查脚本首次
|
||||||
|
把 CLI 的 JSON 和尾部说明当成单个 JSON 解析失败；改为解码首个 JSON 后检查通过，
|
||||||
|
未改生产输出。`git diff --check` 通过。
|
||||||
|
只读旧数据的插值对照仍不满足精度，未将 PCHIP 引入正式代码，未新增运动。
|
||||||
|
|
||||||
|
2026-09-22 用户指定无名指侧摆基准改为 85：同步 Profile、全部相关零位/准备向量、
|
||||||
|
ROS 参数、产品指纹及合成夹具；其他通道不变。来源/指纹、侧摆零位路径、采样节点/
|
||||||
|
速度和局部任务契约共 4 项通过、1.97 秒。额外只读校验参数与 Profile 基准一致，
|
||||||
|
所有零位的 ring_yaw=85；范围示教文件仅修改本机 baseline.target[4]，历史反馈
|
||||||
|
不改写。未启动实机运动，不改旧会话证据；新基准须重新采集，不复用旧断点。
|
||||||
|
|
||||||
|
2026-09-22 整指延后求解采集：显式 `o30_pinky_chain` 实验入口使用原任务路径、
|
||||||
|
原 SDK/基准保护及交错停点，逐任务轮次 0、1、3；原始父参考回访不授予零位或
|
||||||
|
角度权限。`test_chain_capture.py` 与旧旁观隔离失败项复查 15 项通过、1.58 秒；
|
||||||
|
完整真实 coordinator 虚拟时钟 18 单元集成测试通过、19.43 秒；缺失 DIP 标签时
|
||||||
|
原参考超时仍暂停，1 项通过、2.21 秒。验证轮不得进入旧分段拟合器，1 项通过、
|
||||||
|
0.72 秒；旧参考权限/虚拟任务契约 2 项通过、0.86 秒。
|
||||||
|
预设、旁观、冻结验收回归首组 55 项通过、1 项失败、2.08 秒：旧用例把启动日志
|
||||||
|
固定为单条，未考虑已有 camera_preflight；改为断言调用前后记录完全不变，未删
|
||||||
|
生产日志。完整虚拟夹具初次因设备心跳未刷新、合成标签投影小于原 30 px 门限
|
||||||
|
失败，修夹具心跳与焦距后通过，没有放宽生产门限。以上各组不累计为全量回归，
|
||||||
|
更不代表实机几何或 JSON/URDF 发布通过。
|
||||||
|
新增显式 CLI 训练策略/禁恢复契约 1 项通过、0.67 秒。实机 `165627` 三任务
|
||||||
|
18 单元连续采集完成，无暂停、无重扫，安全返回；独立冻结预测仍拒绝，具体精度
|
||||||
|
与候选差异见稳定性报告。没有因采集完成重跑不相关的全型号产物测试。
|
||||||
|
|
||||||
|
2026-09-22 联合模型冻结指令验证：新增 `test_frozen_chain_validation.py`，
|
||||||
|
16 项通过、0.61 秒；测试夹具改为通过构造接口提供窄支持域后，仅复查受影响项，
|
||||||
|
1 项通过、0.53 秒。覆盖已知真值的双向映射、验证期禁止重拟合、错误指令与零位
|
||||||
|
不得被吸收、5 px 阶段平移不得被重新配准、训练/候选选择图像隔离、独立轮次、
|
||||||
|
稳态目标、保持通道、机位/内参/标签尺寸、非有限或非整数线端指令和范围外拒绝。
|
||||||
|
复用已有几何合成夹具，无新增实机运动。`FrozenChainCommands` 是非授权的验收
|
||||||
|
组件，未接入运行时；它不证明物理零位来源、完整采样节点覆盖或候选不确定度。
|
||||||
|
未将该组软件测试标记为正式 2＋1 或 JSON/URDF 验收通过。
|
||||||
|
|
||||||
|
2026-09-22 实时诊断内存修复：不再为诊断强制保留全场完整记录或训练几何；
|
||||||
|
方向结束验收与状态显示共用当前扫描窗口，完整报告在安全返回、关闭采集后从
|
||||||
|
不可变日志按需读取。`test_diagnostic_coordinator.py`、
|
||||||
|
`test_diagnostic_reference_flow.py`、`test_capture_runtime_index.py` 首轮 28 项通过、
|
||||||
|
2 项旧测试因直接解析压缩索引失败，115.33 秒（超过日常目标；后续仅复查受影响项）。
|
||||||
|
改用正式 `load_jsonl` 后失败两项通过，18.25 秒；新增当前扫描身份/容器绑定断言后
|
||||||
|
只复查一项完整流程，25.96 秒（与实机并行时耗时）。没有改变生产哈希或准入规则。
|
||||||
|
只读旧证据保留模型回放及四倍记录量压力数据见
|
||||||
|
`software_review_20260922_stability/retention_validation_20260922.json`。
|
||||||
|
实机 `151025` 完成拇指 CMC roll、MCP 四方向，一次通过原始覆盖；最大 GC
|
||||||
|
0.183 秒、未发生反馈超时，因小指侧摆持续升到 72°C 主动中止。不是全手通过。
|
||||||
|
用户确认仍可运动后，先只读确认温度下降且无故障，再运行 `151821`：491.96 秒、
|
||||||
|
48069 条记录，前六任务的十二方向原始覆盖均一次完成，实测 RSS 最高约 341 MiB、
|
||||||
|
GC 最大 0.239 秒、控制回调最大 0.114 秒，无反馈超时。进入 DIP 时因上游 MCP/PIP
|
||||||
|
均无可信零位，`parent_reference_current_session_source_missing` 正确阻止父参考复用；
|
||||||
|
尚未修改此诊断依赖行为。全手其余任务及正式 2＋1、JSON/URDF 均未通过验收。
|
||||||
|
|
||||||
|
2026-09-22 机械手移动后新会话 `20260922_145145`：全手单轮非发布诊断计划
|
||||||
|
17 任务、20 关节、36 扫描单元，不恢复旧零位、不增加辅助动作。完成前五任务
|
||||||
|
共十个方向的原始覆盖；拇指 MCP 正向重扫一次，拇指 IP 与小指 MCP 零位未确认。
|
||||||
|
小指 PIP 准备阶段发生 1.783 秒 generation-2 GC 停顿，同期反馈时效保护暂停。
|
||||||
|
未完成全手测试，未发布 JSON/URDF；不能称作正式 2＋1 验收。
|
||||||
|
全手入口/报告/分析受影响组 47 项通过（6.32 秒）。随后修复诊断多通道分段身份
|
||||||
|
及各指独立 command/feedback 索引，`test_diagnostic_quality.py` 28 项通过
|
||||||
|
（1.99 秒）；四个分段均检查独立通道覆盖、错误分段拒绝、静止中指不得借其他指
|
||||||
|
运动通过。该修正在本次实机停止后完成，尚未实机验证，且不修复 GC 阻塞。
|
||||||
|
|
||||||
|
候选输出角度及共享不确定度诊断：`test_finger_chain_diagnostic.py` 新增轴符号/
|
||||||
|
2π 等价消除、真实零位差保留、与独立角度重拟合数值导数对照、相关协方差、
|
||||||
|
训练留出身份隔离及验证相机/尺寸/角色不变边界。首组 2 项通过（8.39 秒）；
|
||||||
|
增加输入一致性边界后仅重跑受影响验证组，6 项通过（6.79 秒），不累计为全量。
|
||||||
|
旁观机位诊断入口使用 `test_diagnostic_presets_cli.py`、
|
||||||
|
`test_diagnostic_observation_scope.py`、`test_diagnostic_yaw_hold_plan.py`，39 项
|
||||||
|
通过（1.56 秒），覆盖扫描单元不变、序列化兼容、设备/参考保护和非发布隔离。
|
||||||
|
真实 `142553` 小指一次往返原始采集完成，但 DIP 安装候选仍未区分；
|
||||||
|
`143540` 既有四指侧摆旁观测试因无名指硬件堵转暂停，用户要求重试后的只读
|
||||||
|
检查报过温，未再运动。细节见稳定性报告，软件通过不代表实机正式标定通过。
|
||||||
|
|
||||||
|
2026-09-22 用户最终确认 O30 标签黑框 16×16 mm 后，同步正式 Profile、检测器、
|
||||||
|
标定覆盖表及产品指纹。`test_o30_profile.py` 的来源指纹、跨配置尺寸一致性、
|
||||||
|
图像采集受影响组 3 项通过，0.80 秒；CLI `--validate-only` 通过，
|
||||||
|
`git diff --check` 通过。本次没有实机运动或发布，不代表 DIP 多解已经解决。
|
||||||
|
下文 16.5 mm 的确认及“正式配置未改”描述保留为此前实验阶段记录。
|
||||||
|
|
||||||
|
整指三阶段联合诊断使用 `test_finger_chain_diagnostic.py`：已知几何恢复、
|
||||||
|
Schur 与完整协方差对照、只拟合留出角度、64 组初值的每一位实际参与初始化、
|
||||||
|
参考漂移不得被隐藏、URDF 结构去冗余、分阶段参考以及共同标签尺度诊断。
|
||||||
|
初组 10 项通过，1.17 秒，耗时记录为
|
||||||
|
`software_review_20260922_stability/finger_chain_tests.json`。开发中仅重跑修改所
|
||||||
|
影响的诊断组或新边界，没有重跑未受影响的旧模型和整手产物链。随后新增消除
|
||||||
|
逐图像角度等价 2π 绕回，受影响验证与新边界 4 项通过，0.83 秒；只从六份已保存
|
||||||
|
结果重算分箱比较，没有重复拟合，主要歧义结论不变。
|
||||||
|
真实 `121712` 的六组 64 初值回放均保留独立结果；存在三维多解和尺度一致性
|
||||||
|
问题，详见稳定性报告。此模块不能授权实机角度或发布，不等同于正式 2＋1 通过。
|
||||||
|
用户随后要求固定 16.0/16.5 mm 对照，新增第七份 64 初值结果；比较脚本已核对
|
||||||
|
两组来源哈希、图像分区、结构约束和参考策略相同。16.0 mm 像素误差较低但
|
||||||
|
候选仍不唯一，正式配置未改；未因这次仅诊断输入变更重复无关测试。
|
||||||
|
|
||||||
|
完整父候选与联合父姿态诊断：`test_joint_pair_diagnostic.py` 检查已知真值恢复、
|
||||||
|
父姿态/角度 Schur 协方差与完整逆矩阵一致、固定几何验证、重复图像拒绝、
|
||||||
|
16 个失败初值保留及不能按验证误差选训练候选；不生成发布模型。
|
||||||
|
首次与配置策略组 13 项通过、1.42 秒；向量化/共用有界求解器后受影响 5 项
|
||||||
|
通过、0.69 秒；新增拒绝边界 1 项通过、0.42 秒，验证接口变更受影响 2 项
|
||||||
|
通过、0.65 秒，候选选择边界 1 项通过、0.39 秒。
|
||||||
|
`test_rigid_pair_observer.py -k 'not independent_parent_branches'` 的历史真实回读、
|
||||||
|
篡改和新旧父候选调用边界 9 项通过、20.37 秒；迁移/准备契约/已知真值候选组
|
||||||
|
32 项通过、5.64 秒。没有重跑未受影响的独立补采重拟合或整手产物链。
|
||||||
|
新版无辅助动作配置只读验证通过；`121712` 经新运行时桥接入口正确拒绝歧义。
|
||||||
|
联合诊断的真实 16 初值回放有像素改善，但父几何和候选比较仍不通过，详见稳定性
|
||||||
|
报告；这些软件分组不累计为一次全量测试，也不等同于实机或 JSON/URDF 验收。
|
||||||
|
|
||||||
|
共享零位及双端交接反馈契约复用 `staged_revisit_v2`：新 O30/u8 跨次容差 10，
|
||||||
|
窗内稳定性 2，旧无版本记录仍为 4。`test_shared_tag_pose_bridge.py` 与
|
||||||
|
`test_shared_pose_handoff.py` 首轮 45 项通过、3 项失败，19.03 秒；失败为两个
|
||||||
|
JSON 列表/元组直接相等断言，以及一个未完成任务本不应导入的旧测试假设。
|
||||||
|
改为反序列化后的物理参考相等、区分完成/未完成任务归属，不修改生产恢复规则。
|
||||||
|
针对复查及新增反馈契约篡改组 5 项通过、4.27 秒；源窗口的 7/10/11 边界、
|
||||||
|
旧版门限及窗内抖动新增组 3 项通过、1.30 秒。没有无理由重复完整拟合。
|
||||||
|
真实 `114118` 只读 PIP 回放在新反馈契约下通过独立父姿态检查和准备解算；
|
||||||
|
不把此软件结果视为 DIP 或整手实机通过。
|
||||||
|
随后 `121712` 无辅助侧摆实机完成六方向原始采集、无需重扫；MCP/PIP 和
|
||||||
|
DIP 父参考通过，DIP 几何因约 1.964 px / 4.864° 继续拒绝，未发布。
|
||||||
|
同批只读约束消融保留拒绝结果，详见稳定性报告；没有将删除约束作为修复方案。
|
||||||
|
|
||||||
|
实际启动验证纠正了上轮导出判断：固定 2＋1 分区不是执行决策，应从策略派生，
|
||||||
|
不能填入禁止 YAML 导出的 `task_training_cycles`。修正后新策略/观测策略组
|
||||||
|
15 项通过、16.87 秒，包含正式合成 JSON→URDF；补充有效 YAML 完整往返后，
|
||||||
|
只重跑该项，1 项通过、1.04 秒。单次 v3 预算记录由实机暴露，新增
|
||||||
|
`test_single_trajectory_action_declares_budget_and_cannot_escalate`，与原两次预算恢复
|
||||||
|
用例共 2 项通过、0.78 秒。实机 `113510` 的旧记录保留拒绝；`114118` 的新记录
|
||||||
|
284 帧通过辅助证据验收，但因 PIP 父模型缺失停止，不能写成 DIP 精度通过。
|
||||||
|
|
||||||
|
2＋1 辅助观测接入修正：`test_observer_policy.py` 同时检查 staged 与严格 2＋1，
|
||||||
|
`test_taskwise_two_round.py` 检查实际 v3 依赖/动作编译而非只检查参数接受，且扫描顺序不变。
|
||||||
|
初次 quick 组 13 项通过、1 项导出断言失败、1 项未选（3.22 秒）：运行期分区按现有契约
|
||||||
|
不可导出为可复用 YAML，修正测试预期后该项通过（1.14 秒）。后续策略与真实双标签回放组
|
||||||
|
17 项通过、107.59 秒，其中独立父分支真实补采回放耗时 65.68 秒；未再重复整手产物测试。
|
||||||
|
修正后的实际 CLI `--validate-only` 通过。真实小指的零新增运动反事实对照仍因几何精度
|
||||||
|
不足拒绝，详见 `O30_STABILITY_REVIEW_20260922.md`，不能称为实机修复完成。
|
||||||
|
|
||||||
|
2026-09-22 严格逐任务 2＋1 用 `test_taskwise_two_round.py`：quick 部分检查
|
||||||
|
逐任务顺序、轮间不重复准备、禁止补轮、断点分区及不可观测不跨任务推迟；
|
||||||
|
integration 部分复用正式 O30 夹具，验证 20 关节拟合、独立留出及 JSON 重建相同 URDF。
|
||||||
|
与 `test_diagnostic_quality.py` 共 28 项通过，16.08 秒。诊断压缩证据读取用
|
||||||
|
`test_diagnostic_analysis.py`，19 项通过、2.24 秒。旧准备契约测试直接读取
|
||||||
|
压缩索引导致一次失败，改用公共 `load_jsonl` 后只复查受影响组，30 项通过、
|
||||||
|
1 项未选，3.32 秒。各组不累计为全量通过；实机边界见
|
||||||
|
`O30_STABILITY_REVIEW_20260922.md`。
|
||||||
|
|
||||||
|
辅助侧摆端点离开条件使用 `test_witness_endpoint.py`。真实夹具
|
||||||
|
`o30_witness_terminal_gap.json.gz` 来自 `20260920_183257` 的 226 帧完整侧摆窗口，
|
||||||
|
末尾连续三帧缺命令，仅余两帧有效尾部。真实记录必须继续拒绝；追加的模拟后续帧
|
||||||
|
仅验证实时等待门，不能作为实机或发布证据。与 `test_preparation_witness.py`、
|
||||||
|
`test_preparation_sync.py` 共 53 项通过，14.89 秒，检查在线和离线共用端点窗口、
|
||||||
|
原超时上限、旧记录兼容、数据篡改和辅助拟合失败仍拒绝。
|
||||||
|
|
||||||
|
准备契约升级用 `test_preparation_contract.py`，覆盖新旧断点分离、父子/预算/动作
|
||||||
|
变化、历史记录保留、原会话边界、节点写入和发布入口禁止旧模型冒充新预算。
|
||||||
|
时钟自适应刷新用 `test_camera_timing.py`，正负平滑漂移在模拟 30 Hz 的 60 秒内
|
||||||
|
曝光时间误差保持小于原 1 ms 连续性预算，同时验证真跳变仍拒绝及启动锚点年龄。
|
||||||
|
辅助精度预算前置用 `test_observer_pose.py` 和 `test_observer_reference.py`。
|
||||||
|
|
||||||
|
此次增量验证分组记录在 `software_review_stability_repair`：初始针对组 79 项、8.84 秒；
|
||||||
|
证据/真实 PIP-DIP 回放/runner/合成完整产物组 91 项、58.99 秒；修正启动锚点刷新
|
||||||
|
时机后的相机组 15 项、0.47 秒。组间有重复用例，不将累计次数称为一次全量回归。
|
||||||
|
实际目录的断点选择只读验证约 17 ms，排除了旧准备契约；配置 validate-only 通过。
|
||||||
|
这些结果不替代新版本的连续实机验收。
|
||||||
|
|
||||||
|
相机时钟诊断变更使用 `test_camera_timing.py`：同时覆盖原始计时单位、跳变、陈旧帧、
|
||||||
|
重同步顺序及拒绝事务可重放。新增 `CLOCK_MONOTONIC_RAW` 仅记录诊断边界，不能
|
||||||
|
替换 ROS 时间或放宽同步门限。2026-09-20 针对组 12 项通过，0.33 秒，记录于
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_stability_architecture/camera_timing_tests.json`。
|
||||||
|
这不表示实机重复失同步已经修复；整体评估见 `O30_STABILITY_ASSESSMENT_20260920.md`。
|
||||||
|
|
||||||
|
日常修改运行受影响用例和必要契约用例，目标为 60 秒内完成。不要因为修改了一行代码就运行
|
||||||
|
整手拟合或全量 `colcon test`。公共产物契约变更需要覆盖相关型号，发布前再执行全量回归。
|
||||||
|
|
||||||
|
测试保留原文件位置，避免破坏现有共享夹具。`test/conftest.py` 集中维护分类；函数上的显式标记优先。
|
||||||
|
|
||||||
|
| 标记 | 内容 | 何时执行 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| `quick` | 局部求解、调度、恢复、隔离和拒绝边界 | 日常选择受影响文件 |
|
||||||
|
| `replay` | 已记录的真实图像、准备失败和姿态歧义 | 修改对应观测或恢复逻辑 |
|
||||||
|
| `integration` | 完整拟合、产物回读、进程或 ROS 链路 | 修改公共契约或发布前 |
|
||||||
|
| `legacy` | 历史格式、旧型号算法及兼容入口 | 修改兼容契约或发布前 |
|
||||||
|
|
||||||
|
从工作区根目录准备环境：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
source /opt/ros/jazzy/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
export PYTHONPATH="$PWD/src/linkerhand_calibration:${PYTHONPATH:-}"
|
||||||
|
export PYTHONDONTWRITEBYTECODE=1
|
||||||
|
```
|
||||||
|
|
||||||
|
采集计划、训练接口日常验证（按改动选择文件，不必每次全部执行）：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 -m pytest -q -m quick \
|
||||||
|
src/linkerhand_calibration/test/test_capture_plan.py \
|
||||||
|
src/linkerhand_calibration/test/test_adaptive_training.py \
|
||||||
|
src/linkerhand_calibration/test/test_command_motion.py \
|
||||||
|
--durations=8 --timings-json=/tmp/calibration-quick.json
|
||||||
|
```
|
||||||
|
|
||||||
|
涉及零位恢复时加 `test_zero_recovery.py`；涉及断点时加 `test_prepared_resume.py`、
|
||||||
|
`test_passed_resume.py`；涉及发布时加 `test_artifact_publication.py`。真实中指 DIP 歧义回归位于
|
||||||
|
`test_measured_transfer.py::test_real_middle_dip_fits_but_remains_ambiguous`，其正确结果仍为拒绝不充分证据。
|
||||||
|
|
||||||
|
`test_prepared_resume.py::test_resume_tick_uses_prepared_data_and_still_checks_current_reference`
|
||||||
|
同时覆盖 O30 分阶段恢复：历史证据索引须在启动准备时完成，实时回调不能再次整理整份历史；
|
||||||
|
参考变化仍拒绝复用。五个型号、参考保持/变化共 10 项约 2 秒。真实 `20260920_141822`
|
||||||
|
在该索引阶段发生控制回调阻塞，修复后的 `20260920_142046` 已实际复用 4 个扫描单元；
|
||||||
|
后续因小指零位 ID4 检测断续停止，不代表整手通过。
|
||||||
|
|
||||||
|
O30 回访反馈重复性修改，先运行 `test_staged_training.py -k revisit`：真实反馈 48→41、
|
||||||
|
10 的边界、超限、同批不稳定、图像/安装变化以及 v1/v2 回读共 8 项约 1.2 秒。
|
||||||
|
涉及复核记录格式时，追加 `test_staged_artifacts.py` 的 JSON→URDF 一致性和阶段拒绝组，
|
||||||
|
共享一次整手产物，5 项约 31 秒；无需为每个拒绝条件重复完整拟合。
|
||||||
|
|
||||||
|
分阶段调度、旁观证据和恢复的快速检查：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 -m pytest -q -m quick \
|
||||||
|
src/linkerhand_calibration/test/test_staged_training.py \
|
||||||
|
src/linkerhand_calibration/test/test_staged_runtime.py \
|
||||||
|
src/linkerhand_calibration/test/test_observer_pose.py \
|
||||||
|
src/linkerhand_calibration/test/test_observer_reference.py \
|
||||||
|
--durations=8 --timings-json=/tmp/calibration-staged-quick.json
|
||||||
|
```
|
||||||
|
|
||||||
|
`test_observer_reference.py` 共享一次上下游图像求解，角点、来源、安装和取样篡改直接检验
|
||||||
|
证据回读边界。`test_staged_runtime.py` 的进程复用用例及整手训练用例标为 `integration`。
|
||||||
|
只有训练/冻结/发布契约变化时，才运行下面的分阶段整手组；本次初次完整组 19 项约 45 秒，
|
||||||
|
后增加的回访时序拒绝用例随产物组单独复查，未重复其余已通过的完整拟合：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 -m pytest -q \
|
||||||
|
src/linkerhand_calibration/test/test_staged_training.py \
|
||||||
|
src/linkerhand_calibration/test/test_staged_runtime.py \
|
||||||
|
src/linkerhand_calibration/test/test_staged_artifacts.py \
|
||||||
|
--durations=8 --timings-json=/tmp/calibration-staged-integration.json
|
||||||
|
```
|
||||||
|
|
||||||
|
该组共享整手输入和产物，包含实际混合轮数统计、冻结后不再训练、一次集中补采、失败终止、
|
||||||
|
断点及 JSON→URDF 链路。下面的旧策略整手组仅在旧策略相关逻辑也受影响时追加。
|
||||||
|
|
||||||
|
共享 Tag 的进入姿态联合准备修改，运行真实 MCP 的专门回放组：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 -m pytest -q \
|
||||||
|
src/linkerhand_calibration/test/test_preparation_transition.py \
|
||||||
|
--durations=8 --timings-json=/tmp/calibration-preparation-transition.json
|
||||||
|
```
|
||||||
|
|
||||||
|
该组共享一次真实图像求解，检查正式运行时继续、原始证据回读、缺失辅助数据、留出隔离、
|
||||||
|
保持通道/参考变化、篡改拒绝及取消清理，通常约 10 秒。只有候选选择公共逻辑受影响时追加
|
||||||
|
`test_image_model_selection.py` 的相关用例；已有通过的完整拟合不为文档或断言修正重复执行。
|
||||||
|
本次验证记录位于 `calibration_output/O30_RIGHT_001/software_review_preparation_transition/`，
|
||||||
|
保留失败与针对性复查关系，不把软件回放写成实机通过。
|
||||||
|
|
||||||
|
测量几何候选的精度资格与图像评估分离，回归位于 `test_measured_transfer.py` 的
|
||||||
|
`test_real_ip_compares_uncertain_alternative_before_authorizing`、
|
||||||
|
`test_uncertain_alternative_cannot_be_frozen` 和
|
||||||
|
`test_uncertain_alternative_still_vetoes_when_images_do_not_distinguish`。
|
||||||
|
三项共享 `20260920_140424` 的 129 帧真实 IP 准备及父几何证据；精度不足的候选仍参与
|
||||||
|
留出区分，不能冻结；无法区分仍拒绝。相关旧中指 DIP、候选选择及回读边界共 17 项
|
||||||
|
耗时 5.39 秒，后补否决边界 1 项耗时 2.16 秒。记录位于
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_ip_alternative/`，不代表整手实机通过。
|
||||||
|
|
||||||
|
完整 O30 产物链路按需单独执行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 -m pytest -q \
|
||||||
|
src/linkerhand_calibration/test/test_o30_artifacts.py::test_o30_nonlinear_twenty_channel_finalization \
|
||||||
|
--durations=8 --timings-json=/tmp/calibration-o30-integration.json
|
||||||
|
```
|
||||||
|
|
||||||
|
该用例从正式采集证据入口进入，检查采样完成、训练决策回放、运动授权、受保护相机文件、
|
||||||
|
原始角点、JSON→URDF 重建和回读。输入来自独立 FK 合成场景，使用兼容的几何运动授权；
|
||||||
|
不模拟在线图像模型选择，不替代实机连续采集及精度验收。末节 CAD 零位假设仍须保留原语义。
|
||||||
|
|
||||||
|
整手输入由会话级夹具共享，只读使用；需要篡改的用例先复制。反馈缺失、反馈卡住已改为局部
|
||||||
|
映射契约测试，不再各生成一套整手 JSON/URDF。篡改、取消和失败指针检查优先复用验收边界。
|
||||||
|
|
||||||
|
`--timings-json` 记录各用例 setup/call/teardown 耗时、结果、层级和 Python/YAML 源码指纹。
|
||||||
|
同一版本通过的测试不无理由复跑；修复失败后只重跑失败及受新改动影响的用例。裸 `pytest`
|
||||||
|
仍会运行全量，标记不会悄悄关闭测试。分层收集可用 `--collect-only -m replay` 等命令检查。
|
||||||
|
|
||||||
|
上一版逐任务自适应方案的软件验证记录保存在工作区
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_adaptive_training/software_validation.json`，
|
||||||
|
同目录保留各组耗时记录和历史数据对照。O30 正式产物链路与混合轮数统计约 44 秒，
|
||||||
|
恢复、断点及真实 DIP 歧义回归约 19 秒，在线训练评估与取消约 6 秒。记录保留开发期间
|
||||||
|
的失败及修复后复查关系，不将不同代码版本的增量检查称为一次最终全量回归。
|
||||||
|
|
||||||
|
本次分阶段方案记录在
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_staged_training/software_validation.json`。
|
||||||
|
同目录保留各次运行的源码指纹、耗时及开发失败，报告标明修复关系。新旧策略、旁观图像、
|
||||||
|
原真实 DIP 拒绝和文件链路均分别记录；不重复旧的 2.88 GB 日志性能测量，不宣称实机已通过。
|
||||||
|
|
||||||
|
遮挡感知准备策略的日常检查使用 `test_preparation_witness.py`：真实无名指停点保留为
|
||||||
|
`fixtures/o30_ring_mcp_ambiguous.json.gz`，合成独立侧摆源明确标注，模块共享一次原始
|
||||||
|
拟合和一次新增证据。覆盖逐指避让后单通道往返、整数 SDK 目标、回访不重采、
|
||||||
|
训练/留出隔离、姿态改变、完整窗口、篡改、强制证据缺失及正式读回。只读策略为
|
||||||
|
`--preparation-policy occlusion_aware_witness_v1 --validate-only`，不启动硬件。
|
||||||
|
|
||||||
|
最终遮挡版：该文件加 Profile 加载、采集计划、原图像采集契约，共 58 项约 14.12 秒。
|
||||||
|
源码迭代与现场遮挡澄清导致的增量复查分别记录，不把开发过程累计次数称为一次回归。
|
||||||
|
记录目录为 `calibration_output/O30_RIGHT_001/software_review_preparation_witness/`。
|
||||||
|
新策略仍需完整实机对照；原 O30 JSON→URDF 及取消发布两项共享整手夹具约 32.12 秒，
|
||||||
|
没有逐个拒绝条件重新拟合整手，也没有运行全量 pytest/colcon。
|
||||||
|
|
||||||
|
准备优化器统一恢复检查位于 `test_image_bundle_optimizer.py`。快速用例检查计算次数、
|
||||||
|
内存上限、不收敛/异常/非有限值/误差增大时继续拒绝；真实回放标记为 `replay`，共享
|
||||||
|
一次 `20260920_154722` 拇指 IP 求解。夹具保留真实父参考、上游准备记录及其原始角点，
|
||||||
|
通过正式准备、来源几何和父参考验收；不能仅用孤立合成输出代替这些入口。
|
||||||
|
|
||||||
|
该次真实候选原先耗尽 150 次稀疏求值，使用同一训练证据暖启动直接解法后，再 6 次
|
||||||
|
收敛，随后被原 1.5 px 门限拒绝，合格候选得以通过。未收敛候选仍否决，不参与绕过
|
||||||
|
门限的比较；留出数据不决定计算恢复。主回归 24 项约 32.20 秒，覆盖旧 IP 精度不足
|
||||||
|
候选、真实中指/无名指歧义拒绝、转接/共享姿态回读、小指历史预算、训练/留出隔离和
|
||||||
|
工作进程取消。新增用例开发失败及针对性复查分别保存于
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_solver_recovery/`。
|
||||||
|
未重跑整手拟合和 JSON→URDF 产物组；本次改变的是准备求解及其证据绑定，产物生成、
|
||||||
|
最终角度/像素门限没有修改。软件回放通过不代表整手实机已经通过。
|
||||||
|
|
||||||
|
准备采样有效性与辅助适用性使用 `test_preparation_sync.py`：夹具来自真实
|
||||||
|
`20260920_160630` 小指完整准备窗口，保留两帧原始缺指令值。检查完整原始窗口回读、
|
||||||
|
排除记录绑定、缺值行上的保持关节变化、持续缺样/端点/身份变化拒绝，以及拟合前
|
||||||
|
辅助不适用时主模型独立验收，拟合后不改路线。与 `test_preparation_witness.py` 的
|
||||||
|
原歧义和篡改检查合计 41 项、10.58 秒；历史 v1 和拟合失败拒绝另 3 项、3.97 秒。
|
||||||
|
后补缺候选不可视为转角不足的边界单独执行；没有无理由重跑前述已通过组。
|
||||||
|
验证记录位于 `calibration_output/O30_RIGHT_001/software_review_preparation_admission/`。
|
||||||
|
|
||||||
|
双端共享姿态交接使用 `test_shared_pose_handoff.py`，夹具来自 `20260920_163002`，
|
||||||
|
包括原 MCP 模型/零位/像素来源、上游回基准原始窗口和 PIP 准备完整证据。
|
||||||
|
共享一次 PIP 拟合，检查原失败复现、新双端验证、原门限、正式回读、数据篡改、验证轮
|
||||||
|
隔离、源图像先于全部新准备数据、断点依赖导入/去重、恢复代次和四指模型归属。
|
||||||
|
DIP 用真实像素模拟后续访问，经 `ObservationCapture`、父参考确认和文件回读，
|
||||||
|
下游模型外壳明确为合成数据，不能宣称整指实机通过。
|
||||||
|
|
||||||
|
本轮父参考、有限恢复、旧断点及旧版共享姿态 62 项通过，20.95 秒；最终针对组
|
||||||
|
25 项通过，4.11 秒（含 1 项已受修改影响的旧父参考回读）。随后新增时序边界与导入
|
||||||
|
去重只执行相应用例。现有 O30 正式采集证据入口 JSON→URDF 集成单独运行一次，
|
||||||
|
31.24 秒。开发中的夹具缺来源、策略字段绑定和 ROS 环境收集失败保留原记录，
|
||||||
|
不计为通过；没有全量 pytest 或 colcon。完整记录见
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_shared_pose/`。
|
||||||
|
|
||||||
|
准备观测预算的实机像素回归使用 `test_preparation_density.py`，夹具为
|
||||||
|
`20260920_171710` 的完整小指 PIP/DIP 准备窗口、上游模型和双端交接图像。新 PIP
|
||||||
|
模型重新投影原 DIP 像素后，验证更完整的训练/留出分组、源模型精度传递、候选区分
|
||||||
|
及模型回读；冻结旧父模型仍拒绝，四指多关节求解不扩容。此回放带有明确的离线姿态
|
||||||
|
准入标记，不声称它是一次可发布的实机采集。正式产物另用已有 staged 产物组检验。
|
||||||
|
|
||||||
|
`test_checkpoint_startup.py` 检查断点加载心跳、独立 900 秒上限、恢复后设备/服务
|
||||||
|
时限、异常清理及无断点不发布加载状态。相关启动/共享姿态/真实密集回放 98 项
|
||||||
|
14.79 秒；产物回读、阶段隔离和取消发布 8 项 24.11 秒。前一批 85 项通过、1 项
|
||||||
|
失败是历史测试将最大像素误差写死为 0.75；新增留出图像为 0.752，实际契约为
|
||||||
|
1.5 px。现已断言正式契约，并复查完整共享姿态回读。环境收集失败、反事实回放
|
||||||
|
拒绝分类断言修正和后续通过记录均保留，不混算为一次完整实机回归。
|
||||||
|
|
||||||
|
扩展到准备采集/辅助证据入口后，发现在线密集预算未传入离线重拟合，已补齐
|
||||||
|
`image_sampling_policy` 的版本、报告与角色哈希绑定、重放参数及帧数上限校验。
|
||||||
|
后续来源/辅助/真实回放组 121 项通过、2 项为测试断言失败；原断言分别固定了错误
|
||||||
|
文本和旧采样子集模型。修正为准确拒绝码及相同预算的离线对照后，仅复查失败用例、
|
||||||
|
受影响的启动与产物边界，16 项通过，32.54 秒。发布线程清理再检查 7 项通过。
|
||||||
|
记录完整保存在 `software_review_stability_density`，没有将多版本增量测试宣称为
|
||||||
|
一次全量回归。实机尝试 `20260920_174317` 在发起标定运动前被现有 ROS 控制节点占用
|
||||||
|
阻止，未产生可交付 JSON/URDF。
|
||||||
|
|
||||||
|
完整已声明归零路径用作辅助观测时，运行 `test_declared_witness_path.py`：
|
||||||
|
只有完全相同避让姿态下的既有单关节归零路径可超过默认四分之一行程，
|
||||||
|
任意扩幅、其他关节变化及未经声明的侧摆全行程仍拒绝。
|
||||||
|
`test_conditioned_witness.py` 使用 `20260920_191613` 的真实中指 PIP/MCP 图像，
|
||||||
|
覆盖原来只作事后检查时的共享姿态失败、独立源验证后参与共享零位拟合、
|
||||||
|
正式证据回读以及端点变化/策略降级拒绝。旧 v1/v2 辅助证据继续按原含义读取；
|
||||||
|
新 v3 的共享姿态条件绑定进准备契约，不能复用旧契约模型。
|
||||||
|
|
||||||
|
`test_rigid_pair_observer.py` 使用 `20260920_192330` 的中指 MCP/PIP/DIP 原始
|
||||||
|
图像，验证双 Tag 联合拟合、原 1°/1 mm 来源门限、DIP 有界父几何传递和正式证据
|
||||||
|
回读；保持关节变化、源验证角点变化、当前端点变化及删除必需证据均拒绝。
|
||||||
|
`test_observer_pose.py` 另用已知真值和故意偏置的父姿态初始化验证相对安装恢复，
|
||||||
|
防止把历史父模型当成精确测量。真实夹具包含上游完整来源和会话头。
|
||||||
|
受影响的旧观测/共享姿态/几何传递/配置/准备契约组 86 项通过，28.21 秒；
|
||||||
|
新刚体对与 staged JSON/URDF 产物链组 14 项通过，38.29 秒。
|
||||||
|
初次缺 ROS 环境的收集失败不计入通过；上述结果属于软件回放，不能代替整手实机验收。
|
||||||
|
|
||||||
|
staged `passed` 续采检查 `test_passed_resume.py` 的真实 coordinator 导入边界：
|
||||||
|
已通过访问必须先完整落盘才能跳过，未完成和未来轮次不被授权；导入中止和反馈失效仍暂停。
|
||||||
|
相关 staged 调度和断点启动组首次 21 项通过、4 项夹具失败（O6 不支持 staged、
|
||||||
|
旧合成观测缺会话字段）；修正夹具后只重跑受影响的 6 项，1.39 秒全部通过。
|
||||||
|
|
||||||
|
统一观测策略使用 `test_observer_policy.py`：覆盖整图依赖选择、断开的角色关系、
|
||||||
|
历史模式不变、通过关节策略保持、幂等、序列化和执行校验。
|
||||||
|
`test_checkpoint_observer_upgrade.py` 检查完整审计及拒绝改动既有模型、训练决策、
|
||||||
|
运动/精度契约和保护哈希。`test_rigid_pair_observer.py` 增加真实食指夹具
|
||||||
|
`o30_index_rigid_pair.json.gz`，明确验证有歧义的数据不会因开启新策略而误通过；
|
||||||
|
中指原通过、篡改拒绝和正式证据回读用例继续保留。
|
||||||
|
|
||||||
|
本轮策略/迁移/准备契约/真实回放组 30 项通过、19.43 秒，另 1 项因没有 source
|
||||||
|
ROS 环境导入失败；恢复环境后只重跑该项及受影响的配置/观测兼容组，18 项通过、
|
||||||
|
2.29 秒。启动参数和准备辅助兼容组 32 项通过、9.99 秒，共 80 项通过。
|
||||||
|
最初把食指真实数据预设为可通过的实验出现来源歧义，已记录为负例；未降低任何
|
||||||
|
精度、候选区分或发布门限。这些结果是软件验证，不是整手实机通过。
|
||||||
|
|
||||||
|
有界独立观测使用 `test_observer_preparation.py`：检查按共同祖先生成动作、四分之一
|
||||||
|
行程限制、已通过测量不变、画面内旋转不算视角覆盖、模拟真值恢复、命令/反馈/路径/
|
||||||
|
完成记录篡改拒绝、原始证据导入、运动预算跨重启保持、固定端点等待上限及真实食指
|
||||||
|
提前触发。`o30_observer_initial_hold.json.gz` 保存 `20260920_210259` 的初始静止
|
||||||
|
记录，用于检查稳定窗口与图像时效分离，以及未开始侧摆的计划可以审计后继续。
|
||||||
|
|
||||||
|
初次兼容组 40 项通过、1 项测试替身缺少新增控制器，20.78 秒；补齐测试替身后，
|
||||||
|
新流程及受影响用例 14 项通过、2.80 秒。准备契约、调度、全相机原始像素与既有
|
||||||
|
staged JSON/URDF 产物链 41 项通过、29.74 秒。采集计划兼容组 12 项通过、4.39 秒。
|
||||||
|
修正实机暴露的时效契约及未开始动作的恢复边界后，新流程和旧刚体对回放 22 项
|
||||||
|
通过、19.98 秒。各组有重复用例，不累计为一次全量通过。
|
||||||
|
|
||||||
|
|
||||||
|
观测补采有限序列与父标签跨次反馈契约：`test_observer_preparation.py`、
|
||||||
|
`test_parent_reference_reuse.py`、`test_parent_reference_provenance.py` 验证真实
|
||||||
|
`20260920_211220` 的完整小幅补采、247→255 的跨次反馈、原图像精度检查、
|
||||||
|
历史 v1 语义、两步预算及未开始动作的恢复。父参考组 58 项通过，7.69 秒；
|
||||||
|
序列/迁移/准备契约组 36 项通过，5.63 秒；序列/调度组 27 项通过，6.33 秒；
|
||||||
|
状态显示组 8 项通过，1.47 秒。新增预算用例初次因测试夹具命令和反馈共享列表
|
||||||
|
重复缩放失败，修正夹具后该项通过，0.81 秒；反馈规则篡改拒绝项初次即通过。
|
||||||
|
记录在 `software_review_active_observer_sequence`，这些分组有重复，不累计成全量回归。
|
||||||
|
|
||||||
|
|
||||||
|
训练中的双 Tag 父姿态分支优化：`test_rigid_pair_observer.py` 与
|
||||||
|
`test_observer_preparation.py` 共 25 项通过，47.56 秒，包含 213013 实机补采源、
|
||||||
|
旧中指正式证据回读、源不足拒绝、预算与保留断点。逐帧多初值最大 320 次评估，
|
||||||
|
全局交替最多三次；旧冻结父姿态路径保留原 80 次评估。独立 DIP 几何仍因精度不足拒绝，
|
||||||
|
该组通过只表示观测源/兼容机制通过，不能宣称整手或本批 DIP 通过。
|
||||||
|
|
||||||
|
## 原 MCP/PIP 训练回程参考适配与无侧摆实测（2026-09-22 19:06）
|
||||||
|
|
||||||
|
本轮新适配器使用已有隔离求解工作进程，采集源有界且不读取第三轮；只授权父参考
|
||||||
|
检查，不授权物理零位、角度样本或产物发布。完整 coordinator 合成运动集成首次
|
||||||
|
1 项通过，16.43 秒；增加图像身份及模型证据断言后，`test_chain_capture.py` 与
|
||||||
|
`test_chain_capture_runtime.py` 28 项通过，18.52 秒。新增来源分箱、训练冻结、
|
||||||
|
清理后旧结果拒绝、未解模型拒绝等边界随后 4 项通过，0.84 秒。
|
||||||
|
|
||||||
|
工作进程、旧初始化及 CLI 组 25 项通过，7.44 秒；包括真实 spawn 对新请求/结果
|
||||||
|
类型的传递。实测后仅更新描述文字，CLI/分析输出受影响组 30 项通过，1.83 秒。
|
||||||
|
以上存在重叠用例，不相加为一次全量回归，未重复运行所有已通过用例。
|
||||||
|
|
||||||
|
实机 `20260922_190634` 完成小指 MCP/PIP/DIP 每任务连续 2＋1：18 个方向单元
|
||||||
|
无暂停、无重扫；原始命令核查各三次往返，无侧摆或准备额外圈。冻结在线模型
|
||||||
|
后的第三轮几何检查 MCP/PIP 各 60/60 通过，最大 RMS 0.295269/0.448776 px；
|
||||||
|
DIP 入口父模型复用通过。`frozen_reference_live_audit.json` 留存逐帧结果。
|
||||||
|
这些是采集与几何检查通过，不等于 DIP 曲线、物理零位或 JSON/URDF 发布验收。
|
||||||
|
|
||||||
|
## 整指运动学联合模型离线检查（2026-09-22）
|
||||||
|
|
||||||
|
新增 `test_kinematic_chain_diagnostic.py` 和 `test_chain_candidate_audit.py`，
|
||||||
|
针对性运行 8 项通过，4.03 秒。覆盖共享铰链下的阶段参考姿态、合成真值恢复、
|
||||||
|
Schur 协方差与完整逆矩阵一致、冻结模型拒绝无法解释的阶段平移、相机坐标系
|
||||||
|
相关协方差，以及只按训练状态合并数值等价解、保留失败解和不等价候选。
|
||||||
|
|
||||||
|
真实 `20260922_142553` 回放完整枚举 64 个双标签初值，全部收敛；训练状态分成
|
||||||
|
4 个物理解族，原 0.01 / Holm 门限检验后保留 2 族。保留族的条件输出角度包络
|
||||||
|
为 MCP 0.136425°、PIP 0.197536°、DIP 0.655645°。原始拟合结果不重写，
|
||||||
|
分族和相机几何审计只读取保存参数，不重新拟合。证据位于
|
||||||
|
`calibration_output/O30_RIGHT_001/software_review_20260922_stability/` 下的
|
||||||
|
`kinematic_chain_camera_geometry_audit_142553.json`。
|
||||||
|
|
||||||
|
这不是正式标定稳定性通过：轴距采用精确 URDF 约束，验证仍逐帧估角，且反向数据
|
||||||
|
用于候选检验。尚缺物理零位绑定、冻结双向指令映射、未使用第三轮、完整输出误差
|
||||||
|
及 JSON/URDF 往返验收。新模块为非授权离线模型，未绕过正式父参考或发布门限。
|
||||||
|
|
||||||
|
## O30 raw_joint 正式链路（2026-09-22）
|
||||||
|
|
||||||
|
用户指定停滞重试（2026-09-22）：仅 O30 的 `mechanical_stall` 增加同一运动
|
||||||
|
首次尝试加两次重试，每次重发前等待 3 秒，第 3 次失败停止。等待不发送新位置
|
||||||
|
指令，不作为有效采样；失联、控制权冲突及其他保护仍检查。真实 coordinator
|
||||||
|
虚拟时钟验证三次失败、两次等待和等待期间保护：3 passed，1.00 秒。
|
||||||
|
既有安全及反馈组 44 passed；另一条旧启动测试补充显式复用相机参数后通过。
|
||||||
|
测试夹具曾缺少 SDK 心跳，已补齐模拟心跳，未因此改变生产通信保护。
|
||||||
|
|
||||||
|
新默认协议及完整边界见 `O30_RAW_PROTOCOL_IMPLEMENTATION.md`。
|
||||||
|
|
||||||
|
针对性验证（不同组有重叠，不相加宣称一次全量回归）：
|
||||||
|
|
||||||
|
- 原始采集、断点恢复、存储回归：35 passed，44.24 秒。覆盖首单元尚未通过的中断发现、跨恢复补采预算、实际 coordinator 三轮指令、缺失光学观测禁止运动。
|
||||||
|
- 全手运动效果及显式旧协议路径：3 passed，4.12 秒。原有两个接受关节角夹具改为显式 `TASKWISE_TWO_ROUND`，不修改新协议质量门限。
|
||||||
|
- 光学核验及候选集合门限：8 passed，5.84 秒。错误内外参、覆盖、序列号、同步拒绝；物理超差拒绝，标签安装不唯一不额外否决。
|
||||||
|
- 共享 CAD 图像导数、历史失败角点和发布回读：10 passed，51.30 秒；批量独立角度验证修正稀疏索引读取后，发布组 3 passed，10.92 秒。
|
||||||
|
- 外参工具、原子发布、状态及 O30 launch 公共契约：30 passed，2.32 秒。
|
||||||
|
- 最终候选门禁、共享整指/运动链及真实失败回归：23 passed，12.25 秒。正式终结入口缺少来源证据时，拒绝求解并保存失败状态：1 passed，4.76 秒。
|
||||||
|
- 光学核验采集同步上限统一为 50 ms 后，外参工具受影响组：10 passed，0.57 秒。源码编译与 `git diff --check` 通过。
|
||||||
|
- ID5/9/12 与 ID10 历史回归来自 `o30_raw_failure_observations.json.gz`，仅用于旧角点观察能力回归，不能恢复为新协议数据。
|
||||||
|
- `o30_training_candidate_parameters.npz` 保存两个合成训练解的参数，供配对损失/保留候选边界测试复用，不含任何实机标定授权。
|
||||||
|
|
||||||
|
完整合成真值数值回归：64 个初始化全部收敛，约 192.82 秒；每个候选总求值不超过 200。
|
||||||
|
训练像素门限排除 32 个，训练分层配对检验排除 16 个，剩余 16 个全部通过候选协方差与第三轮验证。
|
||||||
|
训练检验使用原 0.03 px 分辨率余量和 0.01 Holm 家族门限；第三轮不参与选择。
|
||||||
|
保留候选的零位三倍标准差与候选差异联合包络约 0.100°，绝对指令角约 0.192°，轴位置约 0.125 mm，FK 位置约 0.260 mm。
|
||||||
|
产物通过实际 robot_state_publisher、结构授权、JSON 重建及最终 FK 回读；后半程验收和发布约 10.38 秒。
|
||||||
|
只写入 `calibration_output/software_review_20260922_raw_joint/known_truth/` 和独立软件测试指针；实机 `latest_passed` 未修改。
|
||||||
|
|
||||||
|
未进行本次新协议全手实测或实机姿态对照；不能把以上软件结果当作现场精度证明。
|
||||||
|
|
||||||
|
|
||||||
|
## O30 轻量原始角点采集与立即补采（2026-09-23）
|
||||||
|
|
||||||
|
新增集中回归 `test_raw_lightweight.py`，不恢复外部删除的历史测试。覆盖当前方向立即补一次、
|
||||||
|
中断预算、仅固定标签在线跟踪、死区/平台、堵转/反向/噪声、静止采样冻结计时、
|
||||||
|
不稳定终点有界退出、采完禁止发送、封存校验、旧协议读取与混合拒绝、旧合成 URDF 重建。
|
||||||
|
|
||||||
|
首组 6 passed、5 failed（4.25 秒）：五个失败是扫描测试没有先进入任务避让姿态，
|
||||||
|
修正夹具起始向量后只复查该组，5 passed（0.98 秒）。后续边界组 10 passed、
|
||||||
|
1 failed（4.18 秒）：竞争控制测试缺少 Start 后反馈，补齐后单项 1 passed（0.77 秒）。
|
||||||
|
完整真实 coordinator 虚拟时钟、单主机位移动标签、108 方向、无补采、归位封存与
|
||||||
|
训练/验证分区检查 1 passed（85.13 秒），属于必要整手调度集成，不是实机通过。
|
||||||
|
旧合成产物 v1/v2 JSON→URDF 重建与终态入口 3 passed（0.87 秒）。
|
||||||
|
删重复图像记录并保留未稳定原始帧后，受影响采集/补采恢复/封存/兼容组
|
||||||
|
7 passed（6.25 秒）。各组存在重叠，不累计为一次全量测试。
|
||||||
|
|
||||||
|
正式 CLI 配置校验通过。实机 `20260923_104837` 去程通过，拇指旋转回程指令 162、
|
||||||
|
反馈 253，累计实际运动无推进 4.005 秒触发保护；未重复启动，控制栈已退出，
|
||||||
|
停在当前位置，未执行自动归位。仅 1/108 方向完成，未封存为完整采集、未发布 URDF。
|
||||||
|
同段移动标签角点最大位移约 0.47 px、固定基准约 0.50 px，未见明显回程运动；
|
||||||
|
不能据此删除停滞保护，也不能宣称全手稳定采集通过。
|
||||||
|
|
||||||
|
补充只读离线入口检查初次失败（3.61 秒）：虚拟相机夹具缺少实际入口要求的
|
||||||
|
`rectified_camera_model` 日志，补齐夹具后在后续组通过，确认缺项拒绝发生于
|
||||||
|
离线验收、原始文件哈希不变且无新运动。
|
||||||
|
用户随后明确授权硬件停滞重试一次：复用现有 StallRetry，O30 原始采集限制为
|
||||||
|
首次尝试加一次重试，等待 3 秒，同方向共享预算；其他硬件保护不进入重试。
|
||||||
|
首组 6 passed、1 failed（4.19 秒）：测试将保护停止发送的最后保持指令也误认为
|
||||||
|
等待期间推进，修正断言后四种实际 coordinator 场景 4 passed（1.31 秒）。
|
||||||
|
运动保护代码和计划身份同步更新，旧策略会话不得混入新断点。
|
||||||
|
|
||||||
|
补充端点阶段切换与反馈抖动回归：180 ms 图像延迟单项通过（7.87 秒），
|
||||||
|
扩大到 300 ms 并增加 4 u8 往返抖动后复现 3 个失败、4 个通过（14.95 秒）。
|
||||||
|
隔离模块验证有限端点等待和推进历史最高值修正，8 passed（7.98 秒），未影响当时运行的实机。
|
||||||
|
用户随后明确端点只看实际发送 SDK 指令：最终采用有效稳态记录中的已发送指令核验端点，
|
||||||
|
不额外增加端点等待、不要求反馈到 0/255，也不把稳态图混入运动覆盖。
|
||||||
|
真实 coordinator 300 ms 延迟、反馈端点仅 10/240、离线误判复核与中断拒绝，
|
||||||
|
加上方向推进、补采、封存及旧协议边界，17 passed（10.04 秒）。
|
||||||
|
SDK 身份错误保留原字段、其他硬件保护和单次停滞重试、旧合成 URDF 重建，
|
||||||
|
9 passed（1.86 秒）。
|
||||||
|
|
||||||
|
实机 `20260923_110651` 完成 35 个方向，原记录 29 个通过、6 个端点误判；
|
||||||
|
在小指 PIP 验证回程因 `o30_sdk_identity_mismatch` 停止，反馈仍约 29.81 Hz。
|
||||||
|
未触发停滞重试，未归位封存，未更新 `latest_passed`。异常身份字段在旧日志中缺失，
|
||||||
|
不能将此状态直接等同于物理通信断开。修正后只读复核同一数据，35 个完整方向均通过，
|
||||||
|
缺 73 个方向，禁止发布，源数据哈希未变化；报告见实现文档。
|
||||||
|
|
||||||
|
最终端点与推进规则下，完整 coordinator 单主机位、108 方向、归位封存及
|
||||||
|
训练/验证分区回归：1 passed（87.51 秒）。该必要整手集成仍仅为软件验证。
|
||||||
|
源码编译和 `git diff --check` 通过；未恢复外部删除的历史测试。
|
||||||
|
|
||||||
|
2026-09-23 用户确认 ID17 在拇指近端指节后修正安装连杆及产品哈希。
|
||||||
|
正式配置加载、拓扑反馈依赖 0/1/6、仅顶机位、17 任务/108 方向校验通过；
|
||||||
|
临时校验脚本首次误用 TagSpec.id，改为 tag_id 后通过，生产加载未失败。
|
||||||
|
既有原始角点、立即补采和旧协议边界回归 3 passed（0.85 秒）。
|
||||||
|
现有真实数据的局部拟合揭示原绑定错误，但修正后最大训练标签误差仍为 8.047 px，
|
||||||
|
原精度门限未通过；软件回归通过不代表实机 URDF 可发布。未重新运动或改写源证据。
|
||||||
|
|
||||||
|
2026-09-23 低精度离线草稿：新增集中 `test_raw_draft.py`，加已有合成正式 URDF
|
||||||
|
重建检查，13 passed（1.55 秒）。覆盖真实零位写入与 FK、缺测关节不改、JSON 零位/
|
||||||
|
曲线/来源篡改拒绝、不可观测零位拒绝、精度超限仍允许草稿、旧 Profile 仅允许标签
|
||||||
|
连杆修正、同步缺失帧不供给范围证据，以及 CLI 不进入运动或正式发布。
|
||||||
|
实测旧数据的正式 CLI 草稿路径已运行，生成完整结构 URDF 并通过标准 ROS 加载；
|
||||||
|
JSON/URDF FK 最大差 4.44e-16，两个拇指零位和四个行程实际改变。
|
||||||
|
独立精度验收仍失败，输出 `accuracy_passed=false`，没有更新 `latest_passed`。
|
||||||
|
第一次实数据导出因原始帧含空指令失败，修复共享证据过滤后 v2 成功；未删除失败审计。
|
||||||
|
|
||||||
|
2026-09-23 全手 15 零位/20 行程目标及采集后自动导出：受影响范围与真实 runner 生命周期
|
||||||
|
5 passed（1.75 秒），验证完整目标、部分数据不能冒充全手，以及成功/缺项/求解失败三种
|
||||||
|
情况下 SDK 栈、监控节点和自建 ROS 上下文均先关闭，离线阶段从不重新启动运动。
|
||||||
|
补充 Profile 原字节快照后，仅复查三个生命周期场景，3 passed（1.66 秒）。
|
||||||
|
正式 CLI --validate-only 通过，明确列出 15 个零位和 20 个行程目标。
|
||||||
|
|
||||||
|
必要的一次全手数学检查：独立 XML FK 生成 2028 帧训练角点，463 参数图像模型单初值
|
||||||
|
12 次联合迭代收敛；包括生成数据和秩检查共 31.82 秒，最大零位差 0.001054°。
|
||||||
|
复用保存参数进行完整 JSON→URDF 导出与标准加载，不重复拟合；15 个零位修正、五个
|
||||||
|
末端零位保留、20 个行程、最终 FK 最大差 5.55e-16 均通过。非实机验收、不更新正式指针。
|
||||||
|
|
||||||
|
2026-09-23 ID10 调整后的草稿断点：新增 `test_raw_remount.py`，验证 78 个连续通过方向
|
||||||
|
保留、旧 ID10 角点排除、原始像素不改、已用补采次数保留、重复调整声明拒绝、未知/
|
||||||
|
固定标签拒绝，以及固定基准和已知标签漂移仍拒绝。基准不可见标签只能进入草稿，不能
|
||||||
|
正式发布。首轮 5 passed，测试夹具共享列表导致 1 failed；修正夹具后，对失败项、CLI、
|
||||||
|
中断预算及受影响草稿契约运行 19 passed（2.74 秒）。没有恢复已删除的历史测试。
|
||||||
|
随后按 CAD 拓扑隔离整条受影响手指，验证旧观测不混用、已开始扫描禁止扩大排除范围，
|
||||||
|
受影响 6 passed（1.14 秒）。
|
||||||
|
|
||||||
|
2026-09-23 中指侧摆 0–80 短段：按已声明段长度缩放反馈分箱密度，保留图像/指令覆盖、
|
||||||
|
实际运动分辨率、4 秒推进及其他硬件保护。相关短段、堵转/反向/噪声、硬件保护、离线
|
||||||
|
生命周期与范围证据检查 14 passed（3.02 秒）。只读复核 `20260923_125329` 的两个
|
||||||
|
第二次完整尝试均通过，原始失败记录未改写，结果保存为 `short_segment_reassessment.json`。
|
||||||
|
## 离线零位方向回归
|
||||||
|
|
||||||
|
`test_raw_draft.py` 中的 `parallel_root_motion` 用同一批不变角点核对平行根轴方向，
|
||||||
|
四指 SDK/URDF 符号反转时必须拒绝，不能通过关节零位吸收错误。
|
||||||
|
`old_capture_model_correction` 验证离线绑定/方向修订仍保留原始采集身份，并拒绝速度变更。
|
||||||
|
方向用例来自首轮实采角点节选 `fixtures/o30_recorded_root_directions.json.gz`，
|
||||||
|
保留来源 SHA256，不含验证轮。其余数值回归仍使用合成数据；不发送硬件指令。
|
||||||
|
|
||||||
|
2026-09-23 拇指独立原始采集：局部范围编译保留 `raw_joint_2_plus_1`，清除被保留关节的
|
||||||
|
零位测量声明；训练策略允许经过范围契约验证的 O30 局部配置。复用原执行器和补采预算，
|
||||||
|
未修改拟合模型或运动保护。新增范围往返序列化、24 单元原路线/分区一致、立即补一次与归位后
|
||||||
|
结束测试，连同全手原调度两项回归共 4 passed（1.03 秒）。
|
||||||
|
实机会话 `thumb_recapture_20260923_175829/O30_RIGHT_001/20260923_180129` 在启动归位阶段
|
||||||
|
MCP 指令 0/反馈 120，两次无推进后停止；正式扫描 0/24，不能记为实机采集通过。
|
||||||
|
标定控制链已关闭，原全手数据未修改，等待现场机械接触观察。
|
||||||
|
|
||||||
|
2026-09-23 新外参拇指与旧四指离线组合：修复独立验证总判定的 NumPy bool_ JSON
|
||||||
|
序列化失败；新增冻结旧参考参数的图像拟合更新与显式局部验证范围。参数按语义转移，
|
||||||
|
不共享新旧相机外参，不改变 CAD 轴，无观测增量冻结，零位存在自由规范时拒绝。
|
||||||
|
`test_raw_joint_update.py test_raw_draft.py` 共 37 passed（20.74 秒）；随后加入零位
|
||||||
|
可观测性保护，更新工具相关 3 passed（0.19 秒）。全手默认验证仍拒绝缺失关节，
|
||||||
|
局部报告不得视为全手验收。未恢复删除的历史测试。
|
||||||
|
实采组合草稿通过 JSON 重建、最终 FK（7.36e-16）、标准加载与 MuJoCo 编译。
|
||||||
|
曲线模型角度检查通过不等于 URDF 线性驱动通过：纯 URDF yaw 最大误差约 3.52°，
|
||||||
|
精度未认证，不更新 latest_passed。详见
|
||||||
|
`calibration_output/thumb_recapture_20260923_175829/offline_combined_new_thumb_draft/README.md`。
|
||||||
@@ -1,7 +1,7 @@
|
|||||||
schema_version: 3
|
schema_version: 3
|
||||||
profile_id: G20/right/g20_right_19/v1
|
profile_id: G20/right/g20_right_19/v1
|
||||||
profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml
|
profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml
|
||||||
profile_config_sha256: 35cad4432a01b3e1af157f1e914c2054e71522a44c6786db2519e4c2a0966a46
|
profile_config_sha256: 958f74c476730636d2dadb58a30296a7cdf0b25ad00345252754f9c1b6b50c7f
|
||||||
model: G20
|
model: G20
|
||||||
side: right
|
side: right
|
||||||
tag_layout: g20_right_19
|
tag_layout: g20_right_19
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
schema_version: 3
|
schema_version: 3
|
||||||
profile_id: L6/right/l6_right_8/v1
|
profile_id: L6/right/l6_right_8/v1
|
||||||
profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml
|
profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml
|
||||||
profile_config_sha256: b9654ce998b56c10d7094364e8b715663163d2101e1f70e077cb0723fc47f02e
|
profile_config_sha256: d241e87ded3a29d416a876b27f1a251f8daf2f98e3676ab277264f1bdfaf5c60
|
||||||
model: L6
|
model: L6
|
||||||
side: right
|
side: right
|
||||||
tag_layout: l6_right_8
|
tag_layout: l6_right_8
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
schema_version: 3
|
schema_version: 3
|
||||||
profile_id: O12/right/o12_right_16/v1
|
profile_id: O12/right/o12_right_16/v1
|
||||||
profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml
|
profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml
|
||||||
profile_config_sha256: 0b5cd27168bc863e5c1aed5382c08e33ab4c02a23359a325dc9925fd30599e5f
|
profile_config_sha256: 652c2a82dfc7532b4f62c42cebdad57b169435cf950c5f2ee910cc67c7972621
|
||||||
model: O12
|
model: O12
|
||||||
side: right
|
side: right
|
||||||
tag_layout: o12_right_16
|
tag_layout: o12_right_16
|
||||||
@@ -15,7 +15,7 @@ sdk:
|
|||||||
package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170
|
package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170
|
||||||
transport: hcan
|
transport: hcan
|
||||||
setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
|
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
|
config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c
|
||||||
|
|
||||||
cameras:
|
cameras:
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -0,0 +1,57 @@
|
|||||||
|
/o30_calibration/front/apriltag/apriltag:
|
||||||
|
ros__parameters:
|
||||||
|
image_transport: raw
|
||||||
|
qos_profile: sensor_data
|
||||||
|
family: 36h11
|
||||||
|
size: 0.016
|
||||||
|
max_hamming: 0
|
||||||
|
detector:
|
||||||
|
threads: 4
|
||||||
|
decimate: 1.0
|
||||||
|
blur: 0.0
|
||||||
|
refine: true
|
||||||
|
sharpening: 0.25
|
||||||
|
debug: false
|
||||||
|
pose_estimation_method: pnp
|
||||||
|
tag:
|
||||||
|
ids: [0, 1, 2, 12, 13, 14, 15]
|
||||||
|
frames: [front_base, thumb_mcp, thumb_ip, pinky_mcp_roll, ring_mcp_roll, middle_mcp_roll, index_mcp_roll]
|
||||||
|
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||||
|
/o30_calibration/side/apriltag/apriltag:
|
||||||
|
ros__parameters:
|
||||||
|
image_transport: raw
|
||||||
|
qos_profile: sensor_data
|
||||||
|
family: 36h11
|
||||||
|
size: 0.016
|
||||||
|
max_hamming: 0
|
||||||
|
detector:
|
||||||
|
threads: 4
|
||||||
|
decimate: 1.0
|
||||||
|
blur: 0.0
|
||||||
|
refine: true
|
||||||
|
sharpening: 0.25
|
||||||
|
debug: false
|
||||||
|
pose_estimation_method: pnp
|
||||||
|
tag:
|
||||||
|
ids: [3, 4, 5, 6, 7, 8, 9, 10, 11]
|
||||||
|
frames: [side_base, pinky_pip, pinky_dip, ring_pip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
|
||||||
|
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||||
|
/o30_calibration/top/apriltag/apriltag:
|
||||||
|
ros__parameters:
|
||||||
|
image_transport: raw
|
||||||
|
qos_profile: sensor_data
|
||||||
|
family: 36h11
|
||||||
|
size: 0.016
|
||||||
|
max_hamming: 0
|
||||||
|
detector:
|
||||||
|
threads: 4
|
||||||
|
decimate: 1.0
|
||||||
|
blur: 0.0
|
||||||
|
refine: true
|
||||||
|
sharpening: 0.25
|
||||||
|
debug: false
|
||||||
|
pose_estimation_method: pnp
|
||||||
|
tag:
|
||||||
|
ids: [16, 17]
|
||||||
|
frames: [top_base, thumb_cmc_yaw]
|
||||||
|
sizes: [0.016, 0.016]
|
||||||
@@ -0,0 +1,40 @@
|
|||||||
|
schema_version: 3
|
||||||
|
profile_id: O30/right/o30_right_18/v1
|
||||||
|
profile_config: package://linkerhand_calibration/config/profiles/o30_right_18.yaml
|
||||||
|
profile_config_sha256: 0de1efee3ca95a52d04f289f4a118d2119b81e4a84e247f56f2176ea64130b39
|
||||||
|
model: O30
|
||||||
|
side: right
|
||||||
|
tag_layout: o30_right_18
|
||||||
|
namespace: /o30_calibration
|
||||||
|
serial_number: O30_RIGHT_001
|
||||||
|
can_interface: can0
|
||||||
|
output_root: calibration_output
|
||||||
|
resume_mode: verify
|
||||||
|
cameras:
|
||||||
|
front:
|
||||||
|
serial_number: DB2163742
|
||||||
|
camera_name: hikrobot_front_DB2163742
|
||||||
|
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||||
|
side:
|
||||||
|
serial_number: DB2163749
|
||||||
|
camera_name: hikrobot_side_DB2163749
|
||||||
|
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||||
|
top:
|
||||||
|
serial_number: DB2163739
|
||||||
|
camera_name: hikrobot_top_DB2163739
|
||||||
|
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||||
|
artifacts:
|
||||||
|
source_urdf: package://linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf
|
||||||
|
source_urdf_sha256: 2be8428498ed8c17d7c39dee50e76ece6d362310cf26cd43834b8f4c79021b5b
|
||||||
|
camera_extrinsics: config/o30_three_camera_extrinsics_20260923_181333.yaml
|
||||||
|
camera_extrinsics_sha256: e65a09b31bf10aed2fe0926585d446d209929fe7fc84f55f641b0b08a9ef41aa
|
||||||
|
calibration_config: package://linkerhand_calibration/config/o30_three_camera_calibration.yaml
|
||||||
|
calibration_config_sha256: aeb7826ac00e7ab4c32b131319b34bc11a849c668f268eec2bb4fa12fb081970
|
||||||
|
tag_config: package://linkerhand_calibration/config/o30_right_18_tags.yaml
|
||||||
|
tag_config_sha256: baabc4269883dc1fa124f37a95b812734f04456a31f8d8ce52cf2a541df2cdfe
|
||||||
|
release:
|
||||||
|
required_independent_passes: 1
|
||||||
|
static_repeatability_deg: 1.0
|
||||||
|
sdk:
|
||||||
|
driver: linker_hand_o30_ros2_sdk
|
||||||
|
transport: libcanbus
|
||||||
@@ -0,0 +1,58 @@
|
|||||||
|
o30_calibration:
|
||||||
|
ros__parameters:
|
||||||
|
command_topic: /cb_right_hand_control_cmd
|
||||||
|
state_topic: /cb_right_hand_state
|
||||||
|
setting_topic: /cb_right_hand_setting_cmd
|
||||||
|
front_camera_info_topic: /o30_calibration/front/camera/camera_info
|
||||||
|
front_detections_topic: /o30_calibration/front/apriltag/detections
|
||||||
|
side_camera_info_topic: /o30_calibration/side/camera/camera_info
|
||||||
|
side_detections_topic: /o30_calibration/side/apriltag/detections
|
||||||
|
top_camera_info_topic: /o30_calibration/top/camera/camera_info
|
||||||
|
top_detections_topic: /o30_calibration/top/apriltag/detections
|
||||||
|
baseline_command_u8: [0, 0, 255, 196, 127, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
|
||||||
|
baseline_speed_u8: 200
|
||||||
|
preflight_speed_u8: 200
|
||||||
|
formal_speed_u8: 200
|
||||||
|
speed_settle_seconds: 0.2
|
||||||
|
command_trajectory_full_range_seconds: 20.02765316663493
|
||||||
|
torque_u8: 200
|
||||||
|
repetitions: 4
|
||||||
|
preflight_checkpoints_u8: [0, 127, 255]
|
||||||
|
tag_size_m: 0.016
|
||||||
|
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17]
|
||||||
|
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016,
|
||||||
|
0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||||
|
minimum_detection_rate: 0.95
|
||||||
|
minimum_joint_frame_rate: 0.85
|
||||||
|
minimum_feedback_hz: 25.0
|
||||||
|
maximum_state_image_skew_ms: 50.0
|
||||||
|
maximum_hamming: 0
|
||||||
|
minimum_decision_margin: 30.0
|
||||||
|
minimum_edge_pixels: 30.0
|
||||||
|
pnp_maximum_reprojection_error_px: 1.5
|
||||||
|
pnp_maximum_pose_jump_deg: 35.0
|
||||||
|
pnp_maximum_translation_jump_m: 0.04
|
||||||
|
pnp_maximum_tag_tilt_deg: 75.0
|
||||||
|
pnp_tracker_reset_seconds: 5.0
|
||||||
|
minimum_sweep_frames: 40
|
||||||
|
minimum_state_span_u8: 240.0
|
||||||
|
minimum_sweep_bins: 32
|
||||||
|
maximum_bin_gap: 16
|
||||||
|
maximum_monotonic_correction_deg: 2.0
|
||||||
|
passive_maximum_monotonic_correction_deg: 3.0
|
||||||
|
maximum_validation_mae_deg: 1.0
|
||||||
|
maximum_validation_p95_deg: 2.0
|
||||||
|
maximum_validation_error_deg: 3.0
|
||||||
|
mimic_minimum_multiplier: 0.5
|
||||||
|
mimic_maximum_multiplier: 2.2
|
||||||
|
mimic_maximum_cycle_range: 0.03
|
||||||
|
mimic_maximum_residual_p95_deg: 2.0
|
||||||
|
endpoint_tolerance_u8: 2.0
|
||||||
|
endpoint_hold_seconds: 1.0
|
||||||
|
motor_stall_timeout_seconds: 4.0
|
||||||
|
position_timeout_seconds: 60.0
|
||||||
|
sweep_timeout_seconds: 90.0
|
||||||
|
automatic_sweep_retry_limit: 1
|
||||||
|
non_target_motion_tolerance_u8: 3.0
|
||||||
|
fixed_base_maximum_corner_drift_px: 5.0
|
||||||
|
fixed_base_movement_confirmation_frames: 10
|
||||||
@@ -3,7 +3,7 @@
|
|||||||
image_transport: raw
|
image_transport: raw
|
||||||
qos_profile: sensor_data
|
qos_profile: sensor_data
|
||||||
family: 36h11
|
family: 36h11
|
||||||
size: 0.016
|
size: 0.0165
|
||||||
max_hamming: 0
|
max_hamming: 0
|
||||||
detector:
|
detector:
|
||||||
threads: 4
|
threads: 4
|
||||||
@@ -16,14 +16,14 @@
|
|||||||
tag:
|
tag:
|
||||||
ids: [0, 1, 2]
|
ids: [0, 1, 2]
|
||||||
frames: [front_base, thumb_pitch, thumb_ip]
|
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:
|
/o6_calibration/side/apriltag/apriltag:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
image_transport: raw
|
image_transport: raw
|
||||||
qos_profile: sensor_data
|
qos_profile: sensor_data
|
||||||
family: 36h11
|
family: 36h11
|
||||||
size: 0.016
|
size: 0.0165
|
||||||
max_hamming: 0
|
max_hamming: 0
|
||||||
detector:
|
detector:
|
||||||
threads: 4
|
threads: 4
|
||||||
@@ -36,14 +36,14 @@
|
|||||||
tag:
|
tag:
|
||||||
ids: [3, 4, 5]
|
ids: [3, 4, 5]
|
||||||
frames: [side_base, pinky_pitch, pinky_dip]
|
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:
|
/o6_calibration/top/apriltag/apriltag:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
image_transport: raw
|
image_transport: raw
|
||||||
qos_profile: sensor_data
|
qos_profile: sensor_data
|
||||||
family: 36h11
|
family: 36h11
|
||||||
size: 0.016
|
size: 0.0165
|
||||||
max_hamming: 0
|
max_hamming: 0
|
||||||
detector:
|
detector:
|
||||||
threads: 4
|
threads: 4
|
||||||
@@ -56,4 +56,4 @@
|
|||||||
tag:
|
tag:
|
||||||
ids: [6, 7]
|
ids: [6, 7]
|
||||||
frames: [top_base, thumb_yaw]
|
frames: [top_base, thumb_yaw]
|
||||||
sizes: [0.016, 0.016]
|
sizes: [0.0165, 0.0165]
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
schema_version: 3
|
schema_version: 3
|
||||||
profile_id: O6/right/o6_right_8/v1
|
profile_id: O6/right/o6_right_8/v1
|
||||||
profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml
|
profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml
|
||||||
profile_config_sha256: ab1d86cb0c3e514de5322a44c1444e99d9b1f1ce43f8a2451cd032ed9c31e4ff
|
profile_config_sha256: 72bf56b13eb2a0d4d14a5be42838b2ffc176722f470fe1a3ae5fd84604e1df4d
|
||||||
model: O6
|
model: O6
|
||||||
side: right
|
side: right
|
||||||
tag_layout: o6_right_8
|
tag_layout: o6_right_8
|
||||||
@@ -14,25 +14,25 @@ cameras:
|
|||||||
front:
|
front:
|
||||||
serial_number: DB2163742
|
serial_number: DB2163742
|
||||||
camera_name: hikrobot_front_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:
|
side:
|
||||||
serial_number: DB2163749
|
serial_number: DB2163749
|
||||||
camera_name: hikrobot_side_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:
|
top:
|
||||||
serial_number: DB2163739
|
serial_number: DB2163739
|
||||||
camera_name: hikrobot_top_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:
|
artifacts:
|
||||||
source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf
|
source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf
|
||||||
source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
|
source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
|
||||||
camera_extrinsics: config/o6_three_camera_extrinsics.yaml
|
camera_extrinsics: config/o6_three_camera_extrinsics_20260915_192517.yaml
|
||||||
camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
|
camera_extrinsics_sha256: d057183eaba592149ec4c63c4ceab8a70e1a0c9d7cf2460a1c8b7e86a6ca6638
|
||||||
calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
|
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: package://linkerhand_calibration/config/o6_right_8_tags.yaml
|
||||||
tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
|
tag_config_sha256: 219b8aa906fc9e2a13f3bbede46f542ff176ef2c7f60190f0dd2508993f04593
|
||||||
|
|
||||||
release:
|
release:
|
||||||
required_independent_passes: 1
|
required_independent_passes: 1
|
||||||
|
|||||||
@@ -21,9 +21,9 @@ o6_calibration:
|
|||||||
torque_u8: 80
|
torque_u8: 80
|
||||||
repetitions: 4
|
repetitions: 4
|
||||||
preflight_checkpoints_u8: [255, 127, 0]
|
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_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_detection_rate: 0.95
|
||||||
minimum_joint_frame_rate: 0.85
|
minimum_joint_frame_rate: 0.85
|
||||||
|
|||||||
@@ -927,7 +927,114 @@ motion:
|
|||||||
- 5
|
- 5
|
||||||
- 10
|
- 10
|
||||||
- 15
|
- 15
|
||||||
|
joint_zero_references:
|
||||||
|
thumb_cmc_pitch:
|
||||||
|
task_key: thumb_cmc_pitch_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [0, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
thumb_cmc_roll:
|
||||||
|
task_key: thumb_cmc_roll_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 0, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
thumb_mcp:
|
||||||
|
task_key: thumb_mcp_ip_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 0, 255, 255, 255, 255]
|
||||||
|
thumb_ip:
|
||||||
|
task_key: thumb_mcp_ip_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 0, 255, 255, 255, 255]
|
||||||
|
thumb_cmc_yaw:
|
||||||
|
task_key: thumb_cmc_yaw_top
|
||||||
|
command: [255, 255, 255, 255, 255, 145, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 145, 127, 127, 127, 127, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
pinky_mcp_roll:
|
||||||
|
task_key: pinky_roll_multiview
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
pinky_mcp_pitch:
|
||||||
|
task_key: pinky_pitch_side
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 0, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
pinky_pip:
|
||||||
|
task_key: pinky_pip_side
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
pinky_dip:
|
||||||
|
task_key: pinky_pip_side
|
||||||
|
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
ring_mcp_roll:
|
||||||
|
task_key: ring_roll_multiview
|
||||||
|
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 0, 255, 255, 255, 0, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
ring_mcp_pitch:
|
||||||
|
task_key: ring_pitch_side
|
||||||
|
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 0, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
ring_pip:
|
||||||
|
task_key: ring_pip_side
|
||||||
|
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
ring_dip:
|
||||||
|
task_key: ring_pip_side
|
||||||
|
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
middle_mcp_roll:
|
||||||
|
task_key: middle_roll_multiview
|
||||||
|
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 0, 0, 255, 255, 0, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
middle_mcp_pitch:
|
||||||
|
task_key: middle_pitch_side
|
||||||
|
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 0, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
middle_pip:
|
||||||
|
task_key: middle_pip_side
|
||||||
|
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
middle_dip:
|
||||||
|
task_key: middle_pip_side
|
||||||
|
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
index_mcp_roll:
|
||||||
|
task_key: index_roll_multiview
|
||||||
|
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 0, 0, 0, 255, 0, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
index_mcp_pitch:
|
||||||
|
task_key: index_pitch_side
|
||||||
|
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 0, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
index_pip:
|
||||||
|
task_key: index_pip_side
|
||||||
|
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0]
|
||||||
|
index_dip:
|
||||||
|
task_key: index_pip_side
|
||||||
|
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0]
|
||||||
measurement:
|
measurement:
|
||||||
|
transferred_motion_sources: {}
|
||||||
measurements:
|
measurements:
|
||||||
thumb_cmc_pitch:
|
thumb_cmc_pitch:
|
||||||
joint: thumb_cmc_pitch
|
joint: thumb_cmc_pitch
|
||||||
@@ -1152,6 +1259,17 @@ measurement:
|
|||||||
stable_cross_view_cone_bias: true
|
stable_cross_view_cone_bias: true
|
||||||
input_domain: feedback_u8
|
input_domain: feedback_u8
|
||||||
zero:
|
zero:
|
||||||
|
cad_zero_assumptions:
|
||||||
|
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
|
||||||
|
measured.
|
||||||
|
pinky_dip: User accepts original CAD joint zero as the physical baseline; not independently
|
||||||
|
measured.
|
||||||
|
ring_dip: User accepts original CAD joint zero as the physical baseline; not independently
|
||||||
|
measured.
|
||||||
|
thumb_ip: User accepts original CAD joint zero as the physical baseline; not independently
|
||||||
|
measured.
|
||||||
active_joints:
|
active_joints:
|
||||||
- index_mcp_pitch
|
- index_mcp_pitch
|
||||||
- index_mcp_roll
|
- index_mcp_roll
|
||||||
@@ -1364,7 +1482,7 @@ scope:
|
|||||||
- thumb_mcp
|
- thumb_mcp
|
||||||
default_scope: full
|
default_scope: full
|
||||||
artifacts:
|
artifacts:
|
||||||
output_schema_version: 2
|
output_schema_version: 3
|
||||||
calibration_filename: g20_{side}_{serial_number}_calibration.json
|
calibration_filename: g20_{side}_{serial_number}_calibration.json
|
||||||
corrected_urdf_filename: linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf
|
corrected_urdf_filename: linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf
|
||||||
protected_input_fields:
|
protected_input_fields:
|
||||||
@@ -1385,7 +1503,9 @@ artifacts:
|
|||||||
- urdf_zero_publication
|
- urdf_zero_publication
|
||||||
publish_corrected_urdf: true
|
publish_corrected_urdf: true
|
||||||
acquisition:
|
acquisition:
|
||||||
policy_version: unified_engine_v4_dual_mapping
|
command_capture_mode: separate
|
||||||
|
joint_zero_timeout_seconds: 5.0
|
||||||
|
policy_version: unified_engine_v8_all_view_images
|
||||||
mapping_probe_maximum_rad: 0.0
|
mapping_probe_maximum_rad: 0.0
|
||||||
automatic_rescan_limit: 1
|
automatic_rescan_limit: 1
|
||||||
minimum_valid_samples: 40
|
minimum_valid_samples: 40
|
||||||
|
|||||||
@@ -155,7 +155,40 @@ motion:
|
|||||||
stall_timeout_seconds: 2.0
|
stall_timeout_seconds: 2.0
|
||||||
precheck_sweeps: false
|
precheck_sweeps: false
|
||||||
steady_command_checkpoints: false
|
steady_command_checkpoints: false
|
||||||
|
joint_zero_references:
|
||||||
|
rh_thumb_cmc_roll:
|
||||||
|
task_key: thumb_roll_top
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 0, 255, 255, 255, 255]
|
||||||
|
rh_thumb_cmc_pitch:
|
||||||
|
task_key: thumb_pitch_dip_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [0, 255, 255, 255, 255, 255]
|
||||||
|
rh_thumb_dip:
|
||||||
|
task_key: thumb_pitch_dip_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [0, 255, 255, 255, 255, 255]
|
||||||
|
rh_pinky_mcp_pitch:
|
||||||
|
task_key: pinky_pitch_dip_side
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 0]
|
||||||
|
rh_pinky_dip:
|
||||||
|
task_key: pinky_pitch_dip_side
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 0]
|
||||||
measurement:
|
measurement:
|
||||||
|
transferred_motion_sources:
|
||||||
|
rh_index_mcp_pitch: rh_pinky_mcp_pitch
|
||||||
|
rh_index_dip: rh_pinky_dip
|
||||||
|
rh_middle_mcp_pitch: rh_pinky_mcp_pitch
|
||||||
|
rh_middle_dip: rh_pinky_dip
|
||||||
|
rh_ring_mcp_pitch: rh_pinky_mcp_pitch
|
||||||
|
rh_ring_dip: rh_pinky_dip
|
||||||
measurements:
|
measurements:
|
||||||
rh_thumb_cmc_roll:
|
rh_thumb_cmc_roll:
|
||||||
joint: rh_thumb_cmc_roll
|
joint: rh_thumb_cmc_roll
|
||||||
@@ -203,6 +236,17 @@ measurement:
|
|||||||
cross_view_roll_curve: false
|
cross_view_roll_curve: false
|
||||||
stable_cross_view_cone_bias: false
|
stable_cross_view_cone_bias: false
|
||||||
zero:
|
zero:
|
||||||
|
cad_zero_assumptions:
|
||||||
|
rh_index_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_middle_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_pinky_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_ring_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_thumb_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
active_joints:
|
active_joints:
|
||||||
- rh_index_mcp_pitch
|
- rh_index_mcp_pitch
|
||||||
- rh_middle_mcp_pitch
|
- rh_middle_mcp_pitch
|
||||||
@@ -288,7 +332,7 @@ scope:
|
|||||||
- rh_ring_mcp_pitch
|
- rh_ring_mcp_pitch
|
||||||
default_scope: partial
|
default_scope: partial
|
||||||
artifacts:
|
artifacts:
|
||||||
output_schema_version: 2
|
output_schema_version: 3
|
||||||
calibration_filename: l6_right_{serial_number}_partial_calibration.json
|
calibration_filename: l6_right_{serial_number}_partial_calibration.json
|
||||||
corrected_urdf_filename: linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf
|
corrected_urdf_filename: linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf
|
||||||
protected_input_fields:
|
protected_input_fields:
|
||||||
@@ -303,7 +347,9 @@ artifacts:
|
|||||||
- l6_partial_v1
|
- l6_partial_v1
|
||||||
publish_corrected_urdf: true
|
publish_corrected_urdf: true
|
||||||
acquisition:
|
acquisition:
|
||||||
policy_version: unified_engine_v4_dual_mapping
|
command_capture_mode: separate
|
||||||
|
joint_zero_timeout_seconds: 5.0
|
||||||
|
policy_version: unified_engine_v8_all_view_images
|
||||||
mapping_probe_maximum_rad: 0.0
|
mapping_probe_maximum_rad: 0.0
|
||||||
automatic_rescan_limit: 1
|
automatic_rescan_limit: 1
|
||||||
minimum_valid_samples: 40
|
minimum_valid_samples: 40
|
||||||
|
|||||||
@@ -563,7 +563,92 @@ motion:
|
|||||||
- 3
|
- 3
|
||||||
- 5
|
- 5
|
||||||
- 6
|
- 6
|
||||||
|
joint_zero_references:
|
||||||
|
thumb_cmc_pitch:
|
||||||
|
task_key: thumb_pitch_front
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, -0.8272860654453121, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
thumb_cmc_roll:
|
||||||
|
task_key: thumb_roll_front
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.9424777960769379, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
thumb_mcp:
|
||||||
|
task_key: thumb_mcp_dip_front
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, -1.2915436464758039, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
thumb_dip:
|
||||||
|
task_key: thumb_mcp_dip_front
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, -1.2915436464758039, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
thumb_cmc_yaw:
|
||||||
|
task_key: thumb_yaw_top
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, -1.387536755335492, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
pinky_mcp_pitch:
|
||||||
|
task_key: pinky_chain_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501]
|
||||||
|
pinky_pip:
|
||||||
|
task_key: pinky_chain_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501]
|
||||||
|
pinky_dip:
|
||||||
|
task_key: pinky_chain_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501]
|
||||||
|
middle_mcp_roll:
|
||||||
|
task_key: middle_roll_front
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.17, 0.0, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, -0.2617993877991494, 0.17, 0.0, 1.53588974175501, 1.53588974175501]
|
||||||
|
middle_mcp_pitch:
|
||||||
|
task_key: middle_mcp_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 1.3578661580515883, 0.0, 1.53588974175501, 1.53588974175501]
|
||||||
|
middle_pip:
|
||||||
|
task_key: middle_pip_dip_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
middle_dip:
|
||||||
|
task_key: middle_pip_dip_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
index_mcp_roll:
|
||||||
|
task_key: index_roll_front
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.17, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, -0.2617993877991494, 0.17, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
index_mcp_pitch:
|
||||||
|
task_key: index_mcp_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, 0.0, 1.3526301702956054, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
index_pip:
|
||||||
|
task_key: index_pip_dip_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.530653753999027, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
index_dip:
|
||||||
|
task_key: index_pip_dip_side
|
||||||
|
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
|
approach_commands:
|
||||||
|
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.530653753999027, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
|
||||||
measurement:
|
measurement:
|
||||||
|
transferred_motion_sources:
|
||||||
|
ring_mcp_pitch: pinky_mcp_pitch
|
||||||
|
ring_pip: pinky_pip
|
||||||
|
ring_dip: pinky_dip
|
||||||
candidate_selection_tasks:
|
candidate_selection_tasks:
|
||||||
- thumb_mcp_dip_front
|
- thumb_mcp_dip_front
|
||||||
directional_zero: true
|
directional_zero: true
|
||||||
@@ -658,6 +743,20 @@ measurement:
|
|||||||
child_role: index_dip
|
child_role: index_dip
|
||||||
pose_axis_line_required: false
|
pose_axis_line_required: false
|
||||||
zero:
|
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
|
||||||
|
measured.
|
||||||
|
pinky_dip: User accepts original CAD joint zero as the physical baseline; not independently
|
||||||
|
measured.
|
||||||
|
ring_dip: User accepts original CAD joint zero as the physical baseline; not independently
|
||||||
|
measured.
|
||||||
|
thumb_dip: User accepts original CAD joint zero as the physical baseline; not independently
|
||||||
|
measured.
|
||||||
active_joints:
|
active_joints:
|
||||||
- thumb_cmc_roll
|
- thumb_cmc_roll
|
||||||
- thumb_cmc_yaw
|
- thumb_cmc_yaw
|
||||||
@@ -748,7 +847,6 @@ zero:
|
|||||||
- pinky_mcp_pitch
|
- pinky_mcp_pitch
|
||||||
depth_free_axis_projection: true
|
depth_free_axis_projection: true
|
||||||
accept_validated_zero_in_confidence_interval: true
|
accept_validated_zero_in_confidence_interval: true
|
||||||
project_axis_gauge_before_image: true
|
|
||||||
axis_order:
|
axis_order:
|
||||||
- thumb_cmc_roll
|
- thumb_cmc_roll
|
||||||
- thumb_cmc_yaw
|
- thumb_cmc_yaw
|
||||||
@@ -815,7 +913,7 @@ scope:
|
|||||||
frozen_joints:
|
frozen_joints:
|
||||||
full: []
|
full: []
|
||||||
artifacts:
|
artifacts:
|
||||||
output_schema_version: 2
|
output_schema_version: 3
|
||||||
calibration_filename: o12_right_{serial_number}_calibration.json
|
calibration_filename: o12_right_{serial_number}_calibration.json
|
||||||
corrected_urdf_filename: linkerhand_o12_right_{serial_number}_zero_calibrated.urdf
|
corrected_urdf_filename: linkerhand_o12_right_{serial_number}_zero_calibrated.urdf
|
||||||
protected_input_fields:
|
protected_input_fields:
|
||||||
@@ -833,7 +931,9 @@ artifacts:
|
|||||||
- full_sdk_range_v2
|
- full_sdk_range_v2
|
||||||
publish_corrected_urdf: true
|
publish_corrected_urdf: true
|
||||||
acquisition:
|
acquisition:
|
||||||
policy_version: unified_engine_v4_dual_mapping
|
command_capture_mode: separate
|
||||||
|
joint_zero_timeout_seconds: 5.0
|
||||||
|
policy_version: unified_engine_v8_all_view_images
|
||||||
mapping_probe_maximum_rad: 0.05235987755982989
|
mapping_probe_maximum_rad: 0.05235987755982989
|
||||||
urdf:
|
urdf:
|
||||||
authorized_fields:
|
authorized_fields:
|
||||||
@@ -901,8 +1001,10 @@ urdf:
|
|||||||
- mimic.multiplier
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
ring_pip:
|
ring_pip:
|
||||||
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
ring_dip:
|
ring_dip:
|
||||||
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
joint_coverage:
|
joint_coverage:
|
||||||
thumb_cmc_roll: measured_static_dynamic
|
thumb_cmc_roll: measured_static_dynamic
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -55,31 +55,40 @@ vision:
|
|||||||
- role: front_base
|
- role: front_base
|
||||||
fixed_reference: true
|
fixed_reference: true
|
||||||
id: 0
|
id: 0
|
||||||
|
size_m: 0.0165
|
||||||
- role: thumb_pitch
|
- role: thumb_pitch
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
id: 1
|
id: 1
|
||||||
|
size_m: 0.0165
|
||||||
- role: thumb_ip
|
- role: thumb_ip
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
id: 2
|
id: 2
|
||||||
|
size_m: 0.0165
|
||||||
- name: side
|
- name: side
|
||||||
tags:
|
tags:
|
||||||
- role: side_base
|
- role: side_base
|
||||||
fixed_reference: true
|
fixed_reference: true
|
||||||
id: 3
|
id: 3
|
||||||
|
size_m: 0.0165
|
||||||
- role: pinky_pitch
|
- role: pinky_pitch
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
id: 4
|
id: 4
|
||||||
|
size_m: 0.0165
|
||||||
- role: pinky_dip
|
- role: pinky_dip
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
id: 5
|
id: 5
|
||||||
|
size_m: 0.0165
|
||||||
- name: top
|
- name: top
|
||||||
tags:
|
tags:
|
||||||
- role: top_base
|
- role: top_base
|
||||||
fixed_reference: true
|
fixed_reference: true
|
||||||
id: 6
|
id: 6
|
||||||
|
size_m: 0.0165
|
||||||
- role: thumb_yaw
|
- role: thumb_yaw
|
||||||
fixed_reference: false
|
fixed_reference: false
|
||||||
id: 7
|
id: 7
|
||||||
|
size_m: 0.0165
|
||||||
|
link: rh_thumb_distal
|
||||||
common_frame: calibration_common
|
common_frame: calibration_common
|
||||||
extrinsic_reference_view: front
|
extrinsic_reference_view: front
|
||||||
extrinsics_quality_limits:
|
extrinsics_quality_limits:
|
||||||
@@ -151,7 +160,40 @@ motion:
|
|||||||
stall_timeout_seconds: 2.0
|
stall_timeout_seconds: 2.0
|
||||||
precheck_sweeps: false
|
precheck_sweeps: false
|
||||||
steady_command_checkpoints: false
|
steady_command_checkpoints: false
|
||||||
|
joint_zero_references:
|
||||||
|
rh_thumb_cmc_yaw:
|
||||||
|
task_key: thumb_yaw_top
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 0, 255, 255, 255, 255]
|
||||||
|
rh_thumb_cmc_pitch:
|
||||||
|
task_key: thumb_pitch_ip_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [0, 255, 255, 255, 255, 255]
|
||||||
|
rh_thumb_ip:
|
||||||
|
task_key: thumb_pitch_ip_front
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [0, 255, 255, 255, 255, 255]
|
||||||
|
rh_pinky_mcp_pitch:
|
||||||
|
task_key: pinky_pitch_dip_side
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 0]
|
||||||
|
rh_pinky_dip:
|
||||||
|
task_key: pinky_pitch_dip_side
|
||||||
|
command: [255, 255, 255, 255, 255, 255]
|
||||||
|
approach_commands:
|
||||||
|
- [255, 255, 255, 255, 255, 0]
|
||||||
measurement:
|
measurement:
|
||||||
|
transferred_motion_sources:
|
||||||
|
rh_index_mcp_pitch: rh_pinky_mcp_pitch
|
||||||
|
rh_index_dip: rh_pinky_dip
|
||||||
|
rh_middle_mcp_pitch: rh_pinky_mcp_pitch
|
||||||
|
rh_middle_dip: rh_pinky_dip
|
||||||
|
rh_ring_mcp_pitch: rh_pinky_mcp_pitch
|
||||||
|
rh_ring_dip: rh_pinky_dip
|
||||||
measurements:
|
measurements:
|
||||||
rh_thumb_cmc_yaw:
|
rh_thumb_cmc_yaw:
|
||||||
joint: rh_thumb_cmc_yaw
|
joint: rh_thumb_cmc_yaw
|
||||||
@@ -199,6 +241,17 @@ measurement:
|
|||||||
cross_view_roll_curve: false
|
cross_view_roll_curve: false
|
||||||
stable_cross_view_cone_bias: false
|
stable_cross_view_cone_bias: false
|
||||||
zero:
|
zero:
|
||||||
|
cad_zero_assumptions:
|
||||||
|
rh_index_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_middle_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_pinky_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_ring_dip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
|
rh_thumb_ip: User accepts original CAD joint zero as the physical baseline; not
|
||||||
|
independently measured.
|
||||||
active_joints:
|
active_joints:
|
||||||
- rh_index_mcp_pitch
|
- rh_index_mcp_pitch
|
||||||
- rh_middle_mcp_pitch
|
- rh_middle_mcp_pitch
|
||||||
@@ -284,7 +337,9 @@ scope:
|
|||||||
- rh_ring_mcp_pitch
|
- rh_ring_mcp_pitch
|
||||||
default_scope: partial
|
default_scope: partial
|
||||||
artifacts:
|
artifacts:
|
||||||
output_schema_version: 2
|
output_schema_version: 3
|
||||||
|
directional_command_joints:
|
||||||
|
- rh_thumb_cmc_yaw
|
||||||
calibration_filename: o6_right_{serial_number}_partial_calibration.json
|
calibration_filename: o6_right_{serial_number}_partial_calibration.json
|
||||||
corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf
|
corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf
|
||||||
protected_input_fields:
|
protected_input_fields:
|
||||||
@@ -299,7 +354,12 @@ artifacts:
|
|||||||
- o6_partial_v1
|
- o6_partial_v1
|
||||||
publish_corrected_urdf: true
|
publish_corrected_urdf: true
|
||||||
acquisition:
|
acquisition:
|
||||||
policy_version: unified_engine_v4_dual_mapping
|
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
|
mapping_probe_maximum_rad: 0.0
|
||||||
automatic_rescan_limit: 1
|
automatic_rescan_limit: 1
|
||||||
minimum_valid_samples: 40
|
minimum_valid_samples: 40
|
||||||
@@ -314,6 +374,40 @@ acquisition:
|
|||||||
fixed_reference_maximum_drift_px: 5.0
|
fixed_reference_maximum_drift_px: 5.0
|
||||||
fixed_reference_confirmation_frames: 10
|
fixed_reference_confirmation_frames: 10
|
||||||
urdf:
|
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:
|
authorized_fields:
|
||||||
rh_pinky_mcp_pitch:
|
rh_pinky_mcp_pitch:
|
||||||
- limit.lower
|
- limit.lower
|
||||||
@@ -342,18 +436,28 @@ urdf:
|
|||||||
rh_ring_dip:
|
rh_ring_dip:
|
||||||
- mimic.multiplier
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
|
- limit.lower
|
||||||
|
- limit.upper
|
||||||
rh_index_dip:
|
rh_index_dip:
|
||||||
- mimic.multiplier
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
|
- limit.lower
|
||||||
|
- limit.upper
|
||||||
rh_pinky_dip:
|
rh_pinky_dip:
|
||||||
- mimic.multiplier
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
|
- limit.lower
|
||||||
|
- limit.upper
|
||||||
rh_thumb_ip:
|
rh_thumb_ip:
|
||||||
- mimic.multiplier
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
|
- limit.lower
|
||||||
|
- limit.upper
|
||||||
rh_middle_dip:
|
rh_middle_dip:
|
||||||
- mimic.multiplier
|
- mimic.multiplier
|
||||||
- mimic.offset
|
- mimic.offset
|
||||||
|
- limit.lower
|
||||||
|
- limit.upper
|
||||||
joint_coverage:
|
joint_coverage:
|
||||||
rh_pinky_mcp_pitch: measured_static_dynamic
|
rh_pinky_mcp_pitch: measured_static_dynamic
|
||||||
rh_thumb_cmc_pitch: measured_static_dynamic
|
rh_thumb_cmc_pitch: measured_static_dynamic
|
||||||
|
|||||||
@@ -29,6 +29,8 @@ def _launch_stack(context):
|
|||||||
)
|
)
|
||||||
from linkerhand_calibration.profiles import load_hand_profile
|
from linkerhand_calibration.profiles import load_hand_profile
|
||||||
from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics
|
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()
|
model = LaunchConfiguration("model").perform(context).strip().upper()
|
||||||
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
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")
|
raise ValueError("launch identity differs from the protected Profile")
|
||||||
except ValueError as error:
|
except ValueError as error:
|
||||||
raise RuntimeError(str(error)) from 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)
|
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||||
if not requested_tag_config:
|
if not requested_tag_config:
|
||||||
raise RuntimeError("tag_config is required; use calibrate_hand --config")
|
raise RuntimeError("tag_config is required; use calibrate_hand --config")
|
||||||
@@ -57,7 +63,7 @@ def _launch_stack(context):
|
|||||||
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
||||||
topic_prefix = f"/{model.lower()}"
|
topic_prefix = f"/{model.lower()}"
|
||||||
uses_hcan = contract.typed_profile.sdk_adapter == "o12_hcan_sdk"
|
uses_hcan = contract.typed_profile.sdk_adapter == "o12_hcan_sdk"
|
||||||
if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk"}:
|
if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk", "o30_ros"}:
|
||||||
raise RuntimeError("SDK adapter has no ROS launch binding")
|
raise RuntimeError("SDK adapter has no ROS launch binding")
|
||||||
topics = sdk_topics(contract.typed_profile)
|
topics = sdk_topics(contract.typed_profile)
|
||||||
command_topic, state_topic = topics.command, topics.feedback
|
command_topic, state_topic = topics.command, topics.feedback
|
||||||
@@ -110,9 +116,9 @@ def _launch_stack(context):
|
|||||||
for view in views
|
for view in views
|
||||||
}
|
}
|
||||||
if any(not serial for serial in camera_serials.values()):
|
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):
|
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 = []
|
cameras = []
|
||||||
components = []
|
components = []
|
||||||
@@ -153,6 +159,8 @@ def _launch_stack(context):
|
|||||||
),
|
),
|
||||||
"image_width": 1624,
|
"image_width": 1624,
|
||||||
"image_height": 1240,
|
"image_height": 1240,
|
||||||
|
"timestamp_journal_path": str(Path(LaunchConfiguration("session_dir").perform(context))
|
||||||
|
/ f"camera_timing_{view}.jsonl"),
|
||||||
"frame_rate": ParameterValue(
|
"frame_rate": ParameterValue(
|
||||||
LaunchConfiguration("camera_frame_rate"),
|
LaunchConfiguration("camera_frame_rate"),
|
||||||
value_type=float,
|
value_type=float,
|
||||||
@@ -194,15 +202,10 @@ def _launch_stack(context):
|
|||||||
plugin="AprilTagNode",
|
plugin="AprilTagNode",
|
||||||
name="apriltag",
|
name="apriltag",
|
||||||
namespace=detector_namespace,
|
namespace=detector_namespace,
|
||||||
parameters=[
|
# Detector settings belong to the protected model YAML.
|
||||||
str(tag_config),
|
# A generic launch default must not silently replace its
|
||||||
{
|
# full-resolution setting for the small calibration Tags.
|
||||||
"detector.decimate": ParameterValue(
|
parameters=[str(tag_config)],
|
||||||
LaunchConfiguration("apriltag_decimate"),
|
|
||||||
value_type=float,
|
|
||||||
)
|
|
||||||
},
|
|
||||||
],
|
|
||||||
remappings=[
|
remappings=[
|
||||||
("image_rect", rect_topic),
|
("image_rect", rect_topic),
|
||||||
("camera_info", info_topic),
|
("camera_info", info_topic),
|
||||||
@@ -271,6 +274,12 @@ def _launch_stack(context):
|
|||||||
}],
|
}],
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
if contract.typed_profile.sdk_adapter == "o30_ros":
|
||||||
|
from linkerhand_calibration.runtime.adapters.o30_ros import launch_parameters
|
||||||
|
sdk = Node(package="linker_hand_o30_ros2_sdk", executable="linker_hand_o30_ros2_sdk",
|
||||||
|
name="linker_hand_o30_sdk", output="screen",
|
||||||
|
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||||
|
parameters=[launch_parameters(side=hand_type)])
|
||||||
calibration = Node(
|
calibration = Node(
|
||||||
package="linkerhand_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="three_camera_calibration_node",
|
executable="three_camera_calibration_node",
|
||||||
@@ -290,8 +299,18 @@ def _launch_stack(context):
|
|||||||
"resume_raw_samples_path": LaunchConfiguration(
|
"resume_raw_samples_path": LaunchConfiguration(
|
||||||
"resume_raw_samples_path"
|
"resume_raw_samples_path"
|
||||||
),
|
),
|
||||||
|
"resume_mode": LaunchConfiguration("resume_mode"),
|
||||||
|
"training_policy": LaunchConfiguration("training_policy"),
|
||||||
|
"preparation_witnesses_json": ParameterValue(
|
||||||
|
LaunchConfiguration("preparation_witnesses_json"), value_type=str),
|
||||||
|
"capture_plan_expected_sha256": ParameterValue(
|
||||||
|
LaunchConfiguration("capture_plan_expected_sha256"), value_type=str),
|
||||||
|
"initial_command_file": LaunchConfiguration("initial_command_file"),
|
||||||
|
"reuse_camera_calibration": ParameterValue(LaunchConfiguration("reuse_camera_calibration"), value_type=bool),
|
||||||
|
"camera_optical_observations_file": LaunchConfiguration("camera_optical_observations_file"),
|
||||||
"command_topic": command_topic,
|
"command_topic": command_topic,
|
||||||
"state_topic": state_topic,
|
"state_topic": state_topic,
|
||||||
|
**({"setting_topic": topics.setting} if topics.setting else {}),
|
||||||
"camera_extrinsics_file": LaunchConfiguration(
|
"camera_extrinsics_file": LaunchConfiguration(
|
||||||
"camera_extrinsics_file"
|
"camera_extrinsics_file"
|
||||||
),
|
),
|
||||||
@@ -318,6 +337,9 @@ def _launch_stack(context):
|
|||||||
"commands_enabled": ParameterValue(
|
"commands_enabled": ParameterValue(
|
||||||
LaunchConfiguration("commands_enabled"), value_type=bool
|
LaunchConfiguration("commands_enabled"), value_type=bool
|
||||||
),
|
),
|
||||||
|
"diagnostic_capture": ParameterValue(
|
||||||
|
LaunchConfiguration("diagnostic_capture"), value_type=str
|
||||||
|
),
|
||||||
},
|
},
|
||||||
],
|
],
|
||||||
)
|
)
|
||||||
@@ -364,6 +386,10 @@ def _launch_stack(context):
|
|||||||
),
|
),
|
||||||
*cameras,
|
*cameras,
|
||||||
vision,
|
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,
|
sdk,
|
||||||
calibration,
|
calibration,
|
||||||
bag,
|
bag,
|
||||||
@@ -400,7 +426,6 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||||
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
|
||||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||||
DeclareLaunchArgument("camera_extrinsics_file", default_value=""),
|
DeclareLaunchArgument("camera_extrinsics_file", default_value=""),
|
||||||
@@ -430,6 +455,7 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
),
|
),
|
||||||
DeclareLaunchArgument("profile_config", default_value=""),
|
DeclareLaunchArgument("profile_config", default_value=""),
|
||||||
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
||||||
|
DeclareLaunchArgument("diagnostic_capture", default_value=""),
|
||||||
DeclareLaunchArgument("start_cameras", default_value="true"),
|
DeclareLaunchArgument("start_cameras", default_value="true"),
|
||||||
DeclareLaunchArgument("start_sdk", default_value="true"),
|
DeclareLaunchArgument("start_sdk", default_value="true"),
|
||||||
DeclareLaunchArgument("record_bag", default_value="false"),
|
DeclareLaunchArgument("record_bag", default_value="false"),
|
||||||
@@ -439,6 +465,13 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
),
|
),
|
||||||
DeclareLaunchArgument("session_dir", default_value=""),
|
DeclareLaunchArgument("session_dir", default_value=""),
|
||||||
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
|
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
|
||||||
|
DeclareLaunchArgument("resume_mode", default_value="verify"),
|
||||||
|
DeclareLaunchArgument("training_policy", default_value="fixed"),
|
||||||
|
DeclareLaunchArgument("preparation_witnesses_json", default_value=""),
|
||||||
|
DeclareLaunchArgument("capture_plan_expected_sha256", default_value=""),
|
||||||
|
DeclareLaunchArgument("initial_command_file", default_value=""),
|
||||||
|
DeclareLaunchArgument("reuse_camera_calibration", default_value="false"),
|
||||||
|
DeclareLaunchArgument("camera_optical_observations_file", default_value=""),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"calibration_config",
|
"calibration_config",
|
||||||
default_value="",
|
default_value="",
|
||||||
|
|||||||
@@ -93,6 +93,7 @@ def _launch(context):
|
|||||||
parameters=[
|
parameters=[
|
||||||
{
|
{
|
||||||
"output_file": LaunchConfiguration("output_file"),
|
"output_file": LaunchConfiguration("output_file"),
|
||||||
|
"verification_extrinsics_file": LaunchConfiguration("verification_extrinsics_file"),
|
||||||
"checkerboard_columns": ParameterValue(
|
"checkerboard_columns": ParameterValue(
|
||||||
LaunchConfiguration("checkerboard_columns"), value_type=int
|
LaunchConfiguration("checkerboard_columns"), value_type=int
|
||||||
),
|
),
|
||||||
@@ -197,6 +198,7 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
default_value="1.5",
|
default_value="1.5",
|
||||||
),
|
),
|
||||||
DeclareLaunchArgument("auto_capture_default", default_value="false"),
|
DeclareLaunchArgument("auto_capture_default", default_value="false"),
|
||||||
|
DeclareLaunchArgument("verification_extrinsics_file", default_value=""),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"auto_capture_stable_seconds", default_value="1.0"
|
"auto_capture_stable_seconds", default_value="1.0"
|
||||||
),
|
),
|
||||||
|
|||||||
@@ -109,6 +109,7 @@ class Observation:
|
|||||||
class StateSample:
|
class StateSample:
|
||||||
stamp_ns: int
|
stamp_ns: int
|
||||||
position_u8: tuple[float, ...]
|
position_u8: tuple[float, ...]
|
||||||
|
channel_stamps_ns: tuple[int, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
def interpolate_state_u8(
|
def interpolate_state_u8(
|
||||||
@@ -127,6 +128,29 @@ def interpolate_state_u8(
|
|||||||
raise ValueError("maximum_skew_ns must be non-negative")
|
raise ValueError("maximum_skew_ns must be non-negative")
|
||||||
if not samples:
|
if not samples:
|
||||||
return None
|
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]
|
stamps = [int(sample.stamp_ns) for sample in samples]
|
||||||
index = bisect_left(stamps, int(stamp_ns))
|
index = bisect_left(stamps, int(stamp_ns))
|
||||||
|
|
||||||
|
|||||||
@@ -276,7 +276,7 @@ def load_calibrated_command_mapper(
|
|||||||
if not path.is_file():
|
if not path.is_file():
|
||||||
raise ValueError(f"calibration JSON does not exist: {path}")
|
raise ValueError(f"calibration JSON does not exist: {path}")
|
||||||
payload = json.loads(path.read_text(encoding="utf-8"))
|
payload = json.loads(path.read_text(encoding="utf-8"))
|
||||||
if path.name == "release_manifest.json" or payload.get("format") in {"unified_calibration_v1", "unified_calibration_v2"}:
|
if path.name == "release_manifest.json" or payload.get("format") in {"unified_calibration_v1", "unified_calibration_v2", "unified_calibration_v3"}:
|
||||||
from .runtime.artifacts.reader import load_unified_mapper
|
from .runtime.artifacts.reader import load_unified_mapper
|
||||||
return load_unified_mapper(path, expected_side=expected_side, input_kind=input_kind)
|
return load_unified_mapper(path, expected_side=expected_side, input_kind=input_kind)
|
||||||
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
||||||
|
|||||||
@@ -0,0 +1,217 @@
|
|||||||
|
"""Map camera frame clocks to host time before pairing images with feedback.
|
||||||
|
|
||||||
|
The MVS frame counter timestamps exposure start. USB delivery/publication is
|
||||||
|
later. Clock latches bracket the device/host correspondence independently of
|
||||||
|
hand motion, images, or fitted calibration curves.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import asdict, dataclass
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
from pathlib import Path
|
||||||
|
import time
|
||||||
|
|
||||||
|
|
||||||
|
CAMERA_TIMING_POLICY = "mvs_latched_exposure_time_v1"
|
||||||
|
CLOCK_DRIFT_TOLERANCE_NS = 1_000_000
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ClockLatch:
|
||||||
|
device_tick: int
|
||||||
|
before_ns: int
|
||||||
|
after_ns: int
|
||||||
|
raw_before_ns: int | None = None
|
||||||
|
raw_after_ns: int | None = None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def midpoint_ns(self):
|
||||||
|
return (self.before_ns+self.after_ns)//2
|
||||||
|
|
||||||
|
@property
|
||||||
|
def half_width_ns(self):
|
||||||
|
return (self.after_ns-self.before_ns)/2
|
||||||
|
|
||||||
|
|
||||||
|
class DeviceFrameClock:
|
||||||
|
"""A nominal device clock with short, measured host-time brackets.
|
||||||
|
|
||||||
|
A new latch is checked against the previous one before it can change the
|
||||||
|
mapping. This rejects wrong counter units, resets and host clock jumps.
|
||||||
|
Periodic updates bound accumulated drift; none depend on image delivery.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, ticks_per_second):
|
||||||
|
if not math.isfinite(ticks_per_second) or ticks_per_second <= 0:
|
||||||
|
raise ValueError("invalid camera clock frequency")
|
||||||
|
self.ticks_per_second = ticks_per_second
|
||||||
|
self.anchor = None
|
||||||
|
self.last_frame_tick = None
|
||||||
|
self.last_stamp_ns = None
|
||||||
|
self.refresh_interval_seconds = .5
|
||||||
|
|
||||||
|
def elapsed_ns(self, ticks):
|
||||||
|
return round(ticks*1_000_000_000/self.ticks_per_second)
|
||||||
|
|
||||||
|
def update(self, latches):
|
||||||
|
if not latches or any(x.device_tick <= 0 or x.before_ns <= 0
|
||||||
|
or not 0 <= x.after_ns-x.before_ns <= 2_000_000 for x in latches):
|
||||||
|
raise ValueError("camera clock latch is invalid or too uncertain")
|
||||||
|
previous = self.anchor or latches[0]
|
||||||
|
if self.anchor is None and latches[-1].before_ns-latches[0].after_ns < 50_000_000:
|
||||||
|
raise ValueError("camera clock units need an independent elapsed-time check")
|
||||||
|
for current in latches:
|
||||||
|
elapsed = self.elapsed_ns(current.device_tick-previous.device_tick)
|
||||||
|
host_elapsed = current.midpoint_ns-previous.midpoint_ns
|
||||||
|
tolerance = current.half_width_ns+previous.half_width_ns+CLOCK_DRIFT_TOLERANCE_NS
|
||||||
|
if elapsed < 0 or abs(elapsed-host_elapsed) > tolerance:
|
||||||
|
raise ValueError("camera/host clock discontinuity or counter unit mismatch")
|
||||||
|
# Refresh before measured phase drift spends the existing 1 ms
|
||||||
|
# continuity allowance. A fixed 0.5 s period repeatedly invalidates a
|
||||||
|
# smooth slewing host clock. Scheduling changes; admission never does.
|
||||||
|
current = max(latches, key=lambda x: x.midpoint_ns)
|
||||||
|
host_elapsed = current.midpoint_ns-previous.midpoint_ns
|
||||||
|
if host_elapsed >= 50_000_000:
|
||||||
|
drift = abs(host_elapsed-self.elapsed_ns(current.device_tick-previous.device_tick))
|
||||||
|
rate = drift/host_elapsed
|
||||||
|
self.refresh_interval_seconds = max(.02, min(.5,
|
||||||
|
.25*CLOCK_DRIFT_TOLERANCE_NS/1e9/max(rate, 1e-12)))
|
||||||
|
self.anchor = min(latches, key=lambda x: x.half_width_ns)
|
||||||
|
|
||||||
|
def timestamp(self, tick, *, received_ns, exposure_us):
|
||||||
|
if (self.anchor is None or not 0 <= received_ns-self.anchor.midpoint_ns <= 2_000_000_000
|
||||||
|
or not math.isfinite(exposure_us) or exposure_us < 0):
|
||||||
|
raise ValueError("camera frame has no fresh clock or valid exposure")
|
||||||
|
if self.last_frame_tick is not None and tick <= self.last_frame_tick:
|
||||||
|
raise ValueError("camera frame counter reset or duplicate frame")
|
||||||
|
stamp = self.anchor.midpoint_ns+self.elapsed_ns(tick-self.anchor.device_tick)+round(exposure_us*500)
|
||||||
|
if stamp > received_ns+2_000_000 or (self.last_stamp_ns is not None and stamp <= self.last_stamp_ns):
|
||||||
|
raise ValueError("camera image time is future or out of order")
|
||||||
|
self.last_frame_tick, self.last_stamp_ns = tick, stamp
|
||||||
|
return stamp
|
||||||
|
|
||||||
|
|
||||||
|
class MvsCameraTiming:
|
||||||
|
"""The camera-specific clock latch and its optional append-only journal."""
|
||||||
|
|
||||||
|
def __init__(self, camera, mvs, clock_ns, *, exposure_us, journal_path="", raw_clock_ns=None):
|
||||||
|
self.camera, self.mvs, self.clock_ns = camera, mvs, clock_ns
|
||||||
|
# CLOCK_MONOTONIC_RAW is independent of wall-clock synchronization.
|
||||||
|
# It is diagnostic evidence only, never a replacement ROS timestamp.
|
||||||
|
self.raw_clock_ns = raw_clock_ns or (lambda: time.clock_gettime_ns(time.CLOCK_MONOTONIC_RAW))
|
||||||
|
self.exposure_us = exposure_us
|
||||||
|
self.frame_clock = None
|
||||||
|
self.last_stamp_ns = None
|
||||||
|
self.next_refresh = 0.
|
||||||
|
self.journal = None
|
||||||
|
if journal_path:
|
||||||
|
path = Path(journal_path)
|
||||||
|
path.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
self.journal = path.open("a", encoding="utf-8")
|
||||||
|
|
||||||
|
def _integer(self, name):
|
||||||
|
value = self.mvs.MVCC_INTVALUE_EX()
|
||||||
|
status = self.camera.MV_CC_GetIntValueEx(name, value)
|
||||||
|
if status != 0:
|
||||||
|
raise ValueError(f"camera clock cannot read {name}:0x{status:08x}")
|
||||||
|
return int(value.nCurValue)
|
||||||
|
|
||||||
|
def _latch(self):
|
||||||
|
raw_before = self.raw_clock_ns()
|
||||||
|
before = self.clock_ns()
|
||||||
|
status = self.camera.MV_CC_SetCommandValue("DeviceTimestampLatch")
|
||||||
|
if status != 0:
|
||||||
|
raise ValueError(f"camera clock latch failed:0x{status:08x}")
|
||||||
|
tick = self._integer("DeviceTimestamp")
|
||||||
|
after = self.clock_ns()
|
||||||
|
return ClockLatch(tick, before, after, raw_before, self.raw_clock_ns())
|
||||||
|
|
||||||
|
def _write(self, row):
|
||||||
|
if self.journal is not None:
|
||||||
|
self.journal.write(json.dumps(row, separators=(",", ":"))+"\n")
|
||||||
|
|
||||||
|
def start(self):
|
||||||
|
# The installed USB camera firmware returns ticks/second for this
|
||||||
|
# feature (despite its XML unit label). Verify that interpretation
|
||||||
|
# against elapsed host time; do not assume it for another firmware.
|
||||||
|
self.frame_clock = DeviceFrameClock(self._integer("DeviceTimestampIncrement"))
|
||||||
|
latches = []
|
||||||
|
for index in range(40):
|
||||||
|
if index:
|
||||||
|
time.sleep(.02)
|
||||||
|
latches.append(self._latch())
|
||||||
|
valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000]
|
||||||
|
if len(valid) >= 8 and valid[-1].before_ns-valid[0].after_ns >= 50_000_000:
|
||||||
|
self._update(latches)
|
||||||
|
return
|
||||||
|
raise ValueError("camera clock has no bounded-latency startup latches")
|
||||||
|
|
||||||
|
def _update(self, latches):
|
||||||
|
valid = [x for x in latches if 0 <= x.after_ns-x.before_ns <= 2_000_000]
|
||||||
|
if not valid:
|
||||||
|
# A delayed USB control transaction provides no new clock evidence.
|
||||||
|
# Keep the original two-second freshness bound while trying again.
|
||||||
|
self._write(dict(kind="camera_clock_latch_rejected", latches=[asdict(x) for x in latches]))
|
||||||
|
self.next_refresh = time.monotonic()+.02
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
self.frame_clock.update(valid)
|
||||||
|
except ValueError as error:
|
||||||
|
# Invalidation alone loses the transaction that failed. Preserve
|
||||||
|
# both clocks and the old anchor so drift, jumps and bad device
|
||||||
|
# counters can be distinguished without weakening the check.
|
||||||
|
self._write(dict(kind="camera_clock_sync_rejected", policy=CAMERA_TIMING_POLICY,
|
||||||
|
reason=str(error), ticks_per_second=self.frame_clock.ticks_per_second,
|
||||||
|
previous_anchor=asdict(self.frame_clock.anchor) if self.frame_clock.anchor else None,
|
||||||
|
latches=[asdict(x) for x in latches]))
|
||||||
|
if self.journal is not None:
|
||||||
|
self.journal.flush()
|
||||||
|
raise
|
||||||
|
self._write(dict(kind="camera_clock_sync", policy=CAMERA_TIMING_POLICY,
|
||||||
|
ticks_per_second=self.frame_clock.ticks_per_second,
|
||||||
|
refresh_interval_seconds=self.frame_clock.refresh_interval_seconds,
|
||||||
|
latches=[asdict(x) for x in latches], anchor=asdict(self.frame_clock.anchor)))
|
||||||
|
if self.journal is not None:
|
||||||
|
self.journal.flush()
|
||||||
|
# Startup may select a precise latch near the beginning of its window.
|
||||||
|
# That anchor has already spent part of the refresh budget.
|
||||||
|
anchor_age = (max(x.after_ns for x in latches)-self.frame_clock.anchor.midpoint_ns)/1e9
|
||||||
|
self.next_refresh = time.monotonic()+max(0., self.frame_clock.refresh_interval_seconds-anchor_age)
|
||||||
|
|
||||||
|
def refresh_if_due(self):
|
||||||
|
if self.frame_clock is None:
|
||||||
|
self.start()
|
||||||
|
elif time.monotonic() >= self.next_refresh:
|
||||||
|
self._update([self._latch() for _ in range(3)])
|
||||||
|
|
||||||
|
def invalidate(self, reason):
|
||||||
|
"""Stop using an uncertain mapping until a new startup check succeeds."""
|
||||||
|
self.frame_clock = None
|
||||||
|
self.next_refresh = 0.
|
||||||
|
self._write(dict(kind="camera_clock_invalidated", reason=str(reason),
|
||||||
|
last_stamp_ns=self.last_stamp_ns))
|
||||||
|
if self.journal is not None:
|
||||||
|
self.journal.flush()
|
||||||
|
|
||||||
|
def timestamp(self, info, received_ns):
|
||||||
|
if self.frame_clock is None:
|
||||||
|
raise ValueError("camera frame has no synchronized clock")
|
||||||
|
tick = (int(info.nDevTimeStampHigh)<<32)|int(info.nDevTimeStampLow)
|
||||||
|
# Auto-exposure preview uses the exact exposure-start timestamp when
|
||||||
|
# the SDK supplies no per-frame duration. Calibration fixes exposure.
|
||||||
|
measured_exposure = float(info.fExposureTime)
|
||||||
|
exposure = measured_exposure if measured_exposure > 0 else self.exposure_us
|
||||||
|
stamp = self.frame_clock.timestamp(tick, received_ns=received_ns, exposure_us=exposure)
|
||||||
|
if self.last_stamp_ns is not None and stamp <= self.last_stamp_ns:
|
||||||
|
raise ValueError("camera exposure time is not increasing after clock synchronization")
|
||||||
|
self.last_stamp_ns = stamp
|
||||||
|
self._write(dict(kind="camera_frame_time", frame_number=int(info.nFrameNum),
|
||||||
|
device_tick=tick, sdk_host_stamp_ms=int(info.nHostTimeStamp),
|
||||||
|
received_ns=received_ns, stamp_ns=stamp, exposure_us=exposure,
|
||||||
|
timestamp_reference="exposure_midpoint" if exposure else "exposure_start"))
|
||||||
|
return stamp
|
||||||
|
|
||||||
|
def close(self):
|
||||||
|
if self.journal is not None:
|
||||||
|
self.journal.close()
|
||||||
|
self.journal = None
|
||||||
+6
-1
@@ -44,10 +44,12 @@ def build_typed_profile():
|
|||||||
command_index_by_joint=command_index,
|
command_index_by_joint=command_index,
|
||||||
urdf_joint_by_joint={name: name for name in active},
|
urdf_joint_by_joint={name: name for name in active},
|
||||||
),
|
),
|
||||||
motion=replace(right.motion, tasks=tasks),
|
motion=replace(right.motion, tasks=tasks, joint_zero_references={}),
|
||||||
measurement=replace(
|
measurement=replace(
|
||||||
right.measurement,
|
right.measurement,
|
||||||
measurements=measurements,
|
measurements=measurements,
|
||||||
|
# Legacy mirroring does not inherit v3 acquisition authorizations.
|
||||||
|
transferred_motion_sources={},
|
||||||
cross_view_sources={
|
cross_view_sources={
|
||||||
left_joint_name(name): left_joint_name(source)
|
left_joint_name(name): left_joint_name(source)
|
||||||
for name, source in right.measurement.cross_view_sources.items()
|
for name, source in right.measurement.cross_view_sources.items()
|
||||||
@@ -65,6 +67,8 @@ def build_typed_profile():
|
|||||||
for k, v in right.zero.transferred_mimic_sources.items()},
|
for k, v in right.zero.transferred_mimic_sources.items()},
|
||||||
# Runtime-only mirrored products do not acquire/solve new geometry.
|
# Runtime-only mirrored products do not acquire/solve new geometry.
|
||||||
spatial={},
|
spatial={},
|
||||||
|
known_baseline_geometry={},
|
||||||
|
cad_zero_assumptions={},
|
||||||
active_joints=active,
|
active_joints=active,
|
||||||
passive_joints=passive,
|
passive_joints=passive,
|
||||||
direct_zero_joints=tuple(
|
direct_zero_joints=tuple(
|
||||||
@@ -111,6 +115,7 @@ def build_typed_profile():
|
|||||||
),
|
),
|
||||||
artifacts=replace(
|
artifacts=replace(
|
||||||
right.artifacts,
|
right.artifacts,
|
||||||
|
output_schema_version=2,
|
||||||
calibration_filename=(
|
calibration_filename=(
|
||||||
"l6_left_{serial_number}_transferred_calibration.json"
|
"l6_left_{serial_number}_transferred_calibration.json"
|
||||||
),
|
),
|
||||||
|
|||||||
+9
-1
@@ -39,14 +39,18 @@ def build_typed_profile():
|
|||||||
right,
|
right,
|
||||||
key=KEY,
|
key=KEY,
|
||||||
namespace="/o6_left_transferred",
|
namespace="/o6_left_transferred",
|
||||||
|
# This historical mirror has no measured left-hand range evidence.
|
||||||
|
urdf_limit_policies={},
|
||||||
command=replace(
|
command=replace(
|
||||||
right.command,
|
right.command,
|
||||||
command_index_by_joint=command_index,
|
command_index_by_joint=command_index,
|
||||||
urdf_joint_by_joint={name: name for name in active},
|
urdf_joint_by_joint={name: name for name in active},
|
||||||
),
|
),
|
||||||
motion=replace(right.motion, tasks=tasks),
|
motion=replace(right.motion, tasks=tasks, joint_zero_references={}),
|
||||||
measurement=replace(
|
measurement=replace(
|
||||||
right.measurement,
|
right.measurement,
|
||||||
|
# Legacy mirroring does not inherit v3 acquisition authorizations.
|
||||||
|
transferred_motion_sources={},
|
||||||
measurements=measurements,
|
measurements=measurements,
|
||||||
cross_view_sources={
|
cross_view_sources={
|
||||||
left_joint_name(name): left_joint_name(source)
|
left_joint_name(name): left_joint_name(source)
|
||||||
@@ -65,6 +69,8 @@ def build_typed_profile():
|
|||||||
for k, v in right.zero.transferred_mimic_sources.items()},
|
for k, v in right.zero.transferred_mimic_sources.items()},
|
||||||
# Runtime-only mirrored products do not acquire/solve new geometry.
|
# Runtime-only mirrored products do not acquire/solve new geometry.
|
||||||
spatial={},
|
spatial={},
|
||||||
|
known_baseline_geometry={},
|
||||||
|
cad_zero_assumptions={},
|
||||||
active_joints=active,
|
active_joints=active,
|
||||||
passive_joints=passive,
|
passive_joints=passive,
|
||||||
direct_zero_joints=tuple(
|
direct_zero_joints=tuple(
|
||||||
@@ -111,6 +117,8 @@ def build_typed_profile():
|
|||||||
),
|
),
|
||||||
artifacts=replace(
|
artifacts=replace(
|
||||||
right.artifacts,
|
right.artifacts,
|
||||||
|
output_schema_version=2,
|
||||||
|
directional_command_joints=frozenset(),
|
||||||
calibration_filename=(
|
calibration_filename=(
|
||||||
"o6_left_{serial_number}_transferred_calibration.json"
|
"o6_left_{serial_number}_transferred_calibration.json"
|
||||||
),
|
),
|
||||||
|
|||||||
@@ -7,6 +7,7 @@ from .domain import (
|
|||||||
CalibrationProfile,
|
CalibrationProfile,
|
||||||
CalibrationResult,
|
CalibrationResult,
|
||||||
JointMapping,
|
JointMapping,
|
||||||
|
JointLimitPolicy,
|
||||||
CalibrationStatus,
|
CalibrationStatus,
|
||||||
CommandLayout,
|
CommandLayout,
|
||||||
MeasurementPolicy,
|
MeasurementPolicy,
|
||||||
@@ -53,6 +54,7 @@ __all__ = [
|
|||||||
"CalibrationProfile",
|
"CalibrationProfile",
|
||||||
"CalibrationResult",
|
"CalibrationResult",
|
||||||
"JointMapping",
|
"JointMapping",
|
||||||
|
"JointLimitPolicy",
|
||||||
"CalibrationStatus",
|
"CalibrationStatus",
|
||||||
"CommandLayout",
|
"CommandLayout",
|
||||||
"DIRECTION_DECREASING",
|
"DIRECTION_DECREASING",
|
||||||
|
|||||||
@@ -0,0 +1,525 @@
|
|||||||
|
"""Indexed compressed storage for large calibration evidence records.
|
||||||
|
|
||||||
|
The JSONL file remains the ordered, crash-safe journal. Small control records
|
||||||
|
stay inline. Large records are stored in independently compressed blocks and
|
||||||
|
the JSONL line contains the scalar resume index plus a content-bound reference.
|
||||||
|
Readers can either inspect that index or reconstruct the original record.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Iterable, Mapping
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from functools import lru_cache
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
from pathlib import Path
|
||||||
|
import shutil
|
||||||
|
import zlib
|
||||||
|
|
||||||
|
|
||||||
|
FORMAT = "linkerhand_indexed_evidence_v1"
|
||||||
|
REFERENCE_KEY = "$evidence_record"
|
||||||
|
FIELDS_KEY = "$record_fields"
|
||||||
|
SCHEMA_KEY = "$record_schema"
|
||||||
|
CANONICAL_SHA256_KEY = "$record_canonical_sha256"
|
||||||
|
DEFAULT_THRESHOLD_BYTES = 16 * 1024
|
||||||
|
MAXIMUM_BLOCK_BYTES = 4 * 1024 * 1024
|
||||||
|
INDEXED_SCALAR_FIELDS = {
|
||||||
|
"kind", "profile_id", "serial_number", "task_name", "joint",
|
||||||
|
"observation_joint", "view", "image_stamp_ns", "cycle", "direction",
|
||||||
|
"segment_key", "attempt", "sample_phase", "passed", "status",
|
||||||
|
"motion_version", "session_epoch", "reference_path_index",
|
||||||
|
"observer_preparation_id", "preparation_witness_joint", "tag_role",
|
||||||
|
"motor_index", "steady_index",
|
||||||
|
"steady_target", "command_u8", "command_rad", "feedback_u8",
|
||||||
|
"feedback_rad", "command_index", "branch_revision",
|
||||||
|
}
|
||||||
|
DETAILED_RECORD_KINDS = {
|
||||||
|
"motion_branch_observation", "image_observation_frame",
|
||||||
|
"pnp_candidate_frame", "pnp_branch_event", "joint_sample",
|
||||||
|
"secondary_joint_sample", "joint_zero_sample",
|
||||||
|
"visual_motion_observation", "diagnostic_observation_frame",
|
||||||
|
}
|
||||||
|
CANONICAL_INDEX_KINDS = {
|
||||||
|
"motion_branch_observation", "motion_branch_initialization",
|
||||||
|
"image_observation_frame", "pnp_candidate_frame", "joint_zero_sample",
|
||||||
|
"joint_zero_motion_reference_verified", "parent_reference_verified",
|
||||||
|
"observer_preparation_decision", "observer_preparation_segment_complete",
|
||||||
|
"observer_preparation_excursion_started", "observer_preparation_cancelled_before_excursion",
|
||||||
|
"capture_scope_extension", "capture_observation_revision",
|
||||||
|
"passed_tasks_reused", "passed_pose_models_restored", "staged_pose_models_restored",
|
||||||
|
"resume_verification", "joint_zero_recovery", "visual_motion_observation",
|
||||||
|
"visual_motion_settled", "task_observation_state",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class EvidenceRecordReference:
|
||||||
|
offset: int
|
||||||
|
compressed_length: int
|
||||||
|
block_sha256: str
|
||||||
|
record_offset: int
|
||||||
|
record_length: int
|
||||||
|
record_sha256: str
|
||||||
|
archive: str = ""
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_mapping(cls, value):
|
||||||
|
common = {"version", "offset", "compressed_length", "block_sha256",
|
||||||
|
"record_offset", "record_length", "record_sha256"}
|
||||||
|
version = value.get("version")
|
||||||
|
if ((version == 1 and set(value) != common)
|
||||||
|
or (version == 2 and set(value) != common | {"archive"})
|
||||||
|
or version not in {1, 2}):
|
||||||
|
raise ValueError("invalid_compressed_evidence_reference")
|
||||||
|
integers = [value[key] for key in ("offset", "compressed_length", "record_offset", "record_length")]
|
||||||
|
if any(type(item) is not int or item < 0 for item in integers) or integers[1] < 1 or integers[3] < 1:
|
||||||
|
raise ValueError("invalid_compressed_evidence_reference")
|
||||||
|
archive = value.get("archive", "")
|
||||||
|
if archive and (Path(archive).name != archive or archive in {".", ".."}):
|
||||||
|
raise ValueError("invalid_compressed_evidence_reference")
|
||||||
|
return cls(*(value[key] for key in (
|
||||||
|
"offset", "compressed_length", "block_sha256", "record_offset",
|
||||||
|
"record_length", "record_sha256")), archive)
|
||||||
|
|
||||||
|
def as_mapping(self):
|
||||||
|
value = {"version": 2 if self.archive else 1, "offset": self.offset,
|
||||||
|
"compressed_length": self.compressed_length, "block_sha256": self.block_sha256,
|
||||||
|
"record_offset": self.record_offset, "record_length": self.record_length,
|
||||||
|
"record_sha256": self.record_sha256}
|
||||||
|
if self.archive:
|
||||||
|
value["archive"] = self.archive
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def manifest_path(journal_path: str | Path) -> Path:
|
||||||
|
path = Path(journal_path)
|
||||||
|
return path.with_name(path.stem + ".storage.json")
|
||||||
|
|
||||||
|
|
||||||
|
def archive_path(journal_path: str | Path) -> Path:
|
||||||
|
path = Path(journal_path)
|
||||||
|
return path.with_name(path.stem + ".evidence.zlib")
|
||||||
|
|
||||||
|
|
||||||
|
def initialize_evidence_journal(journal_path: str | Path, *,
|
||||||
|
threshold_bytes=DEFAULT_THRESHOLD_BYTES, compression_level=1) -> None:
|
||||||
|
"""Enable indexed storage for a new journal without changing old captures."""
|
||||||
|
journal = Path(journal_path)
|
||||||
|
marker = manifest_path(journal)
|
||||||
|
if marker.exists():
|
||||||
|
payload = json.loads(marker.read_text(encoding="utf-8"))
|
||||||
|
if payload.get("format") != FORMAT:
|
||||||
|
raise ValueError("unsupported_evidence_journal_format")
|
||||||
|
return
|
||||||
|
if journal.exists() and journal.stat().st_size:
|
||||||
|
raise ValueError("indexed_evidence_requires_new_or_migrated_journal")
|
||||||
|
journal.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
if type(compression_level) is not int or not 1 <= compression_level <= 9:
|
||||||
|
raise ValueError("invalid_evidence_compression_level")
|
||||||
|
from .storage import atomic_write_json
|
||||||
|
atomic_write_json(marker, {
|
||||||
|
"format": FORMAT,
|
||||||
|
"archive": archive_path(journal).name,
|
||||||
|
"codec": f"zlib_level_{compression_level}_independent_blocks",
|
||||||
|
"compression_level": compression_level,
|
||||||
|
"envelope_fields": "archive_bound_v2",
|
||||||
|
"field_schemas": {},
|
||||||
|
"large_record_threshold_bytes": int(threshold_bytes),
|
||||||
|
"maximum_uncompressed_block_bytes": MAXIMUM_BLOCK_BYTES,
|
||||||
|
})
|
||||||
|
_journal_configuration.cache_clear()
|
||||||
|
|
||||||
|
|
||||||
|
@lru_cache(maxsize=128)
|
||||||
|
def _journal_configuration(journal_path: str) -> dict | None:
|
||||||
|
marker = manifest_path(journal_path)
|
||||||
|
if not marker.is_file():
|
||||||
|
return None
|
||||||
|
try:
|
||||||
|
value = json.loads(marker.read_text(encoding="utf-8"))
|
||||||
|
except (OSError, ValueError, TypeError):
|
||||||
|
return None
|
||||||
|
return value if value.get("format") == FORMAT else None
|
||||||
|
|
||||||
|
|
||||||
|
def is_indexed_journal(journal_path: str | Path) -> bool:
|
||||||
|
return _journal_configuration(str(Path(journal_path).resolve())) is not None
|
||||||
|
|
||||||
|
|
||||||
|
def _resume_index(record: Mapping) -> dict:
|
||||||
|
return {key: record[key] for key in INDEXED_SCALAR_FIELDS if key in record
|
||||||
|
and (record[key] is None or isinstance(record[key], (str, int, float, bool)))}
|
||||||
|
|
||||||
|
|
||||||
|
def _schema_identity(fields) -> str:
|
||||||
|
return hashlib.sha256(json.dumps(list(fields), separators=(",", ":")).encode()).hexdigest()[:16]
|
||||||
|
|
||||||
|
|
||||||
|
@lru_cache(maxsize=512)
|
||||||
|
def _schema_fields(journal_path: str | Path, identity: str):
|
||||||
|
marker = _journal_configuration(str(Path(journal_path).resolve())) or {}
|
||||||
|
fields = marker.get("field_schemas", {}).get(identity)
|
||||||
|
if not isinstance(fields, list) or not all(isinstance(field, str) for field in fields):
|
||||||
|
raise ValueError("compressed_evidence_schema_missing")
|
||||||
|
if _schema_identity(fields) != identity:
|
||||||
|
raise ValueError("compressed_evidence_schema_changed")
|
||||||
|
return tuple(fields)
|
||||||
|
|
||||||
|
|
||||||
|
def _envelope(record: Mapping, reference, schema_identity) -> dict:
|
||||||
|
indexed = _resume_index(record)
|
||||||
|
if REFERENCE_KEY in indexed or FIELDS_KEY in indexed or SCHEMA_KEY in indexed:
|
||||||
|
raise ValueError("calibration_record_uses_reserved_storage_field")
|
||||||
|
# record_sha256 binds the complete archived object. Repeating its field
|
||||||
|
# names in every one-line resume index added tens of MB without adding an
|
||||||
|
# independent integrity check.
|
||||||
|
result = {REFERENCE_KEY: reference, SCHEMA_KEY: schema_identity, **indexed}
|
||||||
|
if record.get("kind") in CANONICAL_INDEX_KINDS:
|
||||||
|
result[CANONICAL_SHA256_KEY] = hashlib.sha256(json.dumps(record, sort_keys=True,
|
||||||
|
separators=(",", ":"), allow_nan=False).encode()).hexdigest()
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def _write_all(stream, payload: bytes) -> None:
|
||||||
|
written = stream.write(payload)
|
||||||
|
if written != len(payload):
|
||||||
|
raise OSError("incomplete_evidence_archive_write")
|
||||||
|
|
||||||
|
|
||||||
|
def append_indexed_records(journal_path: str | Path, records: Iterable[Mapping], *, durable: bool) -> None:
|
||||||
|
"""Append records in order; evidence is durable before its references."""
|
||||||
|
journal = Path(journal_path)
|
||||||
|
marker = _journal_configuration(str(journal.resolve()))
|
||||||
|
if marker is None:
|
||||||
|
raise ValueError("unsupported_evidence_journal_format")
|
||||||
|
threshold = int(marker["large_record_threshold_bytes"])
|
||||||
|
compression_level = int(marker.get("compression_level", 1))
|
||||||
|
if not 1 <= compression_level <= 9:
|
||||||
|
raise ValueError("invalid_evidence_compression_level")
|
||||||
|
prepared = []
|
||||||
|
imported = {}
|
||||||
|
for record in records:
|
||||||
|
if REFERENCE_KEY in record or FIELDS_KEY in record or SCHEMA_KEY in record:
|
||||||
|
raise ValueError("calibration_record_uses_reserved_storage_field")
|
||||||
|
reference = getattr(record, "reference", None)
|
||||||
|
field_names = getattr(record, "field_names", None)
|
||||||
|
fields = tuple(record) if field_names is None else tuple(field_names)
|
||||||
|
schema_identity = _schema_identity(fields)
|
||||||
|
if isinstance(reference, EvidenceRecordReference):
|
||||||
|
imported[len(prepared)] = (record, schema_identity)
|
||||||
|
line = None
|
||||||
|
else:
|
||||||
|
line = json.dumps(record, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
|
||||||
|
prepared.append((record, line, schema_identity, fields))
|
||||||
|
if not prepared:
|
||||||
|
return
|
||||||
|
|
||||||
|
schemas = dict(marker.get("field_schemas", {}))
|
||||||
|
changed = False
|
||||||
|
for _, _, identity, fields in prepared:
|
||||||
|
existing = schemas.get(identity)
|
||||||
|
if existing is not None and tuple(existing) != fields:
|
||||||
|
raise ValueError("compressed_evidence_schema_collision")
|
||||||
|
if existing is None:
|
||||||
|
schemas[identity] = list(fields)
|
||||||
|
changed = True
|
||||||
|
if changed:
|
||||||
|
marker = {**marker, "field_schemas": schemas}
|
||||||
|
from .storage import atomic_write_json
|
||||||
|
atomic_write_json(manifest_path(journal), marker)
|
||||||
|
_journal_configuration.cache_clear()
|
||||||
|
|
||||||
|
evidence = archive_path(journal)
|
||||||
|
evidence.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
output: list[bytes | None] = [None] * len(prepared)
|
||||||
|
for index, (record, schema_identity) in imported.items():
|
||||||
|
envelope = _linked_envelope(journal, record, schema_identity)
|
||||||
|
output[index] = json.dumps(envelope, ensure_ascii=False,
|
||||||
|
separators=(",", ":")).encode("utf-8")
|
||||||
|
with evidence.open("ab") as archive:
|
||||||
|
large: list[tuple[int, Mapping, bytes, str]] = []
|
||||||
|
size = 0
|
||||||
|
|
||||||
|
def flush_block():
|
||||||
|
nonlocal large, size
|
||||||
|
if not large:
|
||||||
|
return
|
||||||
|
block = b"".join(line for _, _, line, _ in large)
|
||||||
|
compressed = zlib.compress(block, compression_level)
|
||||||
|
offset = archive.tell()
|
||||||
|
_write_all(archive, compressed)
|
||||||
|
block_digest = hashlib.sha256(compressed).hexdigest()
|
||||||
|
local = 0
|
||||||
|
for index, record, line, schema_identity in large:
|
||||||
|
reference = {
|
||||||
|
"version": 1, "offset": offset,
|
||||||
|
"compressed_length": len(compressed),
|
||||||
|
"block_sha256": block_digest,
|
||||||
|
"record_offset": local, "record_length": len(line),
|
||||||
|
"record_sha256": hashlib.sha256(line).hexdigest(),
|
||||||
|
}
|
||||||
|
output[index] = json.dumps(_envelope(record, reference, schema_identity),
|
||||||
|
ensure_ascii=False, separators=(",", ":")).encode("utf-8")
|
||||||
|
local += len(line)
|
||||||
|
large, size = [], 0
|
||||||
|
|
||||||
|
for index, (record, line, schema_identity, _) in enumerate(prepared):
|
||||||
|
if line is None:
|
||||||
|
continue
|
||||||
|
if len(line) < threshold and record.get("kind") not in DETAILED_RECORD_KINDS:
|
||||||
|
output[index] = line
|
||||||
|
continue
|
||||||
|
if large and size + len(line) > MAXIMUM_BLOCK_BYTES:
|
||||||
|
flush_block()
|
||||||
|
large.append((index, record, line, schema_identity))
|
||||||
|
size += len(line)
|
||||||
|
flush_block()
|
||||||
|
archive.flush()
|
||||||
|
if durable:
|
||||||
|
os.fsync(archive.fileno())
|
||||||
|
|
||||||
|
with journal.open("ab") as stream:
|
||||||
|
_write_all(stream, b"\n".join(output) + b"\n")
|
||||||
|
stream.flush()
|
||||||
|
if durable:
|
||||||
|
os.fsync(stream.fileno())
|
||||||
|
|
||||||
|
|
||||||
|
def _linked_envelope(destination: Path, record, schema_identity) -> dict:
|
||||||
|
"""Hard-link an immutable source archive and retain its indexed row."""
|
||||||
|
reference = record.reference
|
||||||
|
source_archive = (record.path.parent / reference.archive
|
||||||
|
if reference.archive else archive_path(record.path))
|
||||||
|
stat = source_archive.stat()
|
||||||
|
identity = hashlib.sha256(
|
||||||
|
f"{stat.st_dev}:{stat.st_ino}:{stat.st_size}".encode()).hexdigest()[:20]
|
||||||
|
target = destination.parent / f"imported_evidence_{identity}.zlib"
|
||||||
|
if target.exists():
|
||||||
|
if not os.path.samefile(source_archive, target):
|
||||||
|
raise ValueError("imported_evidence_identity_collision")
|
||||||
|
else:
|
||||||
|
try:
|
||||||
|
os.link(source_archive, target)
|
||||||
|
except OSError as error:
|
||||||
|
if error.errno != getattr(os, "EXDEV", 18):
|
||||||
|
raise
|
||||||
|
shutil.copy2(source_archive, target)
|
||||||
|
directory = os.open(destination.parent, os.O_RDONLY)
|
||||||
|
try:
|
||||||
|
os.fsync(directory)
|
||||||
|
finally:
|
||||||
|
os.close(directory)
|
||||||
|
linked = EvidenceRecordReference(reference.offset, reference.compressed_length,
|
||||||
|
reference.block_sha256, reference.record_offset, reference.record_length,
|
||||||
|
reference.record_sha256, target.name)
|
||||||
|
result = {REFERENCE_KEY: linked.as_mapping(), SCHEMA_KEY: schema_identity, **record.metadata}
|
||||||
|
if record.canonical_sha256:
|
||||||
|
result[CANONICAL_SHA256_KEY] = record.canonical_sha256
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def sync_indexed_journal(journal_path: str | Path) -> None:
|
||||||
|
"""Order the persistence barrier so a durable reference is never dangling."""
|
||||||
|
evidence = archive_path(journal_path)
|
||||||
|
if evidence.exists():
|
||||||
|
with evidence.open("rb") as stream:
|
||||||
|
os.fsync(stream.fileno())
|
||||||
|
with Path(journal_path).open("rb") as stream:
|
||||||
|
os.fsync(stream.fileno())
|
||||||
|
|
||||||
|
|
||||||
|
@lru_cache(maxsize=8)
|
||||||
|
def _read_block(path: str, offset: int, length: int, digest: str) -> bytes:
|
||||||
|
with Path(path).open("rb") as stream:
|
||||||
|
stream.seek(offset)
|
||||||
|
compressed = stream.read(length)
|
||||||
|
if len(compressed) != length or hashlib.sha256(compressed).hexdigest() != digest:
|
||||||
|
raise ValueError("compressed_evidence_block_changed")
|
||||||
|
try:
|
||||||
|
return zlib.decompress(compressed)
|
||||||
|
except zlib.error as error:
|
||||||
|
raise ValueError("compressed_evidence_block_invalid") from error
|
||||||
|
|
||||||
|
|
||||||
|
def evidence_record_bytes(journal_path: str | Path, reference: Mapping) -> bytes:
|
||||||
|
reference = (reference if isinstance(reference, EvidenceRecordReference)
|
||||||
|
else EvidenceRecordReference.from_mapping(reference))
|
||||||
|
journal_path = Path(journal_path)
|
||||||
|
evidence = (journal_path.parent / reference.archive
|
||||||
|
if reference.archive else archive_path(journal_path))
|
||||||
|
block = _read_block(str(evidence.resolve()), reference.offset,
|
||||||
|
reference.compressed_length, reference.block_sha256)
|
||||||
|
start, length = reference.record_offset, reference.record_length
|
||||||
|
raw = block[start:start + length]
|
||||||
|
if len(raw) != length or hashlib.sha256(raw).hexdigest() != reference.record_sha256:
|
||||||
|
raise ValueError("compressed_evidence_record_changed")
|
||||||
|
return raw
|
||||||
|
|
||||||
|
|
||||||
|
def decode_envelope(journal_path: str | Path, envelope: Mapping, decoder):
|
||||||
|
if REFERENCE_KEY not in envelope:
|
||||||
|
return envelope
|
||||||
|
value = decoder.loads(evidence_record_bytes(journal_path, envelope[REFERENCE_KEY]))
|
||||||
|
expected_fields = (envelope.get(FIELDS_KEY) if FIELDS_KEY in envelope
|
||||||
|
else _schema_fields(journal_path, envelope[SCHEMA_KEY]) if SCHEMA_KEY in envelope else None)
|
||||||
|
if (not isinstance(value, dict) or expected_fields is not None
|
||||||
|
and tuple(value) != tuple(expected_fields)):
|
||||||
|
raise ValueError("compressed_evidence_record_shape_changed")
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def compact_existing_journal(source_path: str | Path, *, replace=False) -> dict:
|
||||||
|
"""Stream a legacy journal into indexed storage and verify every record.
|
||||||
|
|
||||||
|
With ``replace=True`` the original path is swapped only after the compact
|
||||||
|
copy has passed record-content verification. The archive contains every
|
||||||
|
externalized record in full; no observation or diagnostic field is lost.
|
||||||
|
"""
|
||||||
|
source = Path(source_path).resolve()
|
||||||
|
if not source.is_file():
|
||||||
|
raise FileNotFoundError(source)
|
||||||
|
source_is_indexed = is_indexed_journal(source)
|
||||||
|
source_marker = (json.loads(manifest_path(source).read_text(encoding="utf-8"))
|
||||||
|
if source_is_indexed else {})
|
||||||
|
compact = source.with_name(source.name + ".compacting")
|
||||||
|
compact_archive = archive_path(compact)
|
||||||
|
for path in (compact, compact_archive, manifest_path(compact)):
|
||||||
|
if path.exists():
|
||||||
|
path.unlink()
|
||||||
|
# Closed checkpoints favor disk footprint. Live capture remains level 1 so
|
||||||
|
# evidence persistence cannot stall the control loop.
|
||||||
|
initialize_evidence_journal(compact, compression_level=6)
|
||||||
|
|
||||||
|
source_digest = hashlib.sha256()
|
||||||
|
record_count = 0
|
||||||
|
batch = []
|
||||||
|
try:
|
||||||
|
from ...core.geometry.frozen_evidence import EvidenceDecoder
|
||||||
|
decoder = EvidenceDecoder(verify_hashes=False)
|
||||||
|
with source.open("rb") as stream:
|
||||||
|
for number, line in enumerate(stream, 1):
|
||||||
|
source_digest.update(line)
|
||||||
|
if not line.strip():
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
row = decoder.loads(line)
|
||||||
|
if REFERENCE_KEY in row:
|
||||||
|
row = decode_envelope(source, row, decoder)
|
||||||
|
except json.JSONDecodeError as error:
|
||||||
|
raise ValueError(f"invalid calibration JSONL at line {number}") from error
|
||||||
|
if not isinstance(row, dict):
|
||||||
|
raise ValueError(f"calibration JSONL record is not an object at line {number}")
|
||||||
|
batch.append(row)
|
||||||
|
record_count += 1
|
||||||
|
if len(batch) == 256:
|
||||||
|
append_indexed_records(compact, batch, durable=False)
|
||||||
|
batch.clear()
|
||||||
|
append_indexed_records(compact, batch, durable=False)
|
||||||
|
sync_indexed_journal(compact)
|
||||||
|
|
||||||
|
# Stream verification keeps the migration bounded even for multi-GB captures.
|
||||||
|
left_decoder, right_decoder = (EvidenceDecoder(verify_hashes=False) for _ in range(2))
|
||||||
|
with source.open("rb") as left, compact.open("rb") as right:
|
||||||
|
right_lines = (line for line in right if line.strip())
|
||||||
|
verified = 0
|
||||||
|
for left_line in left:
|
||||||
|
if not left_line.strip():
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
right_line = next(right_lines)
|
||||||
|
except StopIteration as error:
|
||||||
|
raise ValueError("compacted_evidence_record_count_changed") from error
|
||||||
|
left_row = left_decoder.loads(left_line)
|
||||||
|
if REFERENCE_KEY in left_row:
|
||||||
|
left_row = decode_envelope(source, left_row, left_decoder)
|
||||||
|
envelope = right_decoder.loads(right_line)
|
||||||
|
right_row = decode_envelope(compact, envelope, right_decoder)
|
||||||
|
if left_row != right_row:
|
||||||
|
raise ValueError("compacted_evidence_differs_from_source")
|
||||||
|
verified += 1
|
||||||
|
try:
|
||||||
|
next(right_lines)
|
||||||
|
except StopIteration:
|
||||||
|
pass
|
||||||
|
else:
|
||||||
|
raise ValueError("compacted_evidence_record_count_changed")
|
||||||
|
if verified != record_count:
|
||||||
|
raise ValueError("compacted_evidence_record_count_changed")
|
||||||
|
def file_sha256(path):
|
||||||
|
with Path(path).open("rb") as stream:
|
||||||
|
return hashlib.file_digest(stream, "sha256").hexdigest()
|
||||||
|
|
||||||
|
compact_digest = file_sha256(compact)
|
||||||
|
archive_digest = file_sha256(compact_archive)
|
||||||
|
result = {
|
||||||
|
"format": FORMAT, "source": str(source),
|
||||||
|
"source_sha256": source_digest.hexdigest(),
|
||||||
|
"source_bytes": source.stat().st_size,
|
||||||
|
"record_count": record_count,
|
||||||
|
"compact_journal_bytes": compact.stat().st_size,
|
||||||
|
"compressed_evidence_bytes": compact_archive.stat().st_size,
|
||||||
|
"compact_journal_sha256": compact_digest,
|
||||||
|
"compressed_evidence_sha256": archive_digest,
|
||||||
|
"semantic_verification": "passed",
|
||||||
|
}
|
||||||
|
if replace:
|
||||||
|
final_archive = archive_path(source)
|
||||||
|
final_manifest = manifest_path(source)
|
||||||
|
backup = source.with_name(source.name + ".uncompressed-backup")
|
||||||
|
backup_archive = final_archive.with_name(final_archive.name + ".backup")
|
||||||
|
backup_manifest = final_manifest.with_name(final_manifest.name + ".backup")
|
||||||
|
if backup.exists() or backup_archive.exists() or backup_manifest.exists():
|
||||||
|
raise FileExistsError("compact journal destination already exists")
|
||||||
|
if not source_is_indexed and (final_archive.exists() or final_manifest.exists()):
|
||||||
|
raise FileExistsError("compact journal destination already exists")
|
||||||
|
marker = json.loads(manifest_path(compact).read_text(encoding="utf-8"))
|
||||||
|
marker["archive"] = final_archive.name
|
||||||
|
marker.update({key: result[key] for key in ("source_sha256", "source_bytes", "record_count")})
|
||||||
|
if source_marker:
|
||||||
|
marker["lineage_source_sha256"] = source_marker.get(
|
||||||
|
"lineage_source_sha256", source_marker.get("source_sha256"))
|
||||||
|
marker["lineage_source_bytes"] = source_marker.get(
|
||||||
|
"lineage_source_bytes", source_marker.get("source_bytes"))
|
||||||
|
from .storage import atomic_write_json
|
||||||
|
try:
|
||||||
|
if source_is_indexed:
|
||||||
|
final_archive.replace(backup_archive)
|
||||||
|
final_manifest.replace(backup_manifest)
|
||||||
|
compact_archive.replace(final_archive)
|
||||||
|
source.replace(backup)
|
||||||
|
compact.replace(source)
|
||||||
|
if (file_sha256(source) != compact_digest
|
||||||
|
or file_sha256(final_archive) != archive_digest):
|
||||||
|
raise ValueError("installed_compact_journal_verification_failed")
|
||||||
|
atomic_write_json(final_manifest, marker)
|
||||||
|
_journal_configuration.cache_clear()
|
||||||
|
except Exception:
|
||||||
|
final_manifest.unlink(missing_ok=True)
|
||||||
|
if backup.exists():
|
||||||
|
if source.exists():
|
||||||
|
source.replace(compact)
|
||||||
|
backup.replace(source)
|
||||||
|
if final_archive.exists():
|
||||||
|
final_archive.replace(compact_archive)
|
||||||
|
if source_is_indexed:
|
||||||
|
if backup_archive.exists():
|
||||||
|
backup_archive.replace(final_archive)
|
||||||
|
if backup_manifest.exists():
|
||||||
|
backup_manifest.replace(final_manifest)
|
||||||
|
raise
|
||||||
|
backup.unlink()
|
||||||
|
backup_archive.unlink(missing_ok=True)
|
||||||
|
backup_manifest.unlink(missing_ok=True)
|
||||||
|
manifest_path(compact).unlink(missing_ok=True)
|
||||||
|
_journal_configuration.cache_clear()
|
||||||
|
result["installed"] = True
|
||||||
|
return result
|
||||||
|
except Exception:
|
||||||
|
_journal_configuration.cache_clear()
|
||||||
|
compact.unlink(missing_ok=True)
|
||||||
|
compact_archive.unlink(missing_ok=True)
|
||||||
|
manifest_path(compact).unlink(missing_ok=True)
|
||||||
|
raise
|
||||||
@@ -23,6 +23,10 @@ def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> None:
|
|||||||
def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
|
def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
|
||||||
destination = Path(path)
|
destination = Path(path)
|
||||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
from .evidence_journal import is_indexed_journal, append_indexed_records
|
||||||
|
if is_indexed_journal(destination):
|
||||||
|
append_indexed_records(destination, (payload,), durable=True)
|
||||||
|
return
|
||||||
line = json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
line = json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
||||||
with destination.open("a", encoding="utf-8") as stream:
|
with destination.open("a", encoding="utf-8") as stream:
|
||||||
stream.write(line + "\n")
|
stream.write(line + "\n")
|
||||||
@@ -31,11 +35,21 @@ def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
|
|||||||
|
|
||||||
|
|
||||||
def append_jsonl_many(
|
def append_jsonl_many(
|
||||||
path: str | Path, payloads: Iterable[Mapping[str, Any]]
|
path: str | Path, payloads: Iterable[Mapping[str, Any]], *, durable: bool = True,
|
||||||
) -> None:
|
) -> 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 = Path(path)
|
||||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
payloads = tuple(payloads)
|
||||||
|
from .evidence_journal import is_indexed_journal, append_indexed_records
|
||||||
|
if is_indexed_journal(destination):
|
||||||
|
append_indexed_records(destination, payloads, durable=durable)
|
||||||
|
return
|
||||||
lines = [
|
lines = [
|
||||||
json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
||||||
for payload in payloads
|
for payload in payloads
|
||||||
@@ -45,6 +59,17 @@ def append_jsonl_many(
|
|||||||
with destination.open("a", encoding="utf-8") as stream:
|
with destination.open("a", encoding="utf-8") as stream:
|
||||||
stream.write("\n".join(lines) + "\n")
|
stream.write("\n".join(lines) + "\n")
|
||||||
stream.flush()
|
stream.flush()
|
||||||
|
if durable:
|
||||||
|
os.fsync(stream.fileno())
|
||||||
|
|
||||||
|
|
||||||
|
def sync_jsonl(path: str | Path) -> None:
|
||||||
|
"""Persist all previously flushed records before hashing or publishing."""
|
||||||
|
from .evidence_journal import is_indexed_journal, sync_indexed_journal
|
||||||
|
if is_indexed_journal(path):
|
||||||
|
sync_indexed_journal(path)
|
||||||
|
return
|
||||||
|
with Path(path).open("rb") as stream:
|
||||||
os.fsync(stream.fileno())
|
os.fsync(stream.fileno())
|
||||||
|
|
||||||
|
|
||||||
@@ -53,6 +78,9 @@ def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
|
|||||||
if not source.exists():
|
if not source.exists():
|
||||||
return []
|
return []
|
||||||
records: list[dict[str, Any]] = []
|
records: list[dict[str, Any]] = []
|
||||||
|
from .evidence_journal import REFERENCE_KEY, decode_envelope
|
||||||
|
from ..geometry.frozen_evidence import EvidenceDecoder
|
||||||
|
decoder = EvidenceDecoder()
|
||||||
with source.open("r", encoding="utf-8") as stream:
|
with source.open("r", encoding="utf-8") as stream:
|
||||||
lines = stream.readlines()
|
lines = stream.readlines()
|
||||||
nonempty_lines = [
|
nonempty_lines = [
|
||||||
@@ -72,6 +100,8 @@ def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
|
|||||||
) from error
|
) from error
|
||||||
if not isinstance(value, dict):
|
if not isinstance(value, dict):
|
||||||
raise ValueError(f"{source}:{line_number}: record must be an object")
|
raise ValueError(f"{source}:{line_number}: record must be an object")
|
||||||
|
if REFERENCE_KEY in value:
|
||||||
|
value = decode_envelope(source, value, decoder)
|
||||||
records.append(value)
|
records.append(value)
|
||||||
return records
|
return records
|
||||||
|
|
||||||
|
|||||||
@@ -17,9 +17,13 @@ from .profile import (
|
|||||||
ViewSpec,
|
ViewSpec,
|
||||||
VisionRigSpec,
|
VisionRigSpec,
|
||||||
ZeroSolvePolicy,
|
ZeroSolvePolicy,
|
||||||
|
JointZeroSpec,
|
||||||
|
KnownBaselineGeometry,
|
||||||
|
JointLimitPolicy,
|
||||||
validate_profile,
|
validate_profile,
|
||||||
)
|
)
|
||||||
from .result import CalibrationResult, JointMapping
|
from .result import CalibrationResult, JointMapping
|
||||||
|
from .reference import JointZeroReference, JointZeroSample
|
||||||
from .status import (
|
from .status import (
|
||||||
AcquisitionStatus,
|
AcquisitionStatus,
|
||||||
CalibrationStatus,
|
CalibrationStatus,
|
||||||
@@ -35,6 +39,11 @@ __all__ = [
|
|||||||
"CalibrationProfile",
|
"CalibrationProfile",
|
||||||
"CalibrationResult",
|
"CalibrationResult",
|
||||||
"JointMapping",
|
"JointMapping",
|
||||||
|
"JointZeroReference",
|
||||||
|
"JointZeroSample",
|
||||||
|
"JointZeroSpec",
|
||||||
|
"KnownBaselineGeometry",
|
||||||
|
"JointLimitPolicy",
|
||||||
"CalibrationStatus",
|
"CalibrationStatus",
|
||||||
"CommandLayout",
|
"CommandLayout",
|
||||||
"MeasurementPolicy",
|
"MeasurementPolicy",
|
||||||
|
|||||||
@@ -0,0 +1,192 @@
|
|||||||
|
"""One immutable partition contract for acquisition, fitting and release.
|
||||||
|
|
||||||
|
Cycle IDs are evidence identities, not display ordinals. Optional training
|
||||||
|
never renumbers the independent validation cycle or changes its command grid.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import asdict, dataclass, replace
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
|
||||||
|
|
||||||
|
PLAN_VERSION = "capture_plan_v1"
|
||||||
|
FIXED = "fixed"
|
||||||
|
ADAPTIVE = "adaptive_2_to_3"
|
||||||
|
STAGED = "staged_2_to_3"
|
||||||
|
TASKWISE_STAGED = "taskwise_staged_2_to_3"
|
||||||
|
TASKWISE_TWO_ROUND = "taskwise_2_plus_1"
|
||||||
|
RAW_JOINT = "raw_joint_2_plus_1"
|
||||||
|
RAW_PROTOCOL = "raw_joint_images_v2"
|
||||||
|
LEGACY_RAW_PROTOCOL = "raw_joint_images_v1"
|
||||||
|
STAGED_POLICIES = frozenset((STAGED, TASKWISE_STAGED))
|
||||||
|
PREPARATION_POLICIES = STAGED_POLICIES | {TASKWISE_TWO_ROUND}
|
||||||
|
LEGACY_TRAINING = (0, 1, 2)
|
||||||
|
LEGACY_HOLDOUT = 3
|
||||||
|
|
||||||
|
|
||||||
|
def raw_joint_capture(profile):
|
||||||
|
return profile.quality.training_policy == RAW_JOINT
|
||||||
|
|
||||||
|
|
||||||
|
def evidence_digest(value):
|
||||||
|
return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(",", ":"),
|
||||||
|
allow_nan=False).encode()).hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class TaskPartition:
|
||||||
|
task_key: str
|
||||||
|
training: tuple[int, ...]
|
||||||
|
holdout: int
|
||||||
|
command_training: tuple[int, ...]
|
||||||
|
command_holdout: int
|
||||||
|
|
||||||
|
@property
|
||||||
|
def motion_cycles(self):
|
||||||
|
return (*self.training, self.holdout)
|
||||||
|
|
||||||
|
def cycles(self, phase):
|
||||||
|
return ((*self.command_training, self.command_holdout)
|
||||||
|
if phase == "steady" else self.motion_cycles)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CapturePlan:
|
||||||
|
policy: str
|
||||||
|
command_capture_mode: str
|
||||||
|
tasks: tuple[TaskPartition, ...]
|
||||||
|
motion_contract_json: str
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_profile(cls, profile):
|
||||||
|
tasks = tuple(task_partition(profile, task) for task in profile.motion.tasks)
|
||||||
|
# JSON normalization keeps equality stable across checkpoint round trips.
|
||||||
|
from .sampling import command_nodes
|
||||||
|
motion = {"unit": profile.command.unit, "motion": asdict(profile.motion),
|
||||||
|
"nodes": {task.key: {"training": command_nodes(profile, task),
|
||||||
|
"holdout": command_nodes(profile, task, holdout=True)} for task in profile.motion.tasks}}
|
||||||
|
# Auxiliary observer excursions produce no scan/partition identities.
|
||||||
|
# Their full paths are protected by preparation_contract instead; old
|
||||||
|
# completed units and training partitions keep their original hashes.
|
||||||
|
motion['motion'].pop('observer_preparations')
|
||||||
|
if not profile.motion.preparation_witnesses:
|
||||||
|
motion["motion"].pop("preparation_witnesses") # Historical plan hashes stay unchanged.
|
||||||
|
return cls(profile.quality.training_policy, profile.acquisition.command_capture_mode, tasks,
|
||||||
|
json.dumps(motion, sort_keys=True, separators=(",", ":"), allow_nan=False))
|
||||||
|
|
||||||
|
def task(self, key):
|
||||||
|
return next(task for task in self.tasks if task.task_key == key)
|
||||||
|
|
||||||
|
def as_dict(self, *, legacy_raw=False):
|
||||||
|
version = (("capture_plan_v6_raw_joint" if legacy_raw else "capture_plan_v7_raw_joint") if self.policy == RAW_JOINT else
|
||||||
|
"capture_plan_v5" if self.policy == TASKWISE_TWO_ROUND else
|
||||||
|
"capture_plan_v3" if self.policy == TASKWISE_STAGED else
|
||||||
|
"capture_plan_v2" if self.policy == STAGED else PLAN_VERSION)
|
||||||
|
result = {"version": version, "policy": self.policy,
|
||||||
|
"command_capture_mode": self.command_capture_mode,
|
||||||
|
"motion_contract": json.loads(self.motion_contract_json),
|
||||||
|
"tasks": {t.task_key: {"training_cycles": list(t.training),
|
||||||
|
"holdout_cycle": t.holdout,
|
||||||
|
"command_training_cycles": list(t.command_training),
|
||||||
|
"command_holdout_cycle": t.command_holdout} for t in self.tasks}}
|
||||||
|
if self.policy in STAGED_POLICIES:
|
||||||
|
result["stages"] = [
|
||||||
|
{"name": name, "cycle": cycle, "tasks": [t.task_key for t in self.tasks
|
||||||
|
if cycle in t.motion_cycles]}
|
||||||
|
for name, cycle in (("first_training", 0), ("second_training", 1),
|
||||||
|
("supplement", 2), ("validation", 3))]
|
||||||
|
if self.policy == TASKWISE_STAGED:
|
||||||
|
result["schedule"] = (
|
||||||
|
"first_whole_hand_pass_then_taskwise_second_pass_with_immediate_supplement_then_validation"
|
||||||
|
)
|
||||||
|
if self.policy == TASKWISE_TWO_ROUND:
|
||||||
|
result["schedule"] = "each_task_two_training_rounds_then_independent_validation"
|
||||||
|
result["preparation_recovery"] = {
|
||||||
|
"policy": "dependency_ordered_task_deferral_v2",
|
||||||
|
"maximum_revisits": 1,
|
||||||
|
"preserve_task_round_order": True,
|
||||||
|
"incomplete_capture_blocks_publication": True,
|
||||||
|
}
|
||||||
|
if self.policy == RAW_JOINT:
|
||||||
|
result["schedule"] = "each_task_two_training_rounds_then_independent_validation"
|
||||||
|
result["observation_protocol"] = {
|
||||||
|
"version": LEGACY_RAW_PROTOCOL if legacy_raw else RAW_PROTOCOL,
|
||||||
|
"online_geometry_authorization": False,
|
||||||
|
"maximum_unit_recaptures": 1,
|
||||||
|
"recapture_order": "queue_tail" if legacy_raw else "immediate",
|
||||||
|
"incomplete_capture_blocks_publication": True,
|
||||||
|
}
|
||||||
|
if not legacy_raw:
|
||||||
|
result["observation_protocol"].update(hardware_stall_retries=1,
|
||||||
|
stall_retry_delay_seconds=3.0, motion_progress_scope="direction_active_time")
|
||||||
|
return result
|
||||||
|
|
||||||
|
@property
|
||||||
|
def sha256(self):
|
||||||
|
return evidence_digest(self.as_dict())
|
||||||
|
|
||||||
|
|
||||||
|
def task_for_joint(profile, joint):
|
||||||
|
donor = profile.measurement.transferred_motion_sources.get(joint, joint)
|
||||||
|
secondary = {source: primary for primary, source in profile.measurement.cross_view_sources.items()}
|
||||||
|
donor = secondary.get(donor, donor)
|
||||||
|
for task in profile.motion.tasks:
|
||||||
|
if donor in task.joints:
|
||||||
|
return task
|
||||||
|
raise ValueError(f"capture_plan_joint_has_no_task:{joint}")
|
||||||
|
|
||||||
|
|
||||||
|
def task_partition(profile, task=None, *, joint=None):
|
||||||
|
if joint is not None:
|
||||||
|
task = task_for_joint(profile, joint)
|
||||||
|
key = getattr(task, "key", task)
|
||||||
|
default = (LEGACY_TRAINING[:2] if profile.quality.training_policy in PREPARATION_POLICIES | {RAW_JOINT}
|
||||||
|
else profile.quality.training_cycles)
|
||||||
|
training = tuple(profile.quality.task_training_cycles.get(key, default))
|
||||||
|
holdout = profile.quality.holdout_cycle
|
||||||
|
separate = profile.acquisition.command_capture_mode == "separate"
|
||||||
|
return TaskPartition(key, training, holdout, (holdout + 1,) if separate else training,
|
||||||
|
holdout + 2 if separate else holdout)
|
||||||
|
|
||||||
|
|
||||||
|
def training_cycles(profile, task=None, *, joint=None):
|
||||||
|
return task_partition(profile, task, joint=joint).training
|
||||||
|
|
||||||
|
|
||||||
|
def select_task_training(profile, task_key, cycles):
|
||||||
|
cycles = tuple(cycles)
|
||||||
|
if task_key not in {t.key for t in profile.motion.tasks}:
|
||||||
|
raise ValueError("capture_plan_unknown_task")
|
||||||
|
if cycles not in (LEGACY_TRAINING, LEGACY_TRAINING[:2]):
|
||||||
|
raise ValueError("capture_plan_invalid_training_partition")
|
||||||
|
if profile.quality.training_policy in {TASKWISE_TWO_ROUND, RAW_JOINT}:
|
||||||
|
if cycles != LEGACY_TRAINING[:2]:
|
||||||
|
raise ValueError("taskwise_two_round_cannot_add_training")
|
||||||
|
return profile
|
||||||
|
if (cycles != tuple(profile.quality.training_cycles)
|
||||||
|
and profile.quality.training_policy not in {ADAPTIVE, *STAGED_POLICIES}):
|
||||||
|
raise ValueError("capture_plan_fixed_training_cannot_change")
|
||||||
|
selections = {**profile.quality.task_training_cycles, task_key: cycles}
|
||||||
|
return replace(profile, quality=replace(profile.quality, task_training_cycles=selections))
|
||||||
|
|
||||||
|
|
||||||
|
def configure_training(profile, policy):
|
||||||
|
if policy not in {FIXED, ADAPTIVE, *STAGED_POLICIES, TASKWISE_TWO_ROUND, RAW_JOINT}:
|
||||||
|
raise ValueError("unknown_training_policy")
|
||||||
|
if policy == RAW_JOINT and profile.key.profile_id != 'O30/right/o30_right_18/v1':
|
||||||
|
raise ValueError("raw_joint_capture_requires_o30_profile")
|
||||||
|
if profile.artifacts.output_schema_version in {2, 3} and (
|
||||||
|
tuple(profile.quality.training_cycles) != LEGACY_TRAINING
|
||||||
|
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
|
||||||
|
raise ValueError("production_partition_requires_versioned_training_policy")
|
||||||
|
if policy in {ADAPTIVE, *STAGED_POLICIES, TASKWISE_TWO_ROUND, RAW_JOINT} and (profile.artifacts.output_schema_version < 3
|
||||||
|
or not profile.command_based_release
|
||||||
|
or profile.acquisition.command_capture_mode != "interleaved"
|
||||||
|
or tuple(profile.quality.training_cycles) != LEGACY_TRAINING
|
||||||
|
or profile.quality.holdout_cycle != LEGACY_HOLDOUT):
|
||||||
|
raise ValueError("adaptive_training_requires_measured_interleaved_command_release")
|
||||||
|
# Fixed two-round collection uses the ordinary task-contiguous scheduler.
|
||||||
|
# Its partition is derived by task_partition, not an executed journal
|
||||||
|
# decision. Keep the reusable profile serializable for the launched node.
|
||||||
|
return replace(profile, quality=replace(profile.quality,
|
||||||
|
training_policy=policy, task_training_cycles={}))
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
"""Declared multi-channel paths and durable scan identities.
|
||||||
|
|
||||||
|
Only paths own interpolation. Every observation retains its own motor channel;
|
||||||
|
the scalar path coordinate is never a joint measurement or a new zero.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
import math
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MotionSegment:
|
||||||
|
key: str
|
||||||
|
start_commands: tuple[tuple[int, float], ...]
|
||||||
|
end_commands: tuple[tuple[int, float], ...]
|
||||||
|
joints: tuple[str, ...]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def moving_channels(self):
|
||||||
|
end = dict(self.end_commands)
|
||||||
|
return tuple(i for i, value in self.start_commands if end[i] != value)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def command_index(self):
|
||||||
|
return self.moving_channels[0]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def start(self):
|
||||||
|
return dict(self.start_commands)[self.command_index]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def end(self):
|
||||||
|
return dict(self.end_commands)[self.command_index]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def direction(self):
|
||||||
|
return "increasing" if self.end > self.start else "decreasing"
|
||||||
|
|
||||||
|
def commands_at(self, value, *, quantize=False):
|
||||||
|
fraction = (value-self.start)/(self.end-self.start)
|
||||||
|
if not math.isfinite(fraction) or not -1e-9 <= fraction <= 1+1e-9:
|
||||||
|
raise ValueError("motion segment coordinate outside declared path")
|
||||||
|
end = dict(self.end_commands)
|
||||||
|
values = {i: start+(end[i]-start)*fraction for i, start in self.start_commands}
|
||||||
|
return {i: float(round(v)) for i, v in values.items()} if quantize else values
|
||||||
|
|
||||||
|
|
||||||
|
def task_segments(task):
|
||||||
|
if task.segments:
|
||||||
|
return task.segments
|
||||||
|
return tuple(MotionSegment("", ((task.command_index, start),),
|
||||||
|
((task.command_index, end),), task.joints)
|
||||||
|
for start, end in ((task.start_value, task.end_value), (task.end_value, task.start_value)))
|
||||||
|
|
||||||
|
|
||||||
|
def task_segment(task, key):
|
||||||
|
for segment in task.segments:
|
||||||
|
if segment.key == key:
|
||||||
|
return segment
|
||||||
|
raise ValueError(f"unknown motion segment:{task.key}:{key}")
|
||||||
|
|
||||||
|
|
||||||
|
def task_channels(task):
|
||||||
|
return tuple(sorted({i for segment in task_segments(task) for i in segment.moving_channels}))
|
||||||
|
|
||||||
|
|
||||||
|
def scan_identity(task, cycle, direction, segment_key=""):
|
||||||
|
base = (task, cycle, direction)
|
||||||
|
return (*base, segment_key) if segment_key else base
|
||||||
|
|
||||||
|
|
||||||
|
def record_scan_identity(row):
|
||||||
|
return scan_identity(row.get("task_name"), row.get("cycle"), row.get("direction"),
|
||||||
|
row.get("segment_key", ""))
|
||||||
|
|
||||||
|
|
||||||
|
def segment_metadata(key):
|
||||||
|
return {"segment_key": key} if key else {}
|
||||||
@@ -6,9 +6,10 @@ from dataclasses import dataclass, field
|
|||||||
import math
|
import math
|
||||||
from pathlib import PurePath
|
from pathlib import PurePath
|
||||||
from typing import Mapping
|
from typing import Mapping
|
||||||
|
from .motion_path import MotionSegment
|
||||||
|
|
||||||
|
|
||||||
ACQUISITION_POLICY_VERSION = "unified_engine_v4_dual_mapping"
|
ACQUISITION_POLICY_VERSION = "unified_engine_v8_all_view_images"
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True, order=True)
|
@dataclass(frozen=True, order=True)
|
||||||
@@ -180,6 +181,30 @@ class VisionRigSpec:
|
|||||||
return frozenset(tag.tag_id for view in self.views for tag in view.tags)
|
return frozenset(tag.tag_id for view in self.views for tag in view.tags)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ModelPreparation:
|
||||||
|
"""Independent parent-Tag image authorization at this task's holding pose."""
|
||||||
|
|
||||||
|
joint: str
|
||||||
|
command: tuple[float, ...]
|
||||||
|
approach_commands: tuple[tuple[float, ...], ...]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ObserverPreparationPlan:
|
||||||
|
"""Finite, ordered observation actions; completed actions survive restarts."""
|
||||||
|
|
||||||
|
actions: tuple[ModelPreparation, ...]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ParentReferenceSpec:
|
||||||
|
"""Independent sources for a held Tag pose and its local hinge geometry."""
|
||||||
|
|
||||||
|
pose_joint: str
|
||||||
|
geometry_joint: str
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class TaskSpec:
|
class TaskSpec:
|
||||||
key: str
|
key: str
|
||||||
@@ -198,6 +223,10 @@ class TaskSpec:
|
|||||||
formal_speed: float | None = None
|
formal_speed: float | None = None
|
||||||
preparation_groups: tuple[tuple[int, ...], ...] = ()
|
preparation_groups: tuple[tuple[int, ...], ...] = ()
|
||||||
entry_waypoints: tuple[tuple[tuple[int, float], ...], ...] = ()
|
entry_waypoints: tuple[tuple[tuple[int, float], ...], ...] = ()
|
||||||
|
segments: tuple["MotionSegment", ...] = ()
|
||||||
|
exit_waypoints: tuple[tuple[tuple[int, float], ...], ...] = ()
|
||||||
|
model_preparations: tuple[ModelPreparation, ...] = ()
|
||||||
|
parent_reference: ParentReferenceSpec | None = None
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def start_value(self) -> float:
|
def start_value(self) -> float:
|
||||||
@@ -215,6 +244,25 @@ class ReferenceWaypoint:
|
|||||||
tag_ids_by_view: Mapping[str, tuple[int, ...]]
|
tag_ids_by_view: Mapping[str, tuple[int, ...]]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointZeroSpec:
|
||||||
|
"""A reviewed full-hand pose at which one joint's baseline is visible."""
|
||||||
|
|
||||||
|
task_key: str
|
||||||
|
command: tuple[float, ...]
|
||||||
|
approach_commands: tuple[tuple[float, ...], ...] = ()
|
||||||
|
evidence_scope: str = "arrival"
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class KnownBaselineGeometry:
|
||||||
|
"""Independent CAD angle at the measured baseline, never an SDK prior."""
|
||||||
|
|
||||||
|
cad_angle_rad: float
|
||||||
|
uncertainty_rad: float
|
||||||
|
evidence_source: str
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class MotionPolicy:
|
class MotionPolicy:
|
||||||
"""Reviewed motion tasks and optional safe waypoint sequences."""
|
"""Reviewed motion tasks and optional safe waypoint sequences."""
|
||||||
@@ -227,6 +275,13 @@ class MotionPolicy:
|
|||||||
steady_command_checkpoints: bool = False
|
steady_command_checkpoints: bool = False
|
||||||
return_groups: tuple[tuple[int, ...], ...] = ()
|
return_groups: tuple[tuple[int, ...], ...] = ()
|
||||||
resume_verification_waypoints: tuple[ReferenceWaypoint, ...] = ()
|
resume_verification_waypoints: tuple[ReferenceWaypoint, ...] = ()
|
||||||
|
joint_zero_references: Mapping[str, JointZeroSpec] = field(default_factory=dict)
|
||||||
|
# Target joint -> bounded upstream excitation at its clearance posture.
|
||||||
|
# These images only authorize preparation; they never enter formal fitting.
|
||||||
|
preparation_witnesses: Mapping[str, ModelPreparation] = field(default_factory=dict)
|
||||||
|
# Independent rigid-pair observation; a common ancestor moves while the
|
||||||
|
# recipient hinge stays at zero. Never a replacement for accepted scans.
|
||||||
|
observer_preparations: Mapping[str, ModelPreparation | ObserverPreparationPlan] = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -243,6 +298,7 @@ class MeasurementSpec:
|
|||||||
# must not reject an otherwise clean image/SO(3) trajectory merely because
|
# must not reject an otherwise clean image/SO(3) trajectory merely because
|
||||||
# the hand was placed at a different valid position in the camera view.
|
# the hand was placed at a different valid position in the camera view.
|
||||||
pose_axis_line_required: bool = True
|
pose_axis_line_required: bool = True
|
||||||
|
observer_pose_policy: str = "frozen_parent_v1"
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -255,6 +311,10 @@ class MeasurementPolicy:
|
|||||||
stable_cross_view_cone_bias: bool = False
|
stable_cross_view_cone_bias: bool = False
|
||||||
candidate_selection_tasks: frozenset[str] = frozenset()
|
candidate_selection_tasks: frozenset[str] = frozenset()
|
||||||
input_domain: str = ""
|
input_domain: str = ""
|
||||||
|
# Explicit motion reuse, independent of legacy zero/mimic transfer policy.
|
||||||
|
# Recipients keep their own SDK channel and cannot become measured donors.
|
||||||
|
transferred_motion_sources: Mapping[str, str] = field(default_factory=dict)
|
||||||
|
release_basis: str = "feedback_and_command"
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -280,6 +340,9 @@ class ZeroSolvePolicy:
|
|||||||
transferred_mimic_sources: Mapping[str, str] = field(default_factory=dict)
|
transferred_mimic_sources: Mapping[str, str] = field(default_factory=dict)
|
||||||
# Geometry policy only; no Python callback/import strings are permitted.
|
# Geometry policy only; no Python callback/import strings are permitted.
|
||||||
spatial: Mapping[str, object] = field(default_factory=dict)
|
spatial: Mapping[str, object] = field(default_factory=dict)
|
||||||
|
known_baseline_geometry: Mapping[str, KnownBaselineGeometry] = field(default_factory=dict)
|
||||||
|
# User-accepted CAD q=0 at physical baseline, not a measured geometry prior.
|
||||||
|
cad_zero_assumptions: Mapping[str, str] = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -289,6 +352,9 @@ class QualityPolicy:
|
|||||||
hard_threshold_keys: frozenset[str]
|
hard_threshold_keys: frozenset[str]
|
||||||
retry_metric_scope: Mapping[str, str] = field(default_factory=dict)
|
retry_metric_scope: Mapping[str, str] = field(default_factory=dict)
|
||||||
isolated_holdout: bool = False
|
isolated_holdout: bool = False
|
||||||
|
training_policy: str = "fixed"
|
||||||
|
# Runtime selections are journal-derived, never guessed from missing rows.
|
||||||
|
task_training_cycles: Mapping[str, tuple[int, ...]] = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -309,12 +375,45 @@ class AcquisitionPolicy:
|
|||||||
fixed_reference_minimum_frames: int = 10
|
fixed_reference_minimum_frames: int = 10
|
||||||
fixed_reference_maximum_drift_px: float = 5.0
|
fixed_reference_maximum_drift_px: float = 5.0
|
||||||
fixed_reference_confirmation_frames: int = 10
|
fixed_reference_confirmation_frames: int = 10
|
||||||
|
fixed_reference_mode: str = "tag_pose"
|
||||||
require_endpoint_observations: bool = False
|
require_endpoint_observations: bool = False
|
||||||
endpoint_tolerance_01: float = 2.0 / 255.0
|
endpoint_tolerance_01: float = 2.0 / 255.0
|
||||||
steady_training_nodes: int = 9
|
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_minimum_samples: int = 3
|
||||||
steady_window_seconds: float = 0.2
|
steady_window_seconds: float = 0.2
|
||||||
steady_timeout_seconds: float = 2.0
|
steady_timeout_seconds: float = 2.0
|
||||||
|
joint_zero_timeout_seconds: float = 2.0
|
||||||
|
motion_model_scope: str = "session"
|
||||||
|
# Native position bytes may have endpoint dead zones or a nonlinear servo
|
||||||
|
# response. Only a declared common scale can turn command travel into an
|
||||||
|
# expected encoder displacement before calibration has measured the map.
|
||||||
|
feedback_travel_matches_command: bool = True
|
||||||
|
motion_observation: str = "feedback"
|
||||||
|
visual_stale_seconds: float = 1.0
|
||||||
|
visual_minimum_motion_px: float = 1.0
|
||||||
|
visual_stability_px: float = 0.35
|
||||||
|
visual_zero_tolerance_px: float = 1.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)
|
||||||
|
class RetainedCadScope:
|
||||||
|
"""Explicit partial capture with unmeasured, unchanged CAD ancestors."""
|
||||||
|
|
||||||
|
reason: str
|
||||||
|
root_anchor_joints: tuple[str, ...]
|
||||||
|
orientation_anchor_joint: str
|
||||||
|
directed_base_axis_joints: tuple[str, ...] = ()
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -322,6 +421,7 @@ class ScopePolicy:
|
|||||||
calibrate_joints: Mapping[str, frozenset[str]]
|
calibrate_joints: Mapping[str, frozenset[str]]
|
||||||
frozen_joints: Mapping[str, frozenset[str]]
|
frozen_joints: Mapping[str, frozenset[str]]
|
||||||
default_scope: str = "full"
|
default_scope: str = "full"
|
||||||
|
retained_cad_scopes: Mapping[str, RetainedCadScope] = field(default_factory=dict)
|
||||||
|
|
||||||
def selected_joints(self, scope: str) -> frozenset[str]:
|
def selected_joints(self, scope: str) -> frozenset[str]:
|
||||||
try:
|
try:
|
||||||
@@ -339,6 +439,22 @@ class ArtifactPolicy:
|
|||||||
publication_pointer: str = "latest_passed"
|
publication_pointer: str = "latest_passed"
|
||||||
session_compatibility_tokens: frozenset[str] = frozenset()
|
session_compatibility_tokens: frozenset[str] = frozenset()
|
||||||
publish_corrected_urdf: bool = False
|
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)
|
@dataclass(frozen=True)
|
||||||
@@ -363,6 +479,21 @@ class CalibrationProfile:
|
|||||||
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
|
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
|
||||||
# and mimic_nominal.
|
# and mimic_nominal.
|
||||||
joint_coverage: Mapping[str, str] = field(default_factory=dict)
|
joint_coverage: Mapping[str, str] = field(default_factory=dict)
|
||||||
|
urdf_limit_policies: Mapping[str, JointLimitPolicy] = field(default_factory=dict)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def retained_joints(self) -> frozenset[str]:
|
||||||
|
if self.scope.default_scope in self.scope.retained_cad_scopes:
|
||||||
|
return self.scope.frozen_joints[self.scope.default_scope]
|
||||||
|
return frozenset()
|
||||||
|
|
||||||
|
@property
|
||||||
|
def command_based_release(self) -> bool:
|
||||||
|
return self.measurement.release_basis == "steady_command"
|
||||||
|
|
||||||
|
@property
|
||||||
|
def vision_motion(self) -> bool:
|
||||||
|
return self.acquisition.motion_observation == "vision"
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def curve_input_domain(self) -> str:
|
def curve_input_domain(self) -> str:
|
||||||
@@ -373,11 +504,83 @@ class ProfileValidationError(ValueError):
|
|||||||
"""Raised before hardware startup when a profile is internally unsafe."""
|
"""Raised before hardware startup when a profile is internally unsafe."""
|
||||||
|
|
||||||
|
|
||||||
|
def _transfer_channel(profile: CalibrationProfile, joint: str) -> int | None:
|
||||||
|
"""Resolve a declared channel without inventing a recipient binding."""
|
||||||
|
seen = set()
|
||||||
|
while joint not in profile.command.command_index_by_joint:
|
||||||
|
if joint in seen or joint not in profile.zero.mimic_source_by_joint:
|
||||||
|
return None
|
||||||
|
seen.add(joint)
|
||||||
|
joint = profile.zero.mimic_source_by_joint[joint]
|
||||||
|
return profile.command.command_index_by_joint[joint]
|
||||||
|
|
||||||
|
|
||||||
|
def _validate_observation_strategy(profile: CalibrationProfile, errors: list[str]) -> None:
|
||||||
|
zero, command = profile.zero, profile.command
|
||||||
|
transfers = profile.measurement.transferred_motion_sources
|
||||||
|
observed = {joint for task in profile.motion.tasks if not task.validation_only
|
||||||
|
for joint in task.joints if joint in profile.measurement.measurements
|
||||||
|
and profile.measurement.measurements[joint].view is not None
|
||||||
|
and profile.measurement.measurements[joint].kind != "axis_cross_view_validation"}
|
||||||
|
all_joints = zero.active_joints | zero.passive_joints
|
||||||
|
for target, source in transfers.items():
|
||||||
|
if target not in all_joints or source not in all_joints or source not in observed or target == source:
|
||||||
|
errors.append(f"motion transfer requires a known recipient and directly observed donor:{target}")
|
||||||
|
continue
|
||||||
|
if target in observed or source in transfers:
|
||||||
|
errors.append(f"motion transfer cannot replace observations or chain donors:{target}")
|
||||||
|
if (target in zero.active_joints) != (source in zero.active_joints):
|
||||||
|
errors.append(f"motion transfer must preserve active/passive joint type:{target}")
|
||||||
|
if target in zero.passive_joints:
|
||||||
|
parent = zero.mimic_source_by_joint.get(target)
|
||||||
|
donor_parent = zero.mimic_source_by_joint.get(source)
|
||||||
|
if parent is None or donor_parent is None or transfers.get(parent, parent) != donor_parent:
|
||||||
|
errors.append(f"motion transfer must preserve mimic parent correspondence:{target}")
|
||||||
|
target_channel, source_channel = _transfer_channel(profile, target), _transfer_channel(profile, source)
|
||||||
|
channels = (target_channel, source_channel)
|
||||||
|
values = (command.baseline_values, command.minimum_values,
|
||||||
|
command.maximum_values, command.joint_directions,
|
||||||
|
command.minimum_feedback_values, command.maximum_feedback_values)
|
||||||
|
if any(index is None or index < 0 or index in command.disabled_indices
|
||||||
|
or any(index >= len(row) for row in values) for index in channels):
|
||||||
|
errors.append(f"motion transfer requires valid enabled SDK bindings:{target}")
|
||||||
|
elif any(row[target_channel] != row[source_channel] for row in values):
|
||||||
|
errors.append(f"motion transfer requires equal SDK ranges, baseline and sign:{target}")
|
||||||
|
if target in zero.direct_zero_joints:
|
||||||
|
errors.append(f"motion recipient cannot claim a directly observed zero:{target}")
|
||||||
|
if target in zero.transferred_zero_sources and zero.transferred_zero_sources[target] != source:
|
||||||
|
errors.append(f"motion and zero transfer donors differ:{target}")
|
||||||
|
for joint, reason in zero.cad_zero_assumptions.items():
|
||||||
|
if joint not in all_joints or not isinstance(reason, str) or not reason.strip():
|
||||||
|
errors.append(f"CAD zero assumption requires a known joint and explicit reason:{joint}")
|
||||||
|
if joint in zero.direct_zero_joints or joint in zero.known_baseline_geometry \
|
||||||
|
or joint in zero.transferred_zero_sources:
|
||||||
|
errors.append(f"CAD zero assumption conflicts with another zero source:{joint}")
|
||||||
|
for joint in zero.transferred_zero_sources:
|
||||||
|
if joint in zero.known_baseline_geometry or joint in zero.direct_zero_joints:
|
||||||
|
errors.append(f"transferred zero conflicts with another zero source:{joint}")
|
||||||
|
|
||||||
|
|
||||||
def validate_profile(profile: CalibrationProfile) -> None:
|
def validate_profile(profile: CalibrationProfile) -> None:
|
||||||
"""Hard-check all cross-policy references before hardware is enabled."""
|
"""Hard-check all cross-policy references before hardware is enabled."""
|
||||||
errors: list[str] = []
|
errors: list[str] = []
|
||||||
|
from .capture_plan import configure_training, select_task_training
|
||||||
|
try:
|
||||||
|
configure_training(profile, profile.quality.training_policy)
|
||||||
|
for key, cycles in profile.quality.task_training_cycles.items():
|
||||||
|
select_task_training(profile, key, cycles)
|
||||||
|
except ValueError as error:
|
||||||
|
errors.append(str(error))
|
||||||
command = profile.command
|
command = profile.command
|
||||||
baseline = command.baseline_values
|
baseline = command.baseline_values
|
||||||
|
if profile.measurement.release_basis not in {"feedback_and_command", "steady_command"}:
|
||||||
|
errors.append("unsupported measurement release basis")
|
||||||
|
if profile.command_based_release and (profile.artifacts.output_schema_version != 3
|
||||||
|
or profile.acquisition.command_capture_mode != "interleaved"
|
||||||
|
or profile.measurement.cross_view_sources
|
||||||
|
or profile.measurement.transferred_motion_sources
|
||||||
|
or profile.zero.passive_joints):
|
||||||
|
errors.append("steady command release requires independent active v3 joints and interleaved acquisition")
|
||||||
if not command.names or len(command.names) != len(baseline):
|
if not command.names or len(command.names) != len(baseline):
|
||||||
errors.append("command names and baseline must be non-empty and aligned")
|
errors.append("command names and baseline must be non-empty and aligned")
|
||||||
if len(set(command.names)) != len(command.names):
|
if len(set(command.names)) != len(command.names):
|
||||||
@@ -519,7 +722,68 @@ def validate_profile(profile: CalibrationProfile) -> None:
|
|||||||
errors.append(f"task {task.key} preparation groups must contain unique valid channels")
|
errors.append(f"task {task.key} preparation groups must contain unique valid channels")
|
||||||
if any(task.command_index in group for group in task.preparation_groups[:-1]):
|
if any(task.command_index in group for group in task.preparation_groups[:-1]):
|
||||||
errors.append(f"task {task.key} measured channel must move after avoidance channels")
|
errors.append(f"task {task.key} measured channel must move after avoidance channels")
|
||||||
for waypoint in task.entry_waypoints:
|
segment_keys = set()
|
||||||
|
for segment in task.segments:
|
||||||
|
starts, ends = dict(segment.start_commands), dict(segment.end_commands)
|
||||||
|
if (not segment.key or segment.key in segment_keys or set(starts) != set(ends)
|
||||||
|
or len(starts) != len(segment.start_commands) or len(ends) != len(segment.end_commands)
|
||||||
|
or not starts or not set(starts) <= indices-command.disabled_indices):
|
||||||
|
errors.append(f"task {task.key} has an invalid segment identity or channels")
|
||||||
|
continue
|
||||||
|
segment_keys.add(segment.key)
|
||||||
|
if any(not math.isfinite(v) or not command.minimum_values[i] <= v <= command.maximum_values[i]
|
||||||
|
for i, v in (*segment.start_commands, *segment.end_commands)):
|
||||||
|
errors.append(f"task {task.key} segment commands are outside domain")
|
||||||
|
moving = {i for i in starts if starts[i] != ends[i]}
|
||||||
|
signs = {1 if ends[i] > starts[i] else -1 for i in moving}
|
||||||
|
if not moving or len(signs) != 1:
|
||||||
|
errors.append(f"task {task.key} segment must move channels in one native direction")
|
||||||
|
if (not segment.joints or not set(segment.joints) <= set(task.joints)
|
||||||
|
or any(_transfer_channel(profile, j) not in moving for j in segment.joints)):
|
||||||
|
errors.append(f"task {task.key} segment observations must follow moving channels")
|
||||||
|
if task.segments and segment_keys == {s.key for s in task.segments}:
|
||||||
|
if set().union(*(set(s.joints) for s in task.segments)) != set(task.joints):
|
||||||
|
errors.append(f"task {task.key} segments must cover every observed joint")
|
||||||
|
for previous, following in zip(task.segments, task.segments[1:]):
|
||||||
|
if dict(previous.end_commands) != dict(following.start_commands):
|
||||||
|
errors.append(f"task {task.key} segment path must be continuous")
|
||||||
|
if dict(task.segments[-1].end_commands) != dict(task.segments[0].start_commands):
|
||||||
|
errors.append(f"task {task.key} segment cycle must be closed")
|
||||||
|
if task.parent_reference is not None:
|
||||||
|
reference = task.parent_reference
|
||||||
|
earlier = {name for previous in profile.motion.tasks[:profile.motion.tasks.index(task)]
|
||||||
|
for name in previous.joints}
|
||||||
|
if (task.model_preparations or len(task.joints) != 1
|
||||||
|
or profile.acquisition.motion_model_scope != "task"
|
||||||
|
or not {reference.pose_joint, reference.geometry_joint} <= earlier):
|
||||||
|
errors.append(f"task {task.key} parent reference requires earlier measured sources and task scope")
|
||||||
|
elif all(name in measurement_names for name in (*task.joints, reference.pose_joint, reference.geometry_joint)):
|
||||||
|
child = profile.measurement.measurements[task.joints[0]]
|
||||||
|
if any(profile.measurement.measurements[name].child_role != child.parent_role
|
||||||
|
or profile.measurement.measurements[name].view != child.view
|
||||||
|
for name in (reference.pose_joint, reference.geometry_joint)):
|
||||||
|
errors.append(f"task {task.key} parent reference must identify the same physical parent Tag")
|
||||||
|
for preparation in task.model_preparations:
|
||||||
|
if profile.acquisition.motion_model_scope != "task" or preparation.joint not in measurement_names:
|
||||||
|
errors.append(f"task {task.key} image preparation needs a measured joint and task model scope")
|
||||||
|
continue
|
||||||
|
spec = profile.measurement.measurements[preparation.joint]
|
||||||
|
roots = {profile.measurement.measurements[j].parent_role for j in task.joints if j in measurement_names}
|
||||||
|
if spec.child_role not in roots or preparation.joint in task.joints or not preparation.approach_commands:
|
||||||
|
errors.append(f"task {task.key} image preparation must authorize a parent Tag")
|
||||||
|
for pose in (*preparation.approach_commands, preparation.command):
|
||||||
|
if len(pose) != command.command_count or any(not math.isfinite(v) or not lo <= v <= hi
|
||||||
|
for v, lo, hi in zip(pose, command.minimum_values, command.maximum_values)):
|
||||||
|
errors.append(f"task {task.key} image preparation command outside domain")
|
||||||
|
if preparation.command not in {spec.command for spec in profile.motion.joint_zero_references.values()
|
||||||
|
if spec.task_key == task.key}:
|
||||||
|
errors.append(f"task {task.key} image preparation must finish at its declared zero pose")
|
||||||
|
channel = _transfer_channel(profile, preparation.joint)
|
||||||
|
if any(i != channel and v != preparation.command[i]
|
||||||
|
for pose in preparation.approach_commands for i, v in enumerate(pose)
|
||||||
|
if i < len(preparation.command)):
|
||||||
|
errors.append(f"task {task.key} image preparation must move only its observed parent joint")
|
||||||
|
for waypoint in (*task.entry_waypoints, *task.exit_waypoints):
|
||||||
if not waypoint or len({i for i, _ in waypoint}) != len(waypoint) or any(
|
if not waypoint or len({i for i, _ in waypoint}) != len(waypoint) or any(
|
||||||
i not in indices or i in command.disabled_indices or not math.isfinite(v)
|
i not in indices or i in command.disabled_indices or not math.isfinite(v)
|
||||||
or not command.minimum_values[i] <= v <= command.maximum_values[i] for i, v in waypoint):
|
or not command.minimum_values[i] <= v <= command.maximum_values[i] for i, v in waypoint):
|
||||||
@@ -548,9 +812,52 @@ def validate_profile(profile: CalibrationProfile) -> None:
|
|||||||
errors.append("return groups must contain unique valid channels")
|
errors.append("return groups must contain unique valid channels")
|
||||||
|
|
||||||
zero = profile.zero
|
zero = profile.zero
|
||||||
|
for name, reference in profile.motion.joint_zero_references.items():
|
||||||
|
if reference.evidence_scope not in {"arrival", "path"}:
|
||||||
|
errors.append(f"invalid zero approach evidence scope:{name}")
|
||||||
|
if reference.evidence_scope == "path":
|
||||||
|
if len(reference.approach_commands) < 2:
|
||||||
|
errors.append(f"path evidence needs at least two declared approach poses:{name}")
|
||||||
|
elif all(len(pose) == command.command_count
|
||||||
|
for pose in (reference.command, *reference.approach_commands)):
|
||||||
|
moving = {i for i, value in enumerate(reference.approach_commands[-1])
|
||||||
|
if value != reference.command[i]}
|
||||||
|
if any(value != reference.command[i] for pose in reference.approach_commands
|
||||||
|
for i, value in enumerate(pose) if i not in moving):
|
||||||
|
errors.append(f"path evidence must preserve the final arrival holding pose:{name}")
|
||||||
|
task = next((t for t in profile.motion.tasks if t.key == reference.task_key), None)
|
||||||
|
if task is None or name not in task.joints:
|
||||||
|
errors.append(f"zero reference must precede an observing task:{name}")
|
||||||
|
continue
|
||||||
|
for pose in (reference.command, *reference.approach_commands):
|
||||||
|
if len(pose) != command.command_count or any(not math.isfinite(v) or not lo <= v <= hi
|
||||||
|
for v, lo, hi in zip(pose, command.minimum_values, command.maximum_values)):
|
||||||
|
errors.append(f"zero reference command outside declared domain:{name}")
|
||||||
|
elif any(pose[i] != command.baseline_values[i] for i in command.disabled_indices):
|
||||||
|
errors.append(f"zero reference moves a disabled channel:{name}")
|
||||||
|
if len(reference.command) == command.command_count:
|
||||||
|
channel = _transfer_channel(profile, name)
|
||||||
|
if channel is None:
|
||||||
|
errors.append(f"zero reference has no motor channel:{name}")
|
||||||
|
continue
|
||||||
|
if reference.command[channel] != command.baseline_values[channel]:
|
||||||
|
errors.append(f"zero reference must use joint baseline, not task start:{name}")
|
||||||
|
if profile.artifacts.output_schema_version >= 3 and (not reference.approach_commands
|
||||||
|
or len(reference.approach_commands[-1]) != command.command_count
|
||||||
|
or reference.approach_commands[-1][channel] == reference.command[channel]):
|
||||||
|
errors.append(f"zero reference requires an explicit nonzero final arrival:{name}")
|
||||||
|
first_task = next((t.key for t in profile.motion.tasks if name in t.joints), None)
|
||||||
|
if first_task != reference.task_key:
|
||||||
|
errors.append(f"zero reference must precede first training task:{name}")
|
||||||
|
for name, geometry in zero.known_baseline_geometry.items():
|
||||||
|
if name not in zero.active_joints | zero.passive_joints or not geometry.evidence_source.strip():
|
||||||
|
errors.append(f"known baseline requires a joint and evidence source:{name}")
|
||||||
|
if not math.isfinite(geometry.cad_angle_rad) or not 0 <= geometry.uncertainty_rad <= math.radians(1):
|
||||||
|
errors.append(f"known baseline geometry is invalid or uncertain:{name}")
|
||||||
if zero.active_joints & zero.passive_joints:
|
if zero.active_joints & zero.passive_joints:
|
||||||
errors.append("active and passive joints must be disjoint")
|
errors.append("active and passive joints must be disjoint")
|
||||||
all_joints = zero.active_joints | zero.passive_joints
|
all_joints = zero.active_joints | zero.passive_joints
|
||||||
|
_validate_observation_strategy(profile, errors)
|
||||||
if not zero.active_joints.issubset(command.command_index_by_joint):
|
if not zero.active_joints.issubset(command.command_index_by_joint):
|
||||||
errors.append("every active joint must bind to a command channel")
|
errors.append("every active joint must bind to a command channel")
|
||||||
if not set(zero.direct_zero_joints).issubset(zero.active_joints):
|
if not set(zero.direct_zero_joints).issubset(zero.active_joints):
|
||||||
@@ -610,10 +917,32 @@ def validate_profile(profile: CalibrationProfile) -> None:
|
|||||||
frozen = profile.scope.frozen_joints[name]
|
frozen = profile.scope.frozen_joints[name]
|
||||||
if selected & frozen or selected | frozen != zero.active_joints:
|
if selected & frozen or selected | frozen != zero.active_joints:
|
||||||
errors.append(f"scope {name} must partition all active joints")
|
errors.append(f"scope {name} must partition all active joints")
|
||||||
|
for name, policy in profile.scope.retained_cad_scopes.items():
|
||||||
|
if (name not in scopes or name == "full" or not policy.reason.strip()
|
||||||
|
or not profile.scope.frozen_joints.get(name)):
|
||||||
|
errors.append(f"retained CAD scope must explicitly name excluded active joints:{name}")
|
||||||
|
anchors = set(policy.root_anchor_joints) | {policy.orientation_anchor_joint}
|
||||||
|
if (not policy.root_anchor_joints or not anchors <= profile.scope.calibrate_joints.get(name, ())
|
||||||
|
or not set(policy.directed_base_axis_joints) <= anchors):
|
||||||
|
errors.append(f"partial base pose must use declared measured anchors:{name}")
|
||||||
|
retained = profile.retained_joints
|
||||||
|
if retained:
|
||||||
|
measured = {joint for task in profile.motion.tasks for joint in task.joints}
|
||||||
|
if (not profile.command_based_release or measured != zero.active_joints - retained
|
||||||
|
or retained & (set(profile.measurement.measurements) | set(profile.urdf_authorized_fields)
|
||||||
|
| set(profile.urdf_limit_policies) | set(zero.direct_zero_joints)
|
||||||
|
| set(zero.cad_zero_assumptions) | set(zero.known_baseline_geometry))):
|
||||||
|
errors.append("partial capture must preserve excluded CAD joints without claiming measurements or zeros")
|
||||||
|
|
||||||
artifacts = profile.artifacts
|
artifacts = profile.artifacts
|
||||||
if artifacts.output_schema_version < 1:
|
if artifacts.output_schema_version < 1:
|
||||||
errors.append("artifact schema version must be positive")
|
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 label, filename in (
|
for label, filename in (
|
||||||
("calibration", artifacts.calibration_filename),
|
("calibration", artifacts.calibration_filename),
|
||||||
("corrected URDF", artifacts.corrected_urdf_filename),
|
("corrected URDF", artifacts.corrected_urdf_filename),
|
||||||
@@ -633,9 +962,50 @@ def validate_profile(profile: CalibrationProfile) -> None:
|
|||||||
valid_urdf_fields
|
valid_urdf_fields
|
||||||
):
|
):
|
||||||
errors.append("URDF authorization contains an unsupported field")
|
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
|
acquisition = profile.acquisition
|
||||||
|
if acquisition.motion_observation not in {"feedback", "vision"}:
|
||||||
|
errors.append("motion observation must be feedback or vision")
|
||||||
|
if profile.vision_motion and not profile.command_based_release:
|
||||||
|
errors.append("visual motion requires independent steady-command release")
|
||||||
|
for key in ("visual_stale_seconds", "visual_minimum_motion_px", "visual_stability_px", "visual_zero_tolerance_px"):
|
||||||
|
if not math.isfinite(getattr(acquisition, key)) or getattr(acquisition, key) <= 0:
|
||||||
|
errors.append(f"visual motion threshold must be finite and positive:{key}")
|
||||||
|
if acquisition.visual_minimum_motion_px <= 2*acquisition.visual_stability_px:
|
||||||
|
errors.append("visual motion witness must exceed twice the stability noise")
|
||||||
|
if not isinstance(acquisition.feedback_travel_matches_command, bool):
|
||||||
|
errors.append("feedback travel correspondence must be a boolean")
|
||||||
|
if acquisition.motion_model_scope not in {"session", "task"}:
|
||||||
|
errors.append("motion model scope must be session or task")
|
||||||
|
if acquisition.fixed_reference_mode not in {"tag_pose", "stationary_image"}:
|
||||||
|
errors.append("fixed reference mode must be tag_pose or stationary_image")
|
||||||
|
if acquisition.fixed_reference_mode == "stationary_image" and profile.artifacts.output_schema_version < 3:
|
||||||
|
errors.append("stationary image references require version 3 measured geometry")
|
||||||
|
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:
|
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")
|
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:
|
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")
|
errors.append("steady acquisition must have a finite stability window and <=2 second wait")
|
||||||
if acquisition.policy_version != ACQUISITION_POLICY_VERSION:
|
if acquisition.policy_version != ACQUISITION_POLICY_VERSION:
|
||||||
|
|||||||
@@ -0,0 +1,249 @@
|
|||||||
|
"""Immutable visual baselines, acquired once before each joint's training."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import asdict, dataclass, replace
|
||||||
|
import math
|
||||||
|
import re
|
||||||
|
from typing import Mapping, Sequence
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointZeroPose:
|
||||||
|
quaternion_xyzw: tuple[float, ...]
|
||||||
|
translation_xyz_m: tuple[float, ...]
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_mapping(cls, row):
|
||||||
|
q, t = tuple(row["quaternion_xyzw"]), tuple(row["translation_xyz_m"])
|
||||||
|
if (len(q) != 4 or len(t) != 3 or not np.all(np.isfinite(q+t))
|
||||||
|
or not np.isclose(np.linalg.norm(q), 1, atol=1e-5)):
|
||||||
|
raise ValueError("joint_zero_pose_invalid")
|
||||||
|
return cls(q, t)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointZeroSample:
|
||||||
|
sample_id: str
|
||||||
|
view: str
|
||||||
|
image_stamp_ns: int
|
||||||
|
relative_quaternion_xyzw: tuple[float, ...]
|
||||||
|
relative_translation_xyz_m: tuple[float, ...]
|
||||||
|
command: tuple[float, ...]
|
||||||
|
feedback: tuple[float, ...]
|
||||||
|
arrival_directions: tuple[str, ...]
|
||||||
|
parent_pose_common: JointZeroPose
|
||||||
|
child_pose_common: JointZeroPose
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_row(cls, row, unit):
|
||||||
|
return cls(f"{row['view']}:{int(row['image_stamp_ns'])}", row["view"],
|
||||||
|
int(row["image_stamp_ns"]), tuple(row["relative_quaternion_xyzw"]),
|
||||||
|
tuple(row["relative_translation_xyz_m"]), tuple(row[f"command_vector_{unit}"]),
|
||||||
|
tuple(row.get(f"state_{unit}") or ()), tuple(row.get("command_direction_by_index", ())),
|
||||||
|
JointZeroPose.from_mapping(row["parent_pose_common"]), JointZeroPose.from_mapping(row["child_pose_common"]))
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointZeroBranchReference:
|
||||||
|
"""A moving physical Tag's immutable pre-zero motion authorization."""
|
||||||
|
|
||||||
|
view: str
|
||||||
|
tag_role: str
|
||||||
|
branch_revision: int
|
||||||
|
motion_evidence_id: str
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
if (not self.view or not self.tag_role or type(self.branch_revision) is not int
|
||||||
|
or self.branch_revision < 1 or not isinstance(self.motion_evidence_id, str)
|
||||||
|
or re.fullmatch(r"[0-9a-f]{64}", self.motion_evidence_id) is None):
|
||||||
|
raise ValueError("joint_zero_motion_evidence_invalid")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class JointZeroReference:
|
||||||
|
joint: str
|
||||||
|
task_key: str
|
||||||
|
baseline_command: float
|
||||||
|
channel: int
|
||||||
|
session_epoch: int
|
||||||
|
motion_version: int
|
||||||
|
samples: tuple[JointZeroSample, ...]
|
||||||
|
branch_references: tuple[JointZeroBranchReference, ...] = ()
|
||||||
|
|
||||||
|
def quaternion(self, view):
|
||||||
|
rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in self.samples if s.view == view])
|
||||||
|
return rotations.mean().as_quat()
|
||||||
|
|
||||||
|
def input_reference(self, view, *, feedback=False):
|
||||||
|
values = [(s.feedback if feedback else s.command)[self.channel]
|
||||||
|
for s in self.samples if s.view == view]
|
||||||
|
if not values:
|
||||||
|
raise ValueError(f"joint_zero_missing_view:{self.joint}:{view}")
|
||||||
|
return float(np.median(values))
|
||||||
|
|
||||||
|
def as_record(self):
|
||||||
|
return {"kind": "joint_zero_reference", **asdict(self)}
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_record(cls, row):
|
||||||
|
for sample in row["samples"]:
|
||||||
|
if sample["sample_id"] != f"{sample['view']}:{int(sample['image_stamp_ns'])}":
|
||||||
|
raise ValueError(f"joint_zero_image_identity_invalid:{row['joint']}")
|
||||||
|
samples = tuple(JointZeroSample(**{**s,
|
||||||
|
"parent_pose_common": JointZeroPose.from_mapping(s["parent_pose_common"]),
|
||||||
|
"child_pose_common": JointZeroPose.from_mapping(s["child_pose_common"]),
|
||||||
|
**{key: tuple(s[key]) for key in ("relative_quaternion_xyzw", "relative_translation_xyz_m",
|
||||||
|
"command", "feedback", "arrival_directions")}}) for s in row["samples"])
|
||||||
|
return cls(str(row["joint"]), str(row["task_key"]), float(row["baseline_command"]),
|
||||||
|
int(row["channel"]), int(row["session_epoch"]), int(row["motion_version"]), samples,
|
||||||
|
tuple(JointZeroBranchReference(**item) for item in row.get("branch_references", ())))
|
||||||
|
|
||||||
|
|
||||||
|
def joint_zero_moving_roles(profile, joint):
|
||||||
|
"""Physical roles that require motion evidence for this joint's zero."""
|
||||||
|
names = (joint, profile.measurement.cross_view_sources.get(joint))
|
||||||
|
roles = set()
|
||||||
|
for name in names:
|
||||||
|
if name is None:
|
||||||
|
continue
|
||||||
|
measurement = profile.measurement.measurements[name]
|
||||||
|
for view in profile.vision.views:
|
||||||
|
if view.name == measurement.view:
|
||||||
|
roles.update((view.name, tag.role) for tag in view.tags
|
||||||
|
if tag.role in (measurement.parent_role, measurement.child_role) and not tag.fixed_reference)
|
||||||
|
return roles
|
||||||
|
|
||||||
|
|
||||||
|
def _branch_references(profile, joint, rows):
|
||||||
|
allowed = joint_zero_moving_roles(profile, joint)
|
||||||
|
result, observed = {}, {}
|
||||||
|
for row in rows:
|
||||||
|
for item in row.get("pnp_observation_evidence", {}).values():
|
||||||
|
key = (row["view"], item.get("tag_role"))
|
||||||
|
diagnostics = item.get("candidate_diagnostics", {})
|
||||||
|
verified, identity = diagnostics.get("motion_evidence_verified"), diagnostics.get("motion_evidence_id")
|
||||||
|
observed.setdefault(key, []).append((verified, identity))
|
||||||
|
if not identity and verified is not True:
|
||||||
|
continue # Legacy image confirmation remains readable as legacy.
|
||||||
|
if (key not in allowed or verified is not True or item.get("pose_source") not in {
|
||||||
|
"current_image", "image_constrained_current_image"}
|
||||||
|
or diagnostics.get("observation_stamp_ns") != row["image_stamp_ns"]
|
||||||
|
or diagnostics.get("branch_status") != "tracking"):
|
||||||
|
raise ValueError(f"joint_zero_motion_evidence_unconfirmed:{joint}")
|
||||||
|
reference = JointZeroBranchReference(*key, diagnostics.get("branch_revision"), identity)
|
||||||
|
if key in result and result[key] != reference:
|
||||||
|
raise ValueError(f"joint_zero_motion_evidence_changed:{joint}")
|
||||||
|
result[key] = reference
|
||||||
|
for key, reference in result.items():
|
||||||
|
required = sum(row["view"] == key[0] for row in rows)
|
||||||
|
if len(observed[key]) != required or any(
|
||||||
|
verified is not True or identity != reference.motion_evidence_id for verified, identity in observed[key]):
|
||||||
|
raise ValueError(f"joint_zero_motion_evidence_incomplete:{joint}")
|
||||||
|
return tuple(result[key] for key in sorted(result))
|
||||||
|
|
||||||
|
|
||||||
|
def _validate_branch_bindings(profile, reference):
|
||||||
|
keys = [(item.view, item.tag_role) for item in reference.branch_references]
|
||||||
|
if len(set(keys)) != len(keys) or not set(keys) <= joint_zero_moving_roles(profile, reference.joint):
|
||||||
|
raise ValueError(f"joint_zero_motion_evidence_binding_invalid:{reference.joint}")
|
||||||
|
|
||||||
|
|
||||||
|
def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, session_epoch, motion_version):
|
||||||
|
spec = profile.motion.joint_zero_references[joint]
|
||||||
|
task = next(t for t in profile.motion.tasks if t.key == spec.task_key)
|
||||||
|
from ..fitting.motion_fit import channel_for_joint
|
||||||
|
channel = channel_for_joint(profile, joint)
|
||||||
|
views = {profile.measurement.measurements[joint].view}
|
||||||
|
secondary = profile.measurement.cross_view_sources.get(joint)
|
||||||
|
if secondary:
|
||||||
|
views.add(profile.measurement.measurements[secondary].view)
|
||||||
|
if session_epoch < 1 or motion_version < 1:
|
||||||
|
raise ValueError(f"joint_zero_session_version_invalid:{joint}")
|
||||||
|
# Incomplete windows are checked by the control timer repeatedly. Check
|
||||||
|
# their image coverage before constructing and validating every pose.
|
||||||
|
minimum = profile.acquisition.fixed_reference_minimum_frames
|
||||||
|
for view in views:
|
||||||
|
count = len({int(row["image_stamp_ns"]) for row in rows if row["view"] == view})
|
||||||
|
if count < minimum:
|
||||||
|
measurements = [profile.measurement.measurements[n] for n in (joint, secondary) if n is not None]
|
||||||
|
roles = {role for m in measurements if m.view == view for role in (m.parent_role, m.child_role)}
|
||||||
|
ids = [t.tag_id for v in profile.vision.views if v.name == view for t in v.tags if t.role in roles]
|
||||||
|
raise ValueError(f"joint_zero_samples_missing:{joint}:{view}:required_tags={ids}:{count}/{minimum}")
|
||||||
|
samples = tuple(JointZeroSample.from_row(row, profile.command.unit) for row in rows)
|
||||||
|
for sample in samples:
|
||||||
|
if sample.image_stamp_ns < 1 or sample.sample_id != f"{sample.view}:{sample.image_stamp_ns}":
|
||||||
|
raise ValueError(f"joint_zero_image_identity_invalid:{joint}")
|
||||||
|
if len(sample.relative_quaternion_xyzw) != 4 or not np.isclose(np.linalg.norm(sample.relative_quaternion_xyzw), 1, atol=1e-5):
|
||||||
|
raise ValueError(f"joint_zero_rotation_invalid:{joint}")
|
||||||
|
parent, child = sample.parent_pose_common, sample.child_pose_common
|
||||||
|
rp, rc = Rotation.from_quat(parent.quaternion_xyzw), Rotation.from_quat(child.quaternion_xyzw)
|
||||||
|
if ((Rotation.from_quat(sample.relative_quaternion_xyzw).inv()*rp.inv()*rc).magnitude() > 1e-7
|
||||||
|
or not np.allclose(rp.inv().apply(np.asarray(child.translation_xyz_m)-parent.translation_xyz_m),
|
||||||
|
sample.relative_translation_xyz_m, atol=1e-8, rtol=0)):
|
||||||
|
raise ValueError(f"joint_zero_relative_pose_inconsistent:{joint}")
|
||||||
|
for view in views:
|
||||||
|
selected = [s for s in samples if s.view == view]
|
||||||
|
rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in selected])
|
||||||
|
if np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1):
|
||||||
|
raise ValueError(f"joint_zero_not_stable:{joint}:{view}")
|
||||||
|
identities = [(s.view, s.sample_id) for s in samples]
|
||||||
|
if len(set(identities)) != len(identities):
|
||||||
|
raise ValueError(f"duplicate_joint_zero_image:{joint}")
|
||||||
|
for sample in samples:
|
||||||
|
if sample.view not in views or sample.command != spec.command:
|
||||||
|
raise ValueError(f"joint_zero_pose_differs:{joint}")
|
||||||
|
if not profile.vision_motion and (len(sample.feedback) != profile.command.command_count or not np.all(np.isfinite(sample.feedback))):
|
||||||
|
raise ValueError(f"joint_zero_feedback_invalid:{joint}")
|
||||||
|
if len(sample.arrival_directions) != profile.command.command_count:
|
||||||
|
raise ValueError(f"joint_zero_arrival_missing:{joint}")
|
||||||
|
if spec.approach_commands:
|
||||||
|
before = spec.approach_commands[-1][channel]
|
||||||
|
expected = "increasing" if spec.command[channel] > before else "decreasing"
|
||||||
|
if sample.arrival_directions[channel] != expected:
|
||||||
|
raise ValueError(f"joint_zero_arrival_changed:{joint}")
|
||||||
|
return JointZeroReference(joint, spec.task_key, profile.command.baseline_values[channel],
|
||||||
|
channel, session_epoch, motion_version, samples, _branch_references(profile, joint, rows))
|
||||||
|
|
||||||
|
|
||||||
|
def read_joint_zero_references(profile, records):
|
||||||
|
"""Reject invented, repeated or late baselines in online and offline inputs."""
|
||||||
|
result = {}
|
||||||
|
first_training_stamp = {}
|
||||||
|
for row in records:
|
||||||
|
if row.get("kind") in {"joint_sample", "secondary_joint_sample"}:
|
||||||
|
name = row.get("joint", row.get("observation_joint"))
|
||||||
|
stamp = int(row["image_stamp_ns"])
|
||||||
|
first_training_stamp[name] = min(first_training_stamp.get(name, stamp), stamp)
|
||||||
|
if row.get("kind") != "joint_zero_reference":
|
||||||
|
continue
|
||||||
|
reference = JointZeroReference.from_record(row)
|
||||||
|
name = reference.joint
|
||||||
|
if name in result or name not in profile.motion.joint_zero_references:
|
||||||
|
raise ValueError(f"duplicate_or_unknown_joint_zero:{name}")
|
||||||
|
spec = profile.motion.joint_zero_references[name]
|
||||||
|
if (not 0 <= reference.channel < profile.command.command_count or reference.task_key != spec.task_key
|
||||||
|
or reference.baseline_command != profile.command.baseline_values[reference.channel]):
|
||||||
|
raise ValueError(f"joint_zero_contract_changed:{name}")
|
||||||
|
# Revalidate the complete stored evidence, not just an averaged quaternion.
|
||||||
|
rows = [{"view": s.view, "image_stamp_ns": s.image_stamp_ns,
|
||||||
|
"relative_quaternion_xyzw": s.relative_quaternion_xyzw,
|
||||||
|
"relative_translation_xyz_m": s.relative_translation_xyz_m,
|
||||||
|
f"command_vector_{profile.command.unit}": s.command, f"state_{profile.command.unit}": s.feedback,
|
||||||
|
"command_direction_by_index": s.arrival_directions,
|
||||||
|
"parent_pose_common": asdict(s.parent_pose_common), "child_pose_common": asdict(s.child_pose_common)}
|
||||||
|
for s in reference.samples]
|
||||||
|
checked = build_joint_zero_reference(profile, name, rows,
|
||||||
|
session_epoch=reference.session_epoch, motion_version=reference.motion_version)
|
||||||
|
_validate_branch_bindings(profile, reference)
|
||||||
|
checked = replace(checked, branch_references=reference.branch_references)
|
||||||
|
if checked.channel != reference.channel:
|
||||||
|
raise ValueError(f"joint_zero_channel_changed:{name}")
|
||||||
|
result[name] = checked
|
||||||
|
for name, reference in result.items():
|
||||||
|
if name in first_training_stamp and max(s.image_stamp_ns for s in reference.samples) >= first_training_stamp[name]:
|
||||||
|
raise ValueError(f"joint_zero_not_before_training:{name}")
|
||||||
|
return result
|
||||||
@@ -18,6 +18,9 @@ if TYPE_CHECKING:
|
|||||||
class CalibrationResult:
|
class CalibrationResult:
|
||||||
"""Training-frozen measurements and parameters consumed by every writer.
|
"""Training-frozen measurements and parameters consumed by every writer.
|
||||||
|
|
||||||
|
output_mappings is the primary geometry/replay domain, explicitly labeled
|
||||||
|
by each JointMapping. Optional feedback diagnostics never supply geometry.
|
||||||
|
|
||||||
This is not a publication certificate: the final serialized URDF must
|
This is not a publication certificate: the final serialized URDF must
|
||||||
still pass independent spatial replay and structural authorization.
|
still pass independent spatial replay and structural authorization.
|
||||||
"""
|
"""
|
||||||
@@ -34,6 +37,9 @@ class CalibrationResult:
|
|||||||
command_mappings: Mapping[str, JointMapping] = field(default_factory=dict)
|
command_mappings: Mapping[str, JointMapping] = field(default_factory=dict)
|
||||||
command_holdout_metrics: Mapping = field(default_factory=dict)
|
command_holdout_metrics: Mapping = field(default_factory=dict)
|
||||||
command_applicability: Mapping = field(default_factory=dict)
|
command_applicability: Mapping = field(default_factory=dict)
|
||||||
|
joint_zero_references: Mapping = field(default_factory=dict)
|
||||||
|
mimic_approximations: Mapping = field(default_factory=dict)
|
||||||
|
feedback_diagnostics: Mapping = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -80,7 +86,9 @@ class JointMapping:
|
|||||||
if not math.isfinite(value) or not self.knots[0] <= value <= self.knots[-1]:
|
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}")
|
raise ValueError(f"SDK input outside fitted support:{self.joint}:{value}")
|
||||||
branch = getattr(self, direction + "_rad") if direction else self.angle_rad
|
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)
|
value = math.floor(value + 0.5)
|
||||||
return float(np.interp(value, self.knots, branch))
|
return float(np.interp(value, self.knots, branch))
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,32 @@
|
|||||||
|
"""The declared command nodes used by acquisition and fitting alike."""
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
def command_training_cycles(profile, task=None, *, joint=None):
|
||||||
|
from .capture_plan import task_partition
|
||||||
|
return task_partition(profile, task, joint=joint).command_training
|
||||||
|
|
||||||
|
|
||||||
|
def command_holdout_cycle(profile):
|
||||||
|
from .capture_plan import task_partition
|
||||||
|
return task_partition(profile).command_holdout
|
||||||
|
|
||||||
|
|
||||||
|
def command_nodes(profile, task, *, holdout=False):
|
||||||
|
lo, hi = sorted((task.start_value, task.end_value))
|
||||||
|
train = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
|
||||||
|
train = np.unique(np.r_[train, profile.acquisition.steady_extra_training_nodes.get(task.key, ())])
|
||||||
|
if profile.artifacts.output_schema_version >= 3:
|
||||||
|
from .motion_path import task_channels
|
||||||
|
baselines = [profile.command.baseline_values[i] for i in task_channels(task)]
|
||||||
|
if any(not lo <= baseline <= hi for baseline in baselines):
|
||||||
|
raise ValueError(f"baseline_outside_calibrated_support:{task.key}")
|
||||||
|
train = np.unique(np.r_[train, baselines])
|
||||||
|
if task.segments:
|
||||||
|
boundaries = [v for s in task.segments for i, v in (*s.start_commands, *s.end_commands)]
|
||||||
|
train = np.unique(np.r_[train, boundaries])
|
||||||
|
values = np.r_[lo, (train[:-1]+train[1:])/2, hi] if holdout else train
|
||||||
|
if profile.command.unit == "u8":
|
||||||
|
values = np.floor(values+.5)
|
||||||
|
return tuple(sorted(set(float(v) for v in values)))
|
||||||
@@ -13,9 +13,12 @@ class TaskStatus:
|
|||||||
cycle: int | None = None
|
cycle: int | None = None
|
||||||
cycle_count: int = 4
|
cycle_count: int = 4
|
||||||
direction: str = ""
|
direction: str = ""
|
||||||
|
capture_stage: str = "motion"
|
||||||
required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field(
|
required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field(
|
||||||
default_factory=dict
|
default_factory=dict
|
||||||
)
|
)
|
||||||
|
segment_key: str = ""
|
||||||
|
segment_commands: Mapping[str, tuple[float, float]] = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
|
|||||||
@@ -0,0 +1,22 @@
|
|||||||
|
"""Training-only monotonic projection with an exact, already observed zero."""
|
||||||
|
|
||||||
|
import math
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .curve import isotonic_nonincreasing
|
||||||
|
|
||||||
|
|
||||||
|
def anchored_mean(knots, increasing, decreasing, baseline, sign):
|
||||||
|
x = np.asarray(knots, dtype=float)
|
||||||
|
values = (np.asarray(increasing) + decreasing) / 2
|
||||||
|
indices = np.flatnonzero(np.isclose(x, baseline, atol=1e-9, rtol=0))
|
||||||
|
if len(indices) != 1:
|
||||||
|
raise ValueError("baseline_requires_an_observed_training_knot")
|
||||||
|
i = int(indices[0])
|
||||||
|
oriented = values * sign
|
||||||
|
left = np.minimum(-isotonic_nonincreasing(-oriented[:i]), 0) if i else []
|
||||||
|
right = np.maximum(-isotonic_nonincreasing(-oriented[i+1:]), 0) if i+1 < len(x) else []
|
||||||
|
fitted = np.r_[left, 0.0, right] * sign
|
||||||
|
if np.max(np.abs(fitted-values)) > math.radians(1):
|
||||||
|
raise ValueError("baseline_constraint_inconsistent_with_training")
|
||||||
|
return tuple(float(v) for v in fitted)
|
||||||
+111
-41
@@ -4,7 +4,7 @@ Feedback is NOT a command label and is never the target of this regression.
|
|||||||
The visual reference/axis was frozen by the continuous training acquisition.
|
The visual reference/axis was frozen by the continuous training acquisition.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from dataclasses import replace
|
from dataclasses import asdict, replace
|
||||||
import math
|
import math
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
@@ -12,41 +12,57 @@ from ..domain.result import JointMapping
|
|||||||
from ..urdf.acceptance import angular_metrics
|
from ..urdf.acceptance import angular_metrics
|
||||||
from .curve import isotonic_nonincreasing
|
from .curve import isotonic_nonincreasing
|
||||||
from .spatial import measure_joint_curve_observation
|
from .spatial import measure_joint_curve_observation
|
||||||
|
from ..domain.sampling import command_nodes, command_training_cycles, command_holdout_cycle
|
||||||
|
from ..domain.motion_path import task_channels
|
||||||
|
|
||||||
|
|
||||||
def fit_command_mappings(profile, fit, records_by_joint):
|
def fit_command_mappings(profile, fit, records_by_joint):
|
||||||
|
mappings, metrics, applicability = fit_command_observations(profile, fit.curves, records_by_joint,
|
||||||
|
channels={name: mapping.motor_index for name, mapping in fit.output_mappings.items()},
|
||||||
|
reference_inputs=fit.reference_inputs, zero_references=fit.joint_zero_references)
|
||||||
|
return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics,
|
||||||
|
command_applicability=applicability)
|
||||||
|
|
||||||
|
|
||||||
|
def fit_command_training(profile, curves, records_by_joint, *, channels, reference_inputs, zero_references):
|
||||||
|
"""Fit stable command labels to angles measured in frozen visual frames."""
|
||||||
mappings, metrics, applicability = {}, {}, {}
|
mappings, metrics, applicability = {}, {}, {}
|
||||||
for joint in sorted(fit.output_mappings):
|
holdout_cycle = command_holdout_cycle(profile)
|
||||||
donor = profile.zero.transferred_zero_sources.get(joint, joint)
|
for joint in sorted(channels):
|
||||||
|
donor = joint if profile.artifacts.output_schema_version >= 3 else profile.zero.transferred_zero_sources.get(joint, joint)
|
||||||
rows = records_by_joint.get(donor, ())
|
rows = records_by_joint.get(donor, ())
|
||||||
if not rows or any(r.get("sample_phase") != "steady" for r in rows):
|
if not rows or any(r.get("sample_phase") != "steady" for r in rows):
|
||||||
raise ValueError(f"steady_command_observations_missing:{joint}")
|
raise ValueError(f"steady_command_observations_missing:{joint}")
|
||||||
curve = fit.curves[donor]
|
training_cycles = command_training_cycles(profile, joint=donor)
|
||||||
gauge = float(np.interp(fit.reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad))
|
if any(r["cycle"] not in training_cycles for r in rows):
|
||||||
|
raise ValueError("command_training_received_nontraining_data")
|
||||||
|
curve = curves[donor]
|
||||||
|
gauge = 0.0
|
||||||
# Byte curves already carry their shared coordinate reference. Native
|
# Byte curves already carry their shared coordinate reference. Native
|
||||||
# curves retain the arbitrary observed Tag reference in fit.curves.
|
# curves retain the arbitrary observed Tag reference in fit.curves.
|
||||||
if profile.command.unit == "u8":
|
if profile.command.unit != "u8" and profile.artifacts.output_schema_version < 3:
|
||||||
gauge = 0.0
|
gauge = float(np.interp(reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad))
|
||||||
observed = {r["sample_id"]: measure_joint_curve_observation(curve,
|
observed = {r["sample_id"]: measure_joint_curve_observation(curve,
|
||||||
quaternion_xyzw=r.get("relative_quaternion_xyzw"),
|
quaternion_xyzw=r.get("relative_quaternion_xyzw"),
|
||||||
image_relative_xy_px=r.get("image_relative_xy_px"))-gauge for r in rows}
|
image_relative_xy_px=r.get("image_relative_xy_px"))-gauge for r in rows}
|
||||||
if len(observed) != len(rows):
|
if len(observed) != len(rows):
|
||||||
raise ValueError("duplicate steady visual image")
|
raise ValueError("duplicate steady visual image")
|
||||||
channel = fit.output_mappings[joint].motor_index
|
channel = channels[joint]
|
||||||
source_channel = fit.output_mappings[donor].motor_index
|
source_channel = channels[donor]
|
||||||
branches = {}
|
branches = {}
|
||||||
|
maximum_correction = 0.0
|
||||||
|
directional = joint in profile.artifacts.directional_command_joints
|
||||||
task = next(t for t in profile.motion.tasks if donor in t.joints)
|
task = next(t for t in profile.motion.tasks if donor in t.joints)
|
||||||
lo, hi = sorted((task.start_value, task.end_value))
|
from .command_sampling import declared_training_nodes
|
||||||
train_knots = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
|
knots = declared_training_nodes(profile, task, rows)
|
||||||
if profile.command.unit == "u8":
|
|
||||||
train_knots = np.floor(train_knots+0.5)
|
|
||||||
knots = tuple(sorted(set(float(v) for v in train_knots)))
|
|
||||||
sign = profile.command.joint_directions[source_channel]
|
sign = profile.command.joint_directions[source_channel]
|
||||||
|
if profile.artifacts.output_schema_version >= 3 and not profile.command_based_release:
|
||||||
|
sign = 1 if curve.angle_rad[-1] > curve.angle_rad[0] else -1
|
||||||
for direction in ("increasing", "decreasing"):
|
for direction in ("increasing", "decreasing"):
|
||||||
values = []
|
values = []
|
||||||
for knot in knots:
|
for knot in knots:
|
||||||
medians = []
|
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
|
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)]
|
and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9)]
|
||||||
if len(selected) < profile.acquisition.steady_minimum_samples:
|
if len(selected) < profile.acquisition.steady_minimum_samples:
|
||||||
@@ -58,41 +74,95 @@ def fit_command_mappings(profile, fit, records_by_joint):
|
|||||||
values.append(float(np.median(medians)))
|
values.append(float(np.median(medians)))
|
||||||
values = np.asarray(values)
|
values = np.asarray(values)
|
||||||
projected = -sign*isotonic_nonincreasing(-sign*values)
|
projected = -sign*isotonic_nonincreasing(-sign*values)
|
||||||
if np.max(np.abs(projected-values)) > math.radians(1):
|
maximum_correction = max(maximum_correction, float(np.max(np.abs(projected-values))))
|
||||||
|
if maximum_correction > math.radians(1):
|
||||||
raise ValueError(f"steady_command_not_monotonic:{joint}:{direction}")
|
raise ValueError(f"steady_command_not_monotonic:{joint}:{direction}")
|
||||||
branches[direction] = tuple(float(v) for v in projected)
|
branches[direction] = tuple(float(v) for v in projected)
|
||||||
|
if directional:
|
||||||
|
from .anchored_mapping import anchored_mean
|
||||||
|
branches = {direction: anchored_mean(knots, values, values,
|
||||||
|
zero_references[joint].baseline_command, sign)
|
||||||
|
for direction, values in branches.items()}
|
||||||
mean = tuple((a+b)/2 for a, b in zip(branches["increasing"], branches["decreasing"]))
|
mean = tuple((a+b)/2 for a, b in zip(branches["increasing"], branches["decreasing"]))
|
||||||
|
if profile.artifacts.output_schema_version >= 3:
|
||||||
|
from .anchored_mapping import anchored_mean
|
||||||
|
mean = anchored_mean(knots, branches["increasing"], branches["decreasing"],
|
||||||
|
zero_references[joint].baseline_command, sign)
|
||||||
mapping = JointMapping(joint, channel, f"command_{profile.command.unit}", knots,
|
mapping = JointMapping(joint, channel, f"command_{profile.command.unit}", knots,
|
||||||
mean, branches["increasing"], branches["decreasing"], donor if donor != joint else None)
|
mean, branches["increasing"], branches["decreasing"], donor if donor != joint else None)
|
||||||
validation = [r for r in rows if r["cycle"] == 3]
|
|
||||||
expected = np.r_[lo, (np.asarray(knots[:-1])+knots[1:])/2, hi]
|
|
||||||
if profile.command.unit == "u8":
|
|
||||||
# Acquisition interleaves the unrounded grid, then encodes bytes.
|
|
||||||
grid = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
|
|
||||||
expected = np.floor(np.r_[lo, (grid[:-1]+grid[1:])/2, hi]+0.5)
|
|
||||||
for direction in ("increasing", "decreasing"):
|
|
||||||
for knot in set(expected):
|
|
||||||
if sum(r["direction"] == direction and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9)
|
|
||||||
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}"]), r["direction"])
|
|
||||||
- observed[r["sample_id"]] for r in validation]
|
|
||||||
quality = angular_metrics(errors)
|
|
||||||
if not quality.passed:
|
|
||||||
raise ValueError(f"steady_command_holdout_failed:{joint}:{quality}")
|
|
||||||
mappings[joint] = mapping
|
mappings[joint] = mapping
|
||||||
from dataclasses import asdict
|
metrics[joint] = {"maximum_monotonic_correction_rad": maximum_correction,
|
||||||
metrics[joint] = {**asdict(quality), "independently_measured": donor == joint,
|
"independently_measured": donor == joint, "source_joint": donor,
|
||||||
"source_joint": donor, "training_cycles": [0, 1, 2], "holdout_cycle": 3}
|
"training_cycles": list(training_cycles), "holdout_cycle": holdout_cycle,
|
||||||
training = [r for r in rows if r["cycle"] in {0, 1, 2}]
|
"training_sample_ids": sorted(r["sample_id"] for r in rows)}
|
||||||
|
training = [r for r in rows if r["cycle"] in training_cycles]
|
||||||
vectors = np.asarray([r[f"command_vector_{profile.command.unit}"] for r in training])
|
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)
|
held = {str(i): float(np.median(vectors[:, i])) for i in range(profile.command.command_count)
|
||||||
if i != source_channel}
|
if i not in task_channels(task)}
|
||||||
for i, value in held.items():
|
for i, value in held.items():
|
||||||
if np.max(np.abs(vectors[:, int(i)]-value)) > 1e-8:
|
if np.max(np.abs(vectors[:, int(i)]-value)) > 1e-8:
|
||||||
raise ValueError(f"scan_changed_multiple_command_channels:{donor}:{i}")
|
raise ValueError(f"scan_changed_multiple_command_channels:{donor}:{i}")
|
||||||
applicability[joint] = {"task": task.key, "held_command_values": held,
|
applicability[joint] = {"task": task.key, "held_command_values": held,
|
||||||
"scope": "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False,
|
"scope": "declared_multi_channel_path" if task.segments else "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False,
|
||||||
"branch_initialization": "full_range_endpoint_reset"}
|
"branch_initialization": "recorded_baseline_then_endpoint_resets" if task.segments else "full_range_endpoint_reset"}
|
||||||
return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics,
|
if task.segments:
|
||||||
command_applicability=applicability)
|
applicability[joint]["motion_segments"] = [asdict(s) for s in task.segments]
|
||||||
|
if directional:
|
||||||
|
applicability[joint].update(command_mapping="directional", direction_source="raw_sdk_input",
|
||||||
|
interior_reversals_validated=False)
|
||||||
|
return mappings, metrics, applicability
|
||||||
|
|
||||||
|
|
||||||
|
def validate_command_training(profile, curves, mappings, metrics, records_by_joint, *, reference_inputs):
|
||||||
|
"""Evaluate independent images against already frozen frames and mappings."""
|
||||||
|
holdout_cycle = command_holdout_cycle(profile)
|
||||||
|
accepted = {}
|
||||||
|
for joint, mapping in mappings.items():
|
||||||
|
donor = metrics[joint]["source_joint"]
|
||||||
|
curve = curves[donor]
|
||||||
|
rows = records_by_joint.get(donor, ())
|
||||||
|
if not rows or any(r["cycle"] != holdout_cycle or r.get("sample_phase") != "steady" for r in rows):
|
||||||
|
raise ValueError(f"steady_holdout_incomplete:{joint}")
|
||||||
|
ids = [r["sample_id"] for r in rows]
|
||||||
|
trained = set(metrics[joint]["training_sample_ids"]) | set(curve.circle.get("training_sample_ids", ()))
|
||||||
|
if len(ids) != len(set(ids)) or trained.intersection(ids):
|
||||||
|
raise ValueError("command_holdout_image_identity_overlaps_training")
|
||||||
|
task = next(t for t in profile.motion.tasks if donor in t.joints)
|
||||||
|
for direction in ("increasing", "decreasing"):
|
||||||
|
for knot in command_nodes(profile, task, holdout=True):
|
||||||
|
if sum(r["direction"] == direction and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9)
|
||||||
|
for r in rows) < profile.acquisition.steady_minimum_samples:
|
||||||
|
raise ValueError(f"steady_holdout_incomplete:{joint}:{direction}:{knot}")
|
||||||
|
gauge = (float(np.interp(reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad))
|
||||||
|
if profile.command.unit != "u8" and profile.artifacts.output_schema_version < 3 else 0.)
|
||||||
|
directional = joint in profile.artifacts.directional_command_joints
|
||||||
|
errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]),
|
||||||
|
r["direction"] if directional or profile.artifacts.output_schema_version < 3 else "")
|
||||||
|
- measure_joint_curve_observation(curve, quaternion_xyzw=r.get("relative_quaternion_xyzw"),
|
||||||
|
image_relative_xy_px=r.get("image_relative_xy_px")) + gauge for r in rows]
|
||||||
|
quality = angular_metrics(errors)
|
||||||
|
if not quality.passed:
|
||||||
|
raise ValueError(f"steady_command_holdout_failed:{joint}:{quality}")
|
||||||
|
accepted[joint] = {**metrics[joint], **asdict(quality)}
|
||||||
|
if profile.artifacts.output_schema_version >= 3:
|
||||||
|
branches = {direction: angular_metrics([error for error, row in zip(errors, rows)
|
||||||
|
if row["direction"] == direction]) for direction in ("increasing", "decreasing")}
|
||||||
|
if not all(value.passed for value in branches.values()):
|
||||||
|
raise ValueError(f"command_curve_direction_holdout_failed:{joint}")
|
||||||
|
accepted[joint]["by_direction"] = {key: asdict(value) for key, value in branches.items()}
|
||||||
|
return accepted
|
||||||
|
|
||||||
|
|
||||||
|
def fit_command_observations(profile, curves, records_by_joint, *, channels, reference_inputs, zero_references):
|
||||||
|
training, validation = {}, {}
|
||||||
|
for name, rows in records_by_joint.items():
|
||||||
|
cycles = command_training_cycles(profile, joint=name)
|
||||||
|
holdout = command_holdout_cycle(profile)
|
||||||
|
if any(row["cycle"] not in (*cycles, holdout) for row in rows):
|
||||||
|
raise ValueError("command_observation_outside_capture_plan")
|
||||||
|
training[name] = [row for row in rows if row["cycle"] in cycles]
|
||||||
|
validation[name] = [row for row in rows if row["cycle"] == holdout]
|
||||||
|
mappings, metrics, applicability = fit_command_training(profile, curves, training,
|
||||||
|
channels=channels, reference_inputs=reference_inputs, zero_references=zero_references)
|
||||||
|
return mappings, validate_command_training(profile, curves, mappings, metrics, validation,
|
||||||
|
reference_inputs=reference_inputs), applicability
|
||||||
|
|||||||
@@ -0,0 +1,114 @@
|
|||||||
|
"""Command-certified motion with an independent, non-blocking feedback diagnostic.
|
||||||
|
|
||||||
|
Moving images determine only a hinge frame. Stable commanded poses train the
|
||||||
|
control curves. A fourth cycle never contributes to either training stage.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import asdict, dataclass, replace
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from ..domain.measurement import JointCurveFit
|
||||||
|
from ..urdf.acceptance import angular_metrics
|
||||||
|
from .command_mapping import fit_command_training as train_commands, validate_command_training
|
||||||
|
from ..domain.capture_plan import training_cycles, LEGACY_TRAINING, LEGACY_HOLDOUT
|
||||||
|
from ..domain.sampling import command_training_cycles, command_holdout_cycle
|
||||||
|
from .motion_fit import channel_for_joint, joint_input_direction
|
||||||
|
from .observed_motion import image_identity
|
||||||
|
from .rotation_curve import RotationObservation, fit_rotation_frame, fit_rotation_curve
|
||||||
|
|
||||||
|
|
||||||
|
def rotation_observations(rows, domain):
|
||||||
|
return [RotationObservation(image_identity(row), float(row[domain]),
|
||||||
|
tuple(row['relative_quaternion_xyzw']), int(row['cycle']), str(row['direction'])) for row in rows]
|
||||||
|
|
||||||
|
|
||||||
|
def feedback_diagnostic(rows, *, domain, sign, reference, cycles=LEGACY_TRAINING, holdout_cycle=LEGACY_HOLDOUT):
|
||||||
|
result = {'required_for_release': False, 'input_domain': domain, 'training_cycles': list(cycles),
|
||||||
|
'holdout_cycle': holdout_cycle, 'extrapolation': 'reject'}
|
||||||
|
try:
|
||||||
|
if any(row.get(domain) is None or not np.isfinite(row[domain]) for row in rows):
|
||||||
|
raise ValueError('position_feedback_missing_or_invalid')
|
||||||
|
observations = rotation_observations(rows, domain)
|
||||||
|
curve = fit_rotation_curve([r for r in observations if r.cycle in cycles],
|
||||||
|
input_domain=domain, sdk_to_joint_direction=sign, reference_xyzw=reference,
|
||||||
|
training_cycles=cycles, holdout_cycle=holdout_cycle)
|
||||||
|
held = [r for r in observations if r.cycle == holdout_cycle]
|
||||||
|
supported = [r for r in held if curve.knots[0] <= r.input_value <= curve.knots[-1]]
|
||||||
|
outside = [r for r in held if not curve.knots[0] <= r.input_value <= curve.knots[-1]]
|
||||||
|
quality = angular_metrics(curve.holdout_errors(supported)) if supported else None
|
||||||
|
metrics = {**asdict(quality), 'passed': quality.passed} if quality else None
|
||||||
|
result.update(status='passed' if held and not outside and metrics and metrics['passed'] else 'incomplete_or_inaccurate',
|
||||||
|
training_support=[curve.knots[0], curve.knots[-1]], holdout_samples=len(held),
|
||||||
|
outside_training_support=len(outside), outside_image_ids=[r.sample_id for r in outside],
|
||||||
|
supported_holdout_metrics=metrics, mapping={**asdict(curve),
|
||||||
|
"training_sample_ids": sorted(curve.training_sample_ids)})
|
||||||
|
except (ValueError, TypeError) as error:
|
||||||
|
result.update(status='unavailable', reason=str(error))
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CommandMotionFit:
|
||||||
|
observed_curves: dict
|
||||||
|
coordinate_curves: dict
|
||||||
|
mappings: dict
|
||||||
|
command_metrics: dict
|
||||||
|
applicability: dict
|
||||||
|
reference_inputs: dict
|
||||||
|
feedback_diagnostics: dict
|
||||||
|
|
||||||
|
@property
|
||||||
|
def holdout_errors_rad(self):
|
||||||
|
# Command errors are retained with their per-direction metrics.
|
||||||
|
return {}
|
||||||
|
|
||||||
|
@property
|
||||||
|
def cross_view_metrics(self):
|
||||||
|
return {}
|
||||||
|
|
||||||
|
|
||||||
|
def fit_command_training(profile, source_model, sweep, steady, references):
|
||||||
|
"""Training-only motion API. Passing a holdout image is a contract error."""
|
||||||
|
if set(sweep) != set(steady) or set(sweep) != set(references):
|
||||||
|
raise ValueError('command_motion_observation_coverage_differs')
|
||||||
|
frames = {}
|
||||||
|
channels = {name: channel_for_joint(profile, name) for name in sweep}
|
||||||
|
for name, rows in sweep.items():
|
||||||
|
cycles = training_cycles(profile, joint=name)
|
||||||
|
if any(row['cycle'] not in cycles for row in rows):
|
||||||
|
raise ValueError('motion_training_received_nontraining_data')
|
||||||
|
view = profile.measurement.measurements[name].view
|
||||||
|
frames[name] = fit_rotation_frame(rotation_observations(rows, f'command_{profile.command.unit}'),
|
||||||
|
sdk_to_joint_direction=joint_input_direction(profile, source_model, name),
|
||||||
|
reference_xyzw=references[name].quaternion(view), training_cycles=cycles)
|
||||||
|
inputs = {name: references[name].baseline_command for name in frames}
|
||||||
|
mappings, metrics, applicability = train_commands(profile, frames, steady,
|
||||||
|
channels=channels, reference_inputs=inputs, zero_references=references)
|
||||||
|
curves = {}
|
||||||
|
for name, mapping in mappings.items():
|
||||||
|
curves[name] = JointCurveFit(mapping.angle_rad, mapping.decreasing_rad, mapping.increasing_rad,
|
||||||
|
{**frames[name].circle, 'input_domain': mapping.input_domain, 'input_knots': mapping.knots,
|
||||||
|
'mapping_training_sample_ids': metrics[name]['training_sample_ids']},
|
||||||
|
metrics[name]["maximum_monotonic_correction_rad"],
|
||||||
|
float(np.max(np.abs(np.array(mapping.increasing_rad)-mapping.decreasing_rad))), {})
|
||||||
|
return CommandMotionFit(curves, curves, mappings, metrics, applicability, inputs, {})
|
||||||
|
|
||||||
|
|
||||||
|
def fit_command_motion(profile, source_model, sweep, steady, references, *, frozen_motion=None):
|
||||||
|
train_sweep = {name: [r for r in rows if r['cycle'] in training_cycles(profile, joint=name)]
|
||||||
|
for name, rows in sweep.items()}
|
||||||
|
train_steady = {name: [r for r in rows if r['cycle'] in command_training_cycles(profile, joint=name)]
|
||||||
|
for name, rows in steady.items()}
|
||||||
|
motion = frozen_motion or fit_command_training(profile, source_model, train_sweep, train_steady, references)
|
||||||
|
held = {name: [r for r in rows if r['cycle'] == command_holdout_cycle(profile)]
|
||||||
|
for name, rows in steady.items()}
|
||||||
|
metrics = validate_command_training(profile, motion.coordinate_curves,
|
||||||
|
motion.mappings, motion.command_metrics, held, reference_inputs=motion.reference_inputs)
|
||||||
|
diagnostics = {}
|
||||||
|
for name, rows in sweep.items():
|
||||||
|
view = profile.measurement.measurements[name].view
|
||||||
|
diagnostics[name] = feedback_diagnostic(rows,
|
||||||
|
domain=f'feedback_{profile.command.unit}', sign=joint_input_direction(profile, source_model, name),
|
||||||
|
reference=references[name].quaternion(view), cycles=training_cycles(profile, joint=name),
|
||||||
|
holdout_cycle=profile.quality.holdout_cycle)
|
||||||
|
return replace(motion, command_metrics=metrics, feedback_diagnostics=diagnostics)
|
||||||
@@ -0,0 +1,149 @@
|
|||||||
|
"""Training-only reduction of static command knots with immutable evidence.
|
||||||
|
|
||||||
|
The continuous curve chooses where to stop, never supplies a static command
|
||||||
|
angle. Every retained static knot is measured afresh in both directions. The
|
||||||
|
original independent validation grid and all publication gates remain intact.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from ..domain.sampling import command_nodes
|
||||||
|
from ..domain.profile import observation_streams
|
||||||
|
from .rotation_curve import RotationObservation, fit_rotation_curve
|
||||||
|
from ..domain.motion_path import record_scan_identity, segment_metadata
|
||||||
|
from ..domain.capture_plan import training_cycles
|
||||||
|
|
||||||
|
|
||||||
|
SAMPLING_POLICY = "training_shape_static_knots_v1"
|
||||||
|
MAXIMUM_REMOVAL_ERROR_RAD = math.radians(.25)
|
||||||
|
|
||||||
|
|
||||||
|
def digest(value):
|
||||||
|
return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
def training_sources(profile, task, records):
|
||||||
|
cycles = training_cycles(profile, task)
|
||||||
|
streams = {(field, name, spec.view) for field, name, spec in observation_streams(profile, task)}
|
||||||
|
latest = {}
|
||||||
|
for row in records:
|
||||||
|
if row.get("task_name") == task.key and row.get("cycle") in cycles and row.get("direction"):
|
||||||
|
key = record_scan_identity(row)
|
||||||
|
latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1)))
|
||||||
|
result = []
|
||||||
|
for row in records:
|
||||||
|
if (row.get("task_name") != task.key or row.get("cycle") not in cycles
|
||||||
|
or row.get("sample_phase", "sweep") != "sweep" or "relative_quaternion_xyzw" not in row):
|
||||||
|
continue
|
||||||
|
if int(row.get("attempt", 1)) != latest.get(record_scan_identity(row)):
|
||||||
|
continue
|
||||||
|
for field, name, view in sorted(streams):
|
||||||
|
if row.get(field) == name and row.get("view") == view:
|
||||||
|
result.append({"field": field, "joint": name, "view": view, "cycle": row["cycle"],
|
||||||
|
"direction": row["direction"], "image_stamp_ns": row["image_stamp_ns"],
|
||||||
|
**segment_metadata(row.get("segment_key", "")),
|
||||||
|
"input": float(row[f"{'command' if profile.command_based_release else 'feedback'}_{profile.command.unit}"]),
|
||||||
|
"quaternion": list(row["relative_quaternion_xyzw"])})
|
||||||
|
return sorted(result, key=lambda row: (row["field"], row["joint"], row["view"], row["image_stamp_ns"]))
|
||||||
|
|
||||||
|
|
||||||
|
def reduced_knots(grid, curves, mandatory):
|
||||||
|
"""Shortest path through the original grid; each omitted knot is bounded."""
|
||||||
|
grid, values = np.asarray(grid), np.asarray(curves)
|
||||||
|
paths = {0: (0,)}
|
||||||
|
for end in range(1, len(grid)):
|
||||||
|
candidates = []
|
||||||
|
for start, path in paths.items():
|
||||||
|
if any(start < index < end for index in mandatory):
|
||||||
|
continue
|
||||||
|
fraction = (grid[start:end+1]-grid[start])/(grid[end]-grid[start])
|
||||||
|
prediction = values[:, start, None] + (values[:, end]-values[:, start])[:, None]*fraction
|
||||||
|
if np.max(np.abs(prediction-values[:, start:end+1])) <= MAXIMUM_REMOVAL_ERROR_RAD:
|
||||||
|
candidates.append((*path, end))
|
||||||
|
paths[end] = min(candidates, key=lambda path: (len(path), path))
|
||||||
|
return tuple(float(grid[index]) for index in paths[len(grid)-1])
|
||||||
|
|
||||||
|
|
||||||
|
def build_sampling_plan(profile, task, records):
|
||||||
|
sources = training_sources(profile, task, records)
|
||||||
|
grid = command_nodes(profile, task)
|
||||||
|
knots, reason = grid, "full_grid_insufficient_training_shape"
|
||||||
|
curves = []
|
||||||
|
if task.segments:
|
||||||
|
reason = "segmented_motion_preserves_full_per_joint_grid"
|
||||||
|
else:
|
||||||
|
try:
|
||||||
|
for field, name, spec in observation_streams(profile, task):
|
||||||
|
rows = [row for row in sources if (row["field"], row["joint"], row["view"]) == (field, name, spec.view)]
|
||||||
|
observations = [RotationObservation(f"{row['view']}:{row['image_stamp_ns']}", row["input"],
|
||||||
|
tuple(row["quaternion"]), row["cycle"], row["direction"]) for row in rows]
|
||||||
|
fitted = fit_rotation_curve(observations, input_domain=f"{'command' if profile.command_based_release else 'feedback'}_{profile.command.unit}",
|
||||||
|
sdk_to_joint_direction=profile.command.joint_directions[task.command_index],
|
||||||
|
training_cycles=training_cycles(profile, task), holdout_cycle=profile.quality.holdout_cycle)
|
||||||
|
if fitted.maximum_monotonic_correction_rad > MAXIMUM_REMOVAL_ERROR_RAD:
|
||||||
|
raise ValueError("training_shape_noisy")
|
||||||
|
for branch in (fitted.increasing_rad, fitted.decreasing_rad):
|
||||||
|
curves.append(np.interp(grid, fitted.knots, branch))
|
||||||
|
mandatory_values = {grid[0], grid[-1], grid[len(grid)//2],
|
||||||
|
profile.command.baseline_values[task.command_index],
|
||||||
|
*profile.acquisition.steady_extra_training_nodes.get(task.key, ())}
|
||||||
|
mandatory = {i for i, value in enumerate(grid) if value in mandatory_values}
|
||||||
|
knots = reduced_knots(grid, curves, mandatory)
|
||||||
|
reason = "training_shape_bounded" if len(knots) < len(grid) else "full_grid_required_by_training_shape"
|
||||||
|
except (ValueError, KeyError, TypeError, np.linalg.LinAlgError) as error:
|
||||||
|
reason = "full_grid:" + str(error)
|
||||||
|
plan = {"kind": "command_sampling_plan", "policy": SAMPLING_POLICY, "task_name": task.key,
|
||||||
|
"training_nodes": list(knots), "validation_nodes": list(command_nodes(profile, task, holdout=True)),
|
||||||
|
"source_sha256": digest(sources), "source_count": len(sources), "training_cycles": list(training_cycles(profile, task)),
|
||||||
|
"maximum_removal_error_rad": MAXIMUM_REMOVAL_ERROR_RAD, "reason": reason}
|
||||||
|
return {**plan, "plan_sha256": digest(plan)}
|
||||||
|
|
||||||
|
|
||||||
|
def declared_training_nodes(profile, task, records):
|
||||||
|
"""Read a plan in complete journals or self-contained individual units."""
|
||||||
|
declarations = set()
|
||||||
|
for row in records:
|
||||||
|
if row.get("task_name") != task.key:
|
||||||
|
continue
|
||||||
|
if row.get("kind") == "command_sampling_plan":
|
||||||
|
if digest({k: v for k, v in row.items() if k != "plan_sha256"}) != row.get("plan_sha256"):
|
||||||
|
raise ValueError("command_sampling_plan_hash_changed")
|
||||||
|
declarations.add(tuple(row["training_nodes"]))
|
||||||
|
elif row.get("sample_phase") == "steady" and "steady_training_nodes" in row:
|
||||||
|
declarations.add(tuple(row["steady_training_nodes"]))
|
||||||
|
if len(declarations) > 1:
|
||||||
|
raise ValueError("command_sampling_plan_conflict")
|
||||||
|
if not declarations:
|
||||||
|
return command_nodes(profile, task)
|
||||||
|
nodes = next(iter(declarations))
|
||||||
|
allowed = set(command_nodes(profile, task))
|
||||||
|
if (len(nodes) < 3 or tuple(sorted(set(nodes))) != nodes or not set(nodes) <= allowed
|
||||||
|
or nodes[0] != min(allowed) or nodes[-1] != max(allowed)):
|
||||||
|
raise ValueError("command_sampling_nodes_invalid")
|
||||||
|
return nodes
|
||||||
|
|
||||||
|
|
||||||
|
def validate_sampling_plans(profile, records):
|
||||||
|
plans = {}
|
||||||
|
for row in records:
|
||||||
|
if row.get("kind") == "command_sampling_plan":
|
||||||
|
task = next(task for task in profile.motion.tasks if task.key == row["task_name"])
|
||||||
|
if row != build_sampling_plan(profile, task, records):
|
||||||
|
raise ValueError("command_sampling_training_evidence_changed")
|
||||||
|
if task.key in plans and plans[task.key] != row:
|
||||||
|
raise ValueError("command_sampling_plan_conflict")
|
||||||
|
plans[task.key] = row
|
||||||
|
required = any(row.get("command_sampling_policy") == SAMPLING_POLICY for row in records)
|
||||||
|
for row in records:
|
||||||
|
if row.get("sample_phase") != "steady" or "steady_target" not in row:
|
||||||
|
continue
|
||||||
|
plan = plans.get(row["task_name"])
|
||||||
|
if required or "sampling_plan_sha256" in row:
|
||||||
|
if (plan is None or row.get("sampling_plan_sha256") != plan["plan_sha256"]
|
||||||
|
or list(row.get("steady_training_nodes", ())) != plan["training_nodes"]):
|
||||||
|
raise ValueError("command_sampling_sample_unbound")
|
||||||
|
return plans
|
||||||
@@ -0,0 +1,77 @@
|
|||||||
|
"""Training-only support for the physically shared turnaround endpoints.
|
||||||
|
|
||||||
|
The last incoming command owns the turnaround image. An unobserved outgoing
|
||||||
|
endpoint is a continuity constraint, never a second independent measurement.
|
||||||
|
Interior parameters and endpoints with actual directional evidence stay free.
|
||||||
|
"""
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
TURNAROUND_POLICY = 'observed_turnaround_continuity_v1'
|
||||||
|
|
||||||
|
|
||||||
|
def compile_turnaround_constraints(bundle):
|
||||||
|
constraints = {}
|
||||||
|
for name, columns in bundle.curve_columns.items():
|
||||||
|
channel = bundle.channels[name]
|
||||||
|
indices = sorted({int(index) for role, data in bundle.observations.items()
|
||||||
|
if any(joint.name == bundle.cad_names[name] for joint in bundle.chains[role])
|
||||||
|
for index in data[0]})
|
||||||
|
if not indices:
|
||||||
|
continue
|
||||||
|
commands = bundle.commands[indices, channel]
|
||||||
|
directions = bundle.directions[indices, channel]
|
||||||
|
for node in (0, len(bundle.knots[name])-1):
|
||||||
|
for recipient in (0, 1):
|
||||||
|
column = int(columns[recipient, node])
|
||||||
|
# Unknown startup history observes the common endpoint; it
|
||||||
|
# does not independently identify the outgoing branch.
|
||||||
|
at_endpoint = commands == bundle.knots[name][node]
|
||||||
|
if column < 0 or np.any(at_endpoint & (directions == recipient)):
|
||||||
|
continue
|
||||||
|
donor = 1-recipient
|
||||||
|
sources = [bundle.rows[index] for index in indices
|
||||||
|
if bundle.commands[index, channel] == bundle.knots[name][node]
|
||||||
|
and bundle.directions[index, channel] == donor]
|
||||||
|
if not sources:
|
||||||
|
continue
|
||||||
|
constraints.setdefault(name, []).append(dict(node=node, recipient=recipient,
|
||||||
|
donor=donor, column=column, command=float(bundle.knots[name][node]),
|
||||||
|
source_sample_ids=sorted({f"{row['view']}:{row['image_stamp_ns']}" for row in sources}),
|
||||||
|
source_cycles=sorted({row['cycle'] for row in sources})))
|
||||||
|
return constraints
|
||||||
|
|
||||||
|
|
||||||
|
def validate_turnaround_mapping(mapping, support):
|
||||||
|
"""Read-back must preserve the saved constraint, including its sources."""
|
||||||
|
if support is None: # Historical artifacts retain their old contract.
|
||||||
|
return
|
||||||
|
if support.get('policy') != TURNAROUND_POLICY or not isinstance(support.get('constraints'), list):
|
||||||
|
raise ValueError('invalid_turnaround_support:' + mapping.joint)
|
||||||
|
endpoints = support.get('endpoints', ())
|
||||||
|
if (len(endpoints) != 2 or [row.get('command') for row in endpoints]
|
||||||
|
!= [mapping.knots[0], mapping.knots[-1]]):
|
||||||
|
raise ValueError('invalid_turnaround_endpoints:' + mapping.joint)
|
||||||
|
for endpoint in endpoints:
|
||||||
|
if not endpoint.get('samples') or any(
|
||||||
|
sample.get('cycle') not in (0, 1) or not isinstance(sample.get('id'), str)
|
||||||
|
or not sample['id'] or sample.get('direction') not in ('', 'increasing', 'decreasing')
|
||||||
|
for sample in endpoint['samples']):
|
||||||
|
raise ValueError('invalid_turnaround_samples:' + mapping.joint)
|
||||||
|
branches = (mapping.increasing_rad, mapping.decreasing_rad)
|
||||||
|
seen = set()
|
||||||
|
for row in support['constraints']:
|
||||||
|
node, recipient, donor = row['node'], row['recipient'], row['donor']
|
||||||
|
if (node not in (0, len(mapping.knots)-1) or recipient not in (0, 1)
|
||||||
|
or donor != 1-recipient or (node, recipient) in seen
|
||||||
|
or row['command'] != mapping.knots[node]
|
||||||
|
or not row['source_sample_ids'] or not row['source_cycles']
|
||||||
|
or not set(row['source_cycles']) <= {0, 1}
|
||||||
|
or abs(branches[recipient][node]-branches[donor][node]) > 1e-10):
|
||||||
|
raise ValueError('turnaround_mapping_differs_from_evidence:' + mapping.joint)
|
||||||
|
seen.add((node, recipient))
|
||||||
|
endpoint = endpoints[0 if node == 0 else 1]
|
||||||
|
observed = {sample['id'] for sample in endpoint['samples']
|
||||||
|
if sample['direction'] == ('increasing', 'decreasing')[donor]}
|
||||||
|
if not observed.intersection(row['source_sample_ids']):
|
||||||
|
raise ValueError('turnaround_source_not_observed:' + mapping.joint)
|
||||||
@@ -106,6 +106,7 @@ def fit_standard_mimic(
|
|||||||
cycles: Sequence[int],
|
cycles: Sequence[int],
|
||||||
offset_rad: float | None,
|
offset_rad: float | None,
|
||||||
offset_observable: bool = False,
|
offset_observable: bool = False,
|
||||||
|
multiplier_bounds: tuple[float, float] | None = None,
|
||||||
) -> LinearMimicFit:
|
) -> LinearMimicFit:
|
||||||
"""Fit actual synchronous observations, never a vendor-generated curve.
|
"""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")
|
raise ValueError("mimic parameters are not independently observable")
|
||||||
residual = rhs - design @ initial
|
residual = rhs - design @ initial
|
||||||
scale = max(math.radians(0.1), 1.4826 * float(np.median(np.abs(residual - np.median(residual)))))
|
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,
|
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)):
|
if not solution.success or not np.all(np.isfinite(solution.x)):
|
||||||
raise ValueError("linear mimic fit failed")
|
raise ValueError("linear mimic fit failed")
|
||||||
multiplier = float(solution.x[0])
|
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)))
|
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)
|
@dataclass(frozen=True)
|
||||||
class CouplingFit:
|
class CouplingFit:
|
||||||
source_joint: str
|
source_joint: str
|
||||||
|
|||||||
@@ -0,0 +1,60 @@
|
|||||||
|
"""Explicit JSON transport for immutable numerical training objects.
|
||||||
|
|
||||||
|
Only a fixed registry of calibration value types can be reconstructed. There
|
||||||
|
is no pickle, dynamic import or executable object in a persisted model.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import fields, is_dataclass
|
||||||
|
import math
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
|
||||||
|
def encode(value):
|
||||||
|
if isinstance(value, Rotation):
|
||||||
|
return {"$type": "Rotation", "value": value.as_quat().tolist()}
|
||||||
|
if isinstance(value, np.ndarray):
|
||||||
|
return {"$type": "array", "value": value.tolist()}
|
||||||
|
if isinstance(value, np.generic):
|
||||||
|
return encode(value.item())
|
||||||
|
if is_dataclass(value):
|
||||||
|
return {"$type": type(value).__name__, "fields": {
|
||||||
|
field.name: encode(getattr(value, field.name)) for field in fields(value)}}
|
||||||
|
if isinstance(value, dict):
|
||||||
|
return {"$type": "mapping", "items": [[encode(k), encode(value[k])] for k in sorted(value, key=repr)]}
|
||||||
|
if isinstance(value, (tuple, list, frozenset, set)):
|
||||||
|
values = sorted(value) if isinstance(value, (set, frozenset)) else value
|
||||||
|
return {"$type": type(value).__name__, "items": [encode(item) for item in values]}
|
||||||
|
if isinstance(value, float) and not math.isfinite(value):
|
||||||
|
raise ValueError("frozen_training_nonfinite_value")
|
||||||
|
if value is None or isinstance(value, (str, int, float, bool)):
|
||||||
|
return value
|
||||||
|
raise TypeError(f"unsupported_frozen_training_type:{type(value).__name__}")
|
||||||
|
|
||||||
|
|
||||||
|
def decode(value):
|
||||||
|
if not isinstance(value, dict):
|
||||||
|
return value
|
||||||
|
kind = value.get("$type")
|
||||||
|
if kind == "Rotation":
|
||||||
|
return Rotation.from_quat(value["value"])
|
||||||
|
if kind == "array":
|
||||||
|
return np.asarray(value["value"], dtype=float)
|
||||||
|
if kind == "mapping":
|
||||||
|
return {decode(k): decode(v) for k, v in value["items"]}
|
||||||
|
sequences = {"tuple": tuple, "list": list, "set": set, "frozenset": frozenset}
|
||||||
|
if kind in sequences:
|
||||||
|
return sequences[kind](decode(item) for item in value["items"])
|
||||||
|
from . import command_motion, tag_installation
|
||||||
|
from .spatial_solver import stages, solve, types
|
||||||
|
from ..domain import measurement, result
|
||||||
|
registry = {name: cls for module in (command_motion, tag_installation, stages, solve,
|
||||||
|
types, measurement, result) for name, cls in vars(module).items()
|
||||||
|
if isinstance(cls, type) and is_dataclass(cls)}
|
||||||
|
if kind not in registry:
|
||||||
|
raise ValueError(f"unknown_frozen_training_type:{kind}")
|
||||||
|
cls = registry[kind]
|
||||||
|
if set(value["fields"]) != {field.name for field in fields(cls)}:
|
||||||
|
raise ValueError("frozen_training_fields_changed")
|
||||||
|
return cls(**{key: decode(item) for key, item in value["fields"].items()})
|
||||||
+107
@@ -0,0 +1,107 @@
|
|||||||
|
"""Training-only URDF approximations, separate from exact JSON lookup tables."""
|
||||||
|
|
||||||
|
from dataclasses import dataclass, replace
|
||||||
|
import math
|
||||||
|
|
||||||
|
from .coupling import LinearMimicFit, fit_standard_mimic, fit_range_bounded_mimic
|
||||||
|
from .observed_motion import paired_visual_observations
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MimicApproximation:
|
||||||
|
output_fit: LinearMimicFit
|
||||||
|
training_sample_ids: tuple[str, ...]
|
||||||
|
holdout_sample_ids: tuple[str, ...]
|
||||||
|
holdout_errors_rad: tuple[float, ...]
|
||||||
|
transferred_from_joint: str | None = None
|
||||||
|
multiplier_bounds: tuple[float, float] | None = None
|
||||||
|
parent_range_rad: tuple[float, float] | None = None
|
||||||
|
child_range_rad: tuple[float, float] | None = None
|
||||||
|
|
||||||
|
def in_cad_coordinates(self, offsets):
|
||||||
|
from dataclasses import replace
|
||||||
|
if self.parent_range_rad is not None:
|
||||||
|
raise ValueError("affine_approximation_intercept_is_not_a_physical_zero")
|
||||||
|
fit = self.output_fit
|
||||||
|
# The offset is supported by the TWO baseline geometry datums.
|
||||||
|
return replace(fit, offset_rad=offsets[fit.target_joint]-fit.multiplier*offsets[fit.source_joint],
|
||||||
|
offset_observed=True)
|
||||||
|
|
||||||
|
|
||||||
|
def shared_zero_multiplier_bounds(parent_range, child_range):
|
||||||
|
"""Intersect endpoint constraints for q_child = a*q_parent.
|
||||||
|
|
||||||
|
These ranges come from training support or mechanical bounds, never from
|
||||||
|
holdout observations. Neither measured curve nor joint range is modified.
|
||||||
|
"""
|
||||||
|
lower, upper = -math.inf, math.inf
|
||||||
|
if any(len(interval) != 2 or not all(math.isfinite(v) for v in interval)
|
||||||
|
or interval[0] >= interval[1] for interval in (parent_range, child_range)):
|
||||||
|
raise ValueError("invalid_mimic_motion_interval")
|
||||||
|
for endpoint in parent_range:
|
||||||
|
if endpoint == 0:
|
||||||
|
if not child_range[0] <= 0 <= child_range[1]:
|
||||||
|
raise ValueError("mimic_range_excludes_shared_zero")
|
||||||
|
continue
|
||||||
|
values = sorted(value / endpoint for value in child_range)
|
||||||
|
lower, upper = max(lower, values[0]), min(upper, values[1])
|
||||||
|
if not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper:
|
||||||
|
raise ValueError("no_feasible_shared_zero_mimic")
|
||||||
|
return lower, upper
|
||||||
|
|
||||||
|
|
||||||
|
def fit_mimic_approximations(model, curves, records, *, output_ranges=None, affine=False):
|
||||||
|
result = {}
|
||||||
|
for name, joint in model.joints.items():
|
||||||
|
parent = joint.mimic_joint
|
||||||
|
if parent is None or name not in curves or parent not in curves:
|
||||||
|
continue
|
||||||
|
rows = paired_visual_observations(records[parent], records[name], curves[parent], curves[name])
|
||||||
|
for cycle in range(4):
|
||||||
|
for direction in ("increasing", "decreasing"):
|
||||||
|
if sum(r.cycle == cycle and r.direction == direction for r in rows) < 40:
|
||||||
|
raise ValueError(f"paired_mimic_observations_missing:{name}:{cycle}:{direction}")
|
||||||
|
training = [r for r in rows if r.cycle != 3]
|
||||||
|
holdout = [r for r in rows if r.cycle == 3]
|
||||||
|
bounds = None
|
||||||
|
arguments = dict(source_joint=parent, target_joint=name,
|
||||||
|
active_rad=[r.source_rad for r in training], passive_rad=[r.target_rad for r in training],
|
||||||
|
cycles=[r.cycle for r in training])
|
||||||
|
if affine:
|
||||||
|
if output_ranges is None:
|
||||||
|
raise ValueError("affine_mimic_requires_training_motion_ranges")
|
||||||
|
fit = fit_range_bounded_mimic(**arguments,
|
||||||
|
parent_range=output_ranges[parent], child_range=output_ranges[name])
|
||||||
|
else:
|
||||||
|
bounds = None if output_ranges is None else shared_zero_multiplier_bounds(
|
||||||
|
output_ranges[parent], output_ranges[name])
|
||||||
|
fit = fit_standard_mimic(**arguments, offset_rad=0.0, multiplier_bounds=bounds)
|
||||||
|
if not math.isfinite(fit.multiplier) or fit.multiplier == 0:
|
||||||
|
raise ValueError(f"invalid_mimic_approximation:{name}")
|
||||||
|
result[name] = MimicApproximation(fit, tuple(r.sample_id for r in training),
|
||||||
|
tuple(r.sample_id for r in holdout),
|
||||||
|
tuple(fit.multiplier*r.source_rad + fit.offset_rad - r.target_rad for r in holdout),
|
||||||
|
multiplier_bounds=bounds,
|
||||||
|
parent_range_rad=tuple(output_ranges[parent]) if affine else None,
|
||||||
|
child_range_rad=tuple(output_ranges[name]) if affine else None)
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def fit_bounded_mimic_approximations(profile, model, fit, records):
|
||||||
|
"""Fit the URDF affine fallback after training establishes both ranges.
|
||||||
|
|
||||||
|
Only independently observed pairs are fitted; explicit donor expansion is
|
||||||
|
performed afterwards. JSON continues to use the independent nonlinear maps.
|
||||||
|
"""
|
||||||
|
ranges = {}
|
||||||
|
for name, mapping in fit.output_mappings.items():
|
||||||
|
joint = model.joints[name]
|
||||||
|
if name in profile.urdf_limit_policies or joint.mimic_joint is None:
|
||||||
|
command = fit.command_mappings[name]
|
||||||
|
ranges[name] = (min(0, mapping.bounds_rad[0], command.bounds_rad[0]),
|
||||||
|
max(0, mapping.bounds_rad[1], command.bounds_rad[1]))
|
||||||
|
else:
|
||||||
|
delta = fit.zero_offsets_rad[name]
|
||||||
|
ranges[name] = (joint.lower - delta, joint.upper - delta)
|
||||||
|
return replace(fit, mimic_approximations=fit_mimic_approximations(
|
||||||
|
model, fit.curves, records, output_ranges=ranges, affine=True))
|
||||||
@@ -71,7 +71,7 @@ def native_coordinate_curve(curve, baseline):
|
|||||||
"coordinate_reference_angle_rad": reference_angle}), reference_input
|
"coordinate_reference_angle_rad": reference_angle}), reference_input
|
||||||
|
|
||||||
|
|
||||||
def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_records=None):
|
def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_records=None, zero_references=None):
|
||||||
expected = {name for name, spec in profile.measurement.measurements.items()
|
expected = {name for name, spec in profile.measurement.measurements.items()
|
||||||
if spec.view is not None and spec.kind != "axis_cross_view_validation"}
|
if spec.view is not None and spec.kind != "axis_cross_view_validation"}
|
||||||
if set(records_by_joint) != expected:
|
if set(records_by_joint) != expected:
|
||||||
@@ -79,7 +79,20 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
|
|||||||
if any(profile.measurement.measurements[name].kind not in {"relative_rotation", "urdf_axis_chain", "curve"} for name in expected):
|
if any(profile.measurement.measurements[name].kind not in {"relative_rotation", "urdf_axis_chain", "curve"} for name in expected):
|
||||||
raise ValueError("measurement primitive is not implemented in this pipeline")
|
raise ValueError("measurement primitive is not implemented in this pipeline")
|
||||||
mimic_sources = {name: profile.zero.mimic_source_by_joint[name] for name in profile.zero.fitted_mimic_joints}
|
mimic_sources = {name: profile.zero.mimic_source_by_joint[name] for name in profile.zero.fitted_mimic_joints}
|
||||||
if profile.command.unit == "u8":
|
if profile.artifacts.output_schema_version >= 3:
|
||||||
|
if set(zero_references or {}) != expected:
|
||||||
|
raise ValueError("joint_zero_references_incomplete")
|
||||||
|
observed, holdout, evidence, references = {}, {}, {}, {}
|
||||||
|
for name, rows in sorted(records_by_joint.items()):
|
||||||
|
view = profile.measurement.measurements[name].view
|
||||||
|
reference = zero_references[name]
|
||||||
|
observed[name], holdout[name] = fit_native_observed_rotation(rows,
|
||||||
|
input_domain=profile.curve_input_domain,
|
||||||
|
input_to_joint_direction=joint_input_direction(profile, source_model, name),
|
||||||
|
reference_xyzw=reference.quaternion(view), joint=name, view=view)
|
||||||
|
references[name] = reference.input_reference(view, feedback=True)
|
||||||
|
coordinates = observed
|
||||||
|
elif profile.command.unit == "u8":
|
||||||
observed, holdout, evidence = fit_byte_observed_motion(records_by_joint,
|
observed, holdout, evidence = fit_byte_observed_motion(records_by_joint,
|
||||||
source_model=source_model, mimic_sources=mimic_sources,
|
source_model=source_model, mimic_sources=mimic_sources,
|
||||||
baseline_by_joint={name: profile.command.baseline_values[channel_for_joint(profile, name)] for name in expected},
|
baseline_by_joint={name: profile.command.baseline_values[channel_for_joint(profile, name)] for name in expected},
|
||||||
@@ -92,7 +105,8 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
|
|||||||
observed, coordinates, holdout, evidence, references = {}, {}, {}, {}, {}
|
observed, coordinates, holdout, evidence, references = {}, {}, {}, {}, {}
|
||||||
for name, rows in sorted(records_by_joint.items()):
|
for name, rows in sorted(records_by_joint.items()):
|
||||||
curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain,
|
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
|
observed[name], holdout[name] = curve, errors
|
||||||
coordinates[name], references[name] = native_coordinate_curve(curve,
|
coordinates[name], references[name] = native_coordinate_curve(curve,
|
||||||
profile.command.baseline_values[channel_for_joint(profile, name)])
|
profile.command.baseline_values[channel_for_joint(profile, name)])
|
||||||
@@ -109,7 +123,11 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
|
|||||||
metrics = {}
|
metrics = {}
|
||||||
for name, rows in secondary.items():
|
for name, rows in secondary.items():
|
||||||
curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain,
|
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),
|
||||||
|
reference_xyzw=(zero_references[name].quaternion(rows[0]["view"])
|
||||||
|
if profile.artifacts.output_schema_version >= 3 else None),
|
||||||
|
joint=name, view=profile.measurement.measurements[
|
||||||
|
profile.measurement.cross_view_sources[name]].view)
|
||||||
if "input_knots" not in observed[name].circle:
|
if "input_knots" not in observed[name].circle:
|
||||||
raise ValueError("cross-view comparison requires native input knots")
|
raise ValueError("cross-view comparison requires native input knots")
|
||||||
comparison = compare_native_views(observed[name], curve)
|
comparison = compare_native_views(observed[name], curve)
|
||||||
|
|||||||
@@ -0,0 +1,111 @@
|
|||||||
|
"""Apply explicit donor policies without creating recipient observations.
|
||||||
|
|
||||||
|
Motion curves, absolute zeros and URDF approximations have separate provenance.
|
||||||
|
Only the numerical mappings are expanded; training and holdout evidence remain
|
||||||
|
attached to the joints that were actually observed.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import replace
|
||||||
|
|
||||||
|
from ..domain.profile import CalibrationProfile, validate_profile
|
||||||
|
from ..domain.result import CalibrationResult, JointMapping
|
||||||
|
from .motion_fit import channel_for_joint
|
||||||
|
|
||||||
|
|
||||||
|
def transferred_mapping(profile: CalibrationProfile, mapping: JointMapping,
|
||||||
|
target: str) -> JointMapping:
|
||||||
|
return replace(mapping, joint=target, motor_index=channel_for_joint(profile, target),
|
||||||
|
transferred_from=mapping.joint)
|
||||||
|
|
||||||
|
|
||||||
|
def expand_profile_transfers(profile: CalibrationProfile, fit: CalibrationResult) -> CalibrationResult:
|
||||||
|
"""Copy frozen fits once, after all measured command holdouts have passed."""
|
||||||
|
transfers = profile.measurement.transferred_motion_sources
|
||||||
|
if not transfers:
|
||||||
|
return fit
|
||||||
|
validate_profile(profile)
|
||||||
|
outputs, commands = dict(fit.output_mappings), dict(fit.command_mappings)
|
||||||
|
offsets, methods = dict(fit.zero_offsets_rad), dict(fit.zero_method_by_joint)
|
||||||
|
approximations = dict(fit.mimic_approximations)
|
||||||
|
for target, source in transfers.items():
|
||||||
|
if target in outputs or target in commands or target in fit.curves or target in fit.joint_zero_references:
|
||||||
|
raise ValueError(f"motion_transfer_cannot_replace_existing_result:{target}")
|
||||||
|
if source not in fit.curves or source not in outputs or source not in commands:
|
||||||
|
raise ValueError(f"motion_transfer_requires_fitted_donor:{target}:{source}")
|
||||||
|
for mappings in (outputs, commands):
|
||||||
|
donor = mappings[source]
|
||||||
|
if donor.joint != source or donor.transferred_from is not None \
|
||||||
|
or donor.motor_index != channel_for_joint(profile, source):
|
||||||
|
raise ValueError(f"motion_transfer_requires_direct_donor_binding:{source}")
|
||||||
|
mappings[target] = transferred_mapping(profile, donor, target)
|
||||||
|
# Dynamic similarity does not authorize copying an absolute CAD zero.
|
||||||
|
if profile.zero.transferred_zero_sources.get(target) == source and source in offsets:
|
||||||
|
offsets[target] = offsets[source]
|
||||||
|
methods[target] = "transferred_static_zero_on_own_cad"
|
||||||
|
# Historical fitted approximations remain transferable for replay.
|
||||||
|
# Current measured fitting emits no mimic fit; each recipient keeps its CAD coupling.
|
||||||
|
if target in profile.zero.passive_joints and approximations:
|
||||||
|
if source not in fit.mimic_approximations:
|
||||||
|
raise ValueError(f"motion_transfer_requires_paired_donor_mimic:{target}:{source}")
|
||||||
|
donor = fit.mimic_approximations[source]
|
||||||
|
approximations[target] = replace(donor,
|
||||||
|
output_fit=replace(donor.output_fit, target_joint=target,
|
||||||
|
source_joint=profile.zero.mimic_source_by_joint[target]),
|
||||||
|
transferred_from_joint=source)
|
||||||
|
return replace(fit, output_mappings=outputs, command_mappings=commands,
|
||||||
|
zero_offsets_rad=offsets, zero_method_by_joint=methods, mimic_approximations=approximations)
|
||||||
|
|
||||||
|
|
||||||
|
def validate_mapping_provenance(profile: CalibrationProfile, fit: CalibrationResult) -> None:
|
||||||
|
"""Require every published table to be measured or an exact authorized copy."""
|
||||||
|
validate_profile(profile)
|
||||||
|
transfers = profile.measurement.transferred_motion_sources
|
||||||
|
observed = set(fit.curves)
|
||||||
|
if observed & transfers.keys():
|
||||||
|
raise ValueError("independent_release_contains_transferred_or_unknown_joints")
|
||||||
|
expected = observed | transfers.keys()
|
||||||
|
for mappings in (fit.output_mappings, fit.command_mappings):
|
||||||
|
if set(mappings) != expected:
|
||||||
|
raise ValueError("independent_release_contains_transferred_or_unknown_joints")
|
||||||
|
for name, mapping in mappings.items():
|
||||||
|
if mapping.joint != name or mapping.motor_index != channel_for_joint(profile, name):
|
||||||
|
raise ValueError(f"mapping_sdk_binding_differs:{name}")
|
||||||
|
source = transfers.get(name)
|
||||||
|
if source is None:
|
||||||
|
if mapping.transferred_from is not None:
|
||||||
|
raise ValueError("independent_release_contains_transferred_or_unknown_joints")
|
||||||
|
elif source not in observed or mappings[source].transferred_from is not None \
|
||||||
|
or mapping != transferred_mapping(profile, mappings[source], name):
|
||||||
|
raise ValueError(f"mapping_differs_from_declared_donor:{name}")
|
||||||
|
for name, approximation in fit.mimic_approximations.items():
|
||||||
|
source = transfers.get(name)
|
||||||
|
if source is None:
|
||||||
|
if approximation.transferred_from_joint is not None:
|
||||||
|
raise ValueError(f"undeclared_mimic_transfer:{name}")
|
||||||
|
elif source not in fit.mimic_approximations:
|
||||||
|
raise ValueError(f"missing_donor_mimic_approximation:{name}")
|
||||||
|
else:
|
||||||
|
donor = fit.mimic_approximations[source]
|
||||||
|
expected_approximation = replace(donor, output_fit=replace(donor.output_fit,
|
||||||
|
target_joint=name, source_joint=profile.zero.mimic_source_by_joint[name]),
|
||||||
|
transferred_from_joint=source)
|
||||||
|
if approximation != expected_approximation:
|
||||||
|
raise ValueError(f"mimic_differs_from_declared_donor:{name}")
|
||||||
|
|
||||||
|
|
||||||
|
def validate_zero_provenance(profile: CalibrationProfile, fit: CalibrationResult) -> None:
|
||||||
|
"""Keep accepted premises and explicit zero copies true in the final plan."""
|
||||||
|
offsets, methods = fit.zero_offsets_rad, fit.zero_method_by_joint
|
||||||
|
for name in profile.zero.cad_zero_assumptions:
|
||||||
|
if offsets.get(name) != 0.0 or methods.get(name) != "assumed_source_cad_zero":
|
||||||
|
raise ValueError(f"assumed_cad_zero_differs_from_profile:{name}")
|
||||||
|
for name, geometry in profile.zero.known_baseline_geometry.items():
|
||||||
|
if offsets.get(name) != geometry.cad_angle_rad \
|
||||||
|
or methods.get(name) != "independent_known_baseline_geometry":
|
||||||
|
raise ValueError(f"known_baseline_geometry_differs_from_profile:{name}")
|
||||||
|
for target, source in profile.zero.transferred_zero_sources.items():
|
||||||
|
if profile.measurement.transferred_motion_sources.get(target) != source:
|
||||||
|
continue
|
||||||
|
if source not in offsets or offsets.get(target) != offsets[source] \
|
||||||
|
or methods.get(target) != "transferred_static_zero_on_own_cad":
|
||||||
|
raise ValueError(f"transferred_zero_differs_from_declared_donor:{target}")
|
||||||
@@ -10,6 +10,7 @@ from .image_curve import fit_joint_image_curve
|
|||||||
from ..urdf.acceptance import angular_metrics
|
from ..urdf.acceptance import angular_metrics
|
||||||
from ..domain.measurement import JointCurveFit
|
from ..domain.measurement import JointCurveFit
|
||||||
from .rotation_curve import RotationObservation, fit_rotation_curve
|
from .rotation_curve import RotationObservation, fit_rotation_curve
|
||||||
|
from .rotation_diagnostics import RotationHoldoutFailure, rotation_holdout_diagnostic
|
||||||
|
|
||||||
|
|
||||||
def image_identity(row: Mapping[str, Any]) -> str:
|
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
|
return output
|
||||||
|
|
||||||
|
|
||||||
def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction):
|
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.
|
"""Native-coordinate 3+1 hinge primitive with the shared geometric result.
|
||||||
|
|
||||||
The metadata carries real knots, not a synthetic byte command domain.
|
The metadata carries real knots, not a synthetic byte command domain.
|
||||||
@@ -58,11 +60,14 @@ def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction
|
|||||||
if sum(row.cycle == 3 and row.direction == direction for row in observations) < 40:
|
if sum(row.cycle == 3 and row.direction == direction for row in observations) < 40:
|
||||||
raise ValueError("holdout requires 40 images in each direction")
|
raise ValueError("holdout requires 40 images in each direction")
|
||||||
fitted = fit_rotation_curve([row for row in observations if row.cycle != 3],
|
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)
|
input_domain=input_domain, sdk_to_joint_direction=input_to_joint_direction,
|
||||||
errors = fitted.holdout_errors([row for row in observations if row.cycle == 3])
|
reference_xyzw=reference_xyzw)
|
||||||
|
holdout = [row for row in observations if row.cycle == 3]
|
||||||
|
errors = fitted.holdout_errors(holdout)
|
||||||
metrics = angular_metrics(errors)
|
metrics = angular_metrics(errors)
|
||||||
if not metrics.passed:
|
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)))
|
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,
|
curve = JointCurveFit(fitted.angle_rad, fitted.decreasing_rad, fitted.increasing_rad,
|
||||||
{"space": "relative_rotation_3d", "input_domain": input_domain, "input_knots": fitted.knots,
|
{"space": "relative_rotation_3d", "input_domain": input_domain, "input_knots": fitted.knots,
|
||||||
|
|||||||
@@ -0,0 +1,322 @@
|
|||||||
|
"""Approximate command curves using the existing CAD image objective.
|
||||||
|
|
||||||
|
Drafts estimate supported internal zeros. Unobserved root frames and declared
|
||||||
|
CAD leaf zeros remain explicit premises, never arbitrary measured offsets.
|
||||||
|
"""
|
||||||
|
from collections import defaultdict
|
||||||
|
from dataclasses import replace
|
||||||
|
from itertools import product
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
from ..domain.capture_plan import task_for_joint
|
||||||
|
from ..geometry.tag_pose.motion_evidence import _axis
|
||||||
|
from ..geometry.tag_pose.urdf_command_images import UrdfCommandImages, pose_vector
|
||||||
|
from .motion_fit import joint_input_direction
|
||||||
|
|
||||||
|
UPRIGHT_SPLAY_JOINTS = frozenset(finger+'_mcp_roll' for finger in ('index', 'middle', 'ring', 'pinky'))
|
||||||
|
UPRIGHT_SPLAY_REFERENCE = 'operator_defined_upright_splay_baseline'
|
||||||
|
|
||||||
|
|
||||||
|
def define_upright_splay_zero(profile, model):
|
||||||
|
"""Use the operator's upright baseline as a definition, not a measurement."""
|
||||||
|
if (profile.key.model != 'O30' or profile.key.side != 'right'
|
||||||
|
or not UPRIGHT_SPLAY_JOINTS <= profile.zero.active_joints):
|
||||||
|
raise ValueError('draft_upright_splay_requires_o30_right_four_fingers')
|
||||||
|
for name in UPRIGHT_SPLAY_JOINTS:
|
||||||
|
finger = name.split('_')[0]
|
||||||
|
positions = [model.link_transform(profile.command.urdf_joint_by_joint.get(joint, joint),
|
||||||
|
zero_offsets={}, joint_angles={}, independent_mimic_angles=True)[:3, 3]
|
||||||
|
for joint in (finger+'_mcp_pitch', finger+'_pip')]
|
||||||
|
direction = positions[1]-positions[0]
|
||||||
|
if not np.allclose(direction/np.linalg.norm(direction), [0., 0., 1.], atol=1e-6):
|
||||||
|
raise ValueError('draft_source_zero_is_not_upright:' + name)
|
||||||
|
assumptions = dict(profile.zero.cad_zero_assumptions)
|
||||||
|
assumptions.update({name: UPRIGHT_SPLAY_REFERENCE for name in UPRIGHT_SPLAY_JOINTS})
|
||||||
|
return replace(profile, zero=replace(profile.zero, cad_zero_assumptions=assumptions))
|
||||||
|
|
||||||
|
|
||||||
|
def draft_training_coverage(completed_units, units):
|
||||||
|
"""Audit accepted directions without borrowing from independent validation."""
|
||||||
|
completed = set(completed_units)
|
||||||
|
coverage = {}
|
||||||
|
for task in sorted({unit.task_key for unit in units}):
|
||||||
|
cycles = {}
|
||||||
|
for cycle in (0, 1):
|
||||||
|
expected = {unit.identity for unit in units if unit.task_key == task and unit.cycle == cycle}
|
||||||
|
accepted = expected & completed
|
||||||
|
cycles[cycle] = dict(expected_units=len(expected), accepted_units=len(accepted),
|
||||||
|
complete=bool(expected) and accepted == expected)
|
||||||
|
complete = [cycle for cycle, row in cycles.items() if row['complete']]
|
||||||
|
coverage[task] = dict(cycles=cycles, complete_cycles=complete,
|
||||||
|
initialization_cycle=complete[0] if complete else None)
|
||||||
|
return coverage
|
||||||
|
|
||||||
|
|
||||||
|
def draft_targets(profile, selected=None):
|
||||||
|
"""Report the requested whole-hand result independently of partial output."""
|
||||||
|
target_ranges = set(profile.zero.active_joints)
|
||||||
|
retained_leaves = set(profile.zero.cad_zero_assumptions)
|
||||||
|
target_zeros = target_ranges-retained_leaves
|
||||||
|
measured_ranges = set() if selected is None else set(selected.zero.active_joints)
|
||||||
|
measured_zeros = set() if selected is None else measured_ranges-set(selected.zero.cad_zero_assumptions)
|
||||||
|
defined_zeros = set() if selected is None else {name for name, reason in
|
||||||
|
selected.zero.cad_zero_assumptions.items() if reason == UPRIGHT_SPLAY_REFERENCE}
|
||||||
|
return dict(zero_target_joints=sorted(target_zeros), range_target_joints=sorted(target_ranges),
|
||||||
|
retained_leaf_zero_joints=sorted(retained_leaves), estimated_zero_joints=sorted(measured_zeros),
|
||||||
|
defined_zero_joints=sorted(defined_zeros),
|
||||||
|
estimated_range_joints=sorted(measured_ranges),
|
||||||
|
missing_zero_joints=sorted(target_zeros-measured_zeros-defined_zeros),
|
||||||
|
missing_range_joints=sorted(target_ranges-measured_ranges),
|
||||||
|
full_target_covered=(target_zeros <= measured_zeros | defined_zeros and target_ranges <= measured_ranges))
|
||||||
|
|
||||||
|
|
||||||
|
def draft_profile(profile, model, completed_units, units, training):
|
||||||
|
"""One complete training cycle supports a draft, with observed ancestors."""
|
||||||
|
coverage = draft_training_coverage(completed_units, units)
|
||||||
|
tasks = {task for task, row in coverage.items() if row['complete_cycles']}
|
||||||
|
names = {name for task in profile.motion.tasks if task.key in tasks for name in task.joints}
|
||||||
|
cad_names = {name: profile.command.urdf_joint_by_joint.get(name, name) for name in names}
|
||||||
|
observed = set(cad_names.values())
|
||||||
|
names = frozenset(name for name in names if all(j.kind == 'fixed' or j.name in observed
|
||||||
|
for j in model._chain(cad_names[name])))
|
||||||
|
if not names:
|
||||||
|
raise ValueError('draft_no_complete_ancestor_supported_training')
|
||||||
|
from ...profiles.observations import compile_tag_links
|
||||||
|
links = compile_tag_links(profile, model)
|
||||||
|
observed = {cad_names[name] for name in names}
|
||||||
|
visible = {role for row in training for role in row.get('tags', {})}
|
||||||
|
roles = {role for role, link in links.items() if role in visible and (link == model.root_link or all(
|
||||||
|
j.kind == 'fixed' or j.name in observed
|
||||||
|
for j in model._chain(model.parent_joint_by_child[link])))}
|
||||||
|
# Fixed tags belong to the root, which has no parent joint.
|
||||||
|
views = tuple(replace(view, tags=tuple(tag for tag in view.tags
|
||||||
|
if tag.fixed_reference or tag.role in roles)) for view in profile.vision.views)
|
||||||
|
assumptions = {name: reason for name, reason in profile.zero.cad_zero_assumptions.items() if name in names}
|
||||||
|
for name, observer in profile.zero.spatial.get('offset_observer_joint', {}).items():
|
||||||
|
if name in names and observer not in names:
|
||||||
|
assumptions[name] = 'partial_missing_zero_observer:' + observer
|
||||||
|
roots = [name for name in names if not any(j.kind != 'fixed'
|
||||||
|
for j in model._chain(cad_names[name])[:-1])]
|
||||||
|
# With only one root chain, rotating palm and root zero together leaves
|
||||||
|
# every image identical. Fix that gauge, while fitting the internal zeros.
|
||||||
|
if len(roots) == 1:
|
||||||
|
assumptions[roots[0]] = 'partial_single_root_zero_unobservable_without_palm_reference'
|
||||||
|
selected = replace(profile,
|
||||||
|
zero=replace(profile.zero, active_joints=names,
|
||||||
|
cad_zero_assumptions=assumptions),
|
||||||
|
motion=replace(profile.motion, tasks=tuple(task for task in profile.motion.tasks
|
||||||
|
if task.key in tasks and set(task.joints) <= names)),
|
||||||
|
vision=replace(profile.vision, views=views))
|
||||||
|
required = {profile.measurement.measurements[name].child_role for name in names}
|
||||||
|
if not required <= roles:
|
||||||
|
raise ValueError('draft_required_training_tags_missing:' + str(sorted(required-roles)))
|
||||||
|
return selected
|
||||||
|
|
||||||
|
|
||||||
|
class DraftCommandImages(UrdfCommandImages):
|
||||||
|
"""Shared image objective, with exact repeated-pose least-squares reduction."""
|
||||||
|
|
||||||
|
# Numerical stopping for an explicitly uncertified draft; release angular,
|
||||||
|
# image, observability and final-file acceptance limits remain independent.
|
||||||
|
solver_ftol = 1e-5
|
||||||
|
|
||||||
|
def __init__(self, profile, model, extrinsics, rows, tag_links, *, initialization_cycles=None,
|
||||||
|
training_initialization=None):
|
||||||
|
super().__init__(profile, model, extrinsics, rows, tag_links)
|
||||||
|
self.training_initialization = training_initialization
|
||||||
|
self.initialization_cycles = (dict(initialization_cycles) if initialization_cycles is not None
|
||||||
|
else {task.key: 0 for task in profile.motion.tasks})
|
||||||
|
if any(self.initialization_cycles.get(task.key) not in (0, 1) for task in profile.motion.tasks):
|
||||||
|
raise ValueError('draft_initialization_training_cycle_required')
|
||||||
|
self.original_observations = self.observations
|
||||||
|
self.observations, weights, within_group_error = {}, [], 0.
|
||||||
|
local_names = {cad: name for name, cad in self.cad_names.items()}
|
||||||
|
for role, (indices, pixels, matrices, objects) in self.original_observations.items():
|
||||||
|
channels = [self.channels[local_names[joint.name]] for joint in self.chains[role]
|
||||||
|
if joint.kind != 'fixed']
|
||||||
|
groups = defaultdict(list)
|
||||||
|
for position, index in enumerate(indices):
|
||||||
|
key = (tuple(self.commands[index, channels]), tuple(self.directions[index, channels]),
|
||||||
|
matrices[position].tobytes())
|
||||||
|
groups[key].append(position)
|
||||||
|
representatives, means = [], []
|
||||||
|
for positions in groups.values():
|
||||||
|
mean = pixels[positions].mean(axis=0)
|
||||||
|
within_group_error += float(np.sum((pixels[positions]-mean)**2))
|
||||||
|
representatives.append(positions[0])
|
||||||
|
means.append(mean)
|
||||||
|
weights.extend([np.sqrt(len(positions))]*8)
|
||||||
|
self.observations[role] = (indices[representatives], np.asarray(means),
|
||||||
|
matrices[representatives], objects)
|
||||||
|
self.residual_weights = np.asarray(weights)
|
||||||
|
self.within_group_error = within_group_error
|
||||||
|
self.original_residual_count = sum(len(data[0])*8 for data in self.original_observations.values())
|
||||||
|
|
||||||
|
def residual(self, parameters):
|
||||||
|
# sum ||prediction - pixel_i||² = n ||prediction - mean||² + scatter.
|
||||||
|
# Keep scatter in the objective, including the solver stopping test.
|
||||||
|
return np.r_[super().residual(parameters)*self.residual_weights,
|
||||||
|
np.sqrt(self.within_group_error), np.zeros(7)]
|
||||||
|
|
||||||
|
def jacobian(self, parameters):
|
||||||
|
weighted = super().jacobian(parameters)*self.residual_weights[:, None]
|
||||||
|
return np.vstack((weighted, np.zeros((8, len(parameters)))))
|
||||||
|
|
||||||
|
def original_residual(self, parameters):
|
||||||
|
predicted = self.project_observations(parameters, self.commands, self.directions, self.original_observations)
|
||||||
|
return np.concatenate([(predicted[role]-data[1]).ravel()
|
||||||
|
for role, data in self.original_observations.items()])
|
||||||
|
|
||||||
|
def audit_root_directions(self):
|
||||||
|
"""Reject opposite SDK signs for CAD-parallel roots, before fitting zeros.
|
||||||
|
|
||||||
|
Only complete training cycles enter this check. Both planar candidates
|
||||||
|
and both scan directions are retained; an ambiguous axis is not a
|
||||||
|
direction correction. Nothing here changes commands or chooses a pose.
|
||||||
|
"""
|
||||||
|
roots = [name for name in self.profile.zero.spatial.get('root_anchor_joints', ())
|
||||||
|
if name in self.joints]
|
||||||
|
axes = {}
|
||||||
|
for name in roots:
|
||||||
|
spec = self.profile.measurement.measurements[name]
|
||||||
|
task = task_for_joint(self.profile, name)
|
||||||
|
channel = self.channels[name]
|
||||||
|
observed = []
|
||||||
|
for direction in range(2):
|
||||||
|
indices = self.motion_indices(name, direction, self.initialization_cycles[task.key])
|
||||||
|
if len(indices) < 3:
|
||||||
|
continue
|
||||||
|
for branch in (0, 1):
|
||||||
|
_, poses = self.motion_poses(indices, spec.child_role, branch)
|
||||||
|
axis, phase = _axis(Rotation.from_matrix(np.asarray(poses)[:, :3, :3]))
|
||||||
|
if abs(phase[-1]-phase[0]) < np.radians(15):
|
||||||
|
continue
|
||||||
|
observed.append(axis*np.sign(phase[-1]-phase[0])
|
||||||
|
*joint_input_direction(self.profile, self.model, name))
|
||||||
|
axes[name] = observed
|
||||||
|
comparisons = []
|
||||||
|
for i, first in enumerate(roots):
|
||||||
|
a, _ = self.model.axis_line(self.cad_names[first], zero_offsets={}, joint_angles={})
|
||||||
|
for second in roots[i+1:]:
|
||||||
|
b, _ = self.model.axis_line(self.cad_names[second], zero_offsets={}, joint_angles={})
|
||||||
|
if abs(a @ b) < .999 or not axes[first] or not axes[second]:
|
||||||
|
continue
|
||||||
|
dots = np.asarray(axes[first]) @ np.asarray(axes[second]).T * np.sign(a @ b)
|
||||||
|
comparisons.append(dict(joints=[first, second], minimum_cosine=float(dots.min()),
|
||||||
|
maximum_cosine=float(dots.max()), reversed_for_all_candidates=bool(dots.max() < -.5)))
|
||||||
|
return dict(training_only=True, validation_used=False, comparisons=comparisons,
|
||||||
|
passed=not any(row['reversed_for_all_candidates'] for row in comparisons))
|
||||||
|
|
||||||
|
def initialize(self, branches):
|
||||||
|
roles = sorted(self.tags)
|
||||||
|
if len(branches) != min(6, len(roles)) or any(b not in (0, 1) for b in branches):
|
||||||
|
raise ValueError('draft_initialization_seed_invalid')
|
||||||
|
branch_by_role = {role: branches[min(i, len(branches)-1)] for i, role in enumerate(roles)}
|
||||||
|
initial = np.zeros(len(self.scale))
|
||||||
|
lines = {}
|
||||||
|
for name in self.joints:
|
||||||
|
task = task_for_joint(self.profile, name)
|
||||||
|
spec = self.profile.measurement.measurements[name]
|
||||||
|
channel = self.channels[name]
|
||||||
|
sign = joint_input_direction(self.profile, self.model, name)
|
||||||
|
for direction in range(2):
|
||||||
|
indices = self.motion_indices(name, direction, self.initialization_cycles[task.key])
|
||||||
|
if len(indices) < 3:
|
||||||
|
raise ValueError('draft_directional_training_missing:' + name)
|
||||||
|
indices, poses = self.motion_poses(indices, spec.child_role, branch_by_role[spec.child_role])
|
||||||
|
axis, phase = _axis(Rotation.from_matrix(poses[:, :3, :3]))
|
||||||
|
if (phase[-1]-phase[0])*sign < 0:
|
||||||
|
axis, phase = -axis, -phase
|
||||||
|
commands = self.commands[indices, channel]
|
||||||
|
phase -= np.interp(self.profile.command.baseline_values[channel], commands, phase)
|
||||||
|
self.set_initial_curve(initial, name, direction, np.interp(self.knots[name], commands, phase))
|
||||||
|
if direction == 0:
|
||||||
|
turns = poses[:, :3, :3] @ poses[0, :3, :3].T
|
||||||
|
blocks = np.eye(3)-turns
|
||||||
|
target = poses[:, :3, 3]-np.einsum('nij,j->ni', turns, poses[0, :3, 3])
|
||||||
|
lines[name] = (axis, np.linalg.lstsq(blocks.reshape(-1, 3), target.ravel(), rcond=1e-6)[0], indices[0])
|
||||||
|
angles = self.joint_angles(initial, self.commands, self.directions)
|
||||||
|
cad_axes, axes, cad_points, points = [], [], [], []
|
||||||
|
for name, (axis, point, index) in lines.items():
|
||||||
|
cad_axis, cad_point = self.model.axis_line(self.cad_names[name], zero_offsets={},
|
||||||
|
joint_angles={joint: values[index] for joint, values in angles.items()}, independent_mimic_angles=True)
|
||||||
|
cad_axes.append(cad_axis); axes.append(axis)
|
||||||
|
cad_points.append(cad_point); points.append(point)
|
||||||
|
if np.linalg.matrix_rank(np.asarray(cad_axes), tol=.01) < 2:
|
||||||
|
raise ValueError('draft_initialization_requires_nonparallel_observed_axes')
|
||||||
|
rotation, _ = Rotation.align_vectors(np.asarray(axes), np.asarray(cad_axes))
|
||||||
|
blocks = np.asarray([np.eye(3)-np.outer(axis, axis) for axis in axes])
|
||||||
|
targets = np.einsum('nij,nj->ni', blocks, np.asarray(points)-rotation.apply(cad_points))
|
||||||
|
initial[:3] = rotation.as_rotvec()
|
||||||
|
initial[3:6] = np.linalg.lstsq(blocks.reshape(-1, 3), targets.ravel(), rcond=1e-6)[0]
|
||||||
|
if self.training_initialization is not None:
|
||||||
|
hint = self.training_initialization
|
||||||
|
initial[:6] = hint['palm']
|
||||||
|
for name, mapping in hint['mappings'].items():
|
||||||
|
if name not in self.joints:
|
||||||
|
continue
|
||||||
|
if name in self.zero_columns:
|
||||||
|
initial[self.zero_columns[name]] = hint['offsets'][name]
|
||||||
|
for direction, values in enumerate((mapping.increasing_rad, mapping.decreasing_rad)):
|
||||||
|
self.set_initial_curve(initial, name, direction,
|
||||||
|
np.interp(self.knots[name], mapping.knots, values))
|
||||||
|
transforms = self.link_poses(initial, self.commands, self.directions)
|
||||||
|
for role, column in self.mount_columns.items():
|
||||||
|
index = self.observations[role][0][0]
|
||||||
|
initial[column:column+6] = pose_vector(np.linalg.inv(transforms[self.tag_links[role]][index])
|
||||||
|
@ self._pose(index, role, branch_by_role[role]))
|
||||||
|
self._initialization_costs[tuple(branches)] = self._refine_mount_guesses(initial, transforms,
|
||||||
|
observations=self.original_observations)
|
||||||
|
return initial
|
||||||
|
|
||||||
|
|
||||||
|
def fit_draft(bundle, candidate_completed=lambda _row: None):
|
||||||
|
"""Training-only representative; image precision is reported, not certified."""
|
||||||
|
candidates, audit = [], []
|
||||||
|
seed_width = min(6, len(bundle.tags))
|
||||||
|
for seed in product((0, 1), repeat=seed_width):
|
||||||
|
row = dict(seed=list(seed), converged=False)
|
||||||
|
try:
|
||||||
|
fit = bundle.fit(seed)
|
||||||
|
if not fit.success or not np.all(np.isfinite(fit.x)) or not np.all(np.isfinite(fit.fun)):
|
||||||
|
raise ValueError('draft_optimizer_did_not_converge')
|
||||||
|
bundle.output_mappings(fit.x)
|
||||||
|
residual = bundle.original_residual(fit.x) if hasattr(bundle, 'original_residual') else fit.fun
|
||||||
|
errors = np.sqrt(np.mean(np.sum(residual.reshape(-1, 4, 2)**2, axis=-1), axis=-1))
|
||||||
|
row.update(converged=True, coordinate_rms_px=float(np.sqrt(np.mean(residual**2))),
|
||||||
|
tag_rms_p95_px=float(np.percentile(errors, 95)), maximum_tag_rms_px=float(errors.max()),
|
||||||
|
original_image_precision_passed=bool(errors.max() <= 1.5),
|
||||||
|
evaluations=int(fit.nfev+fit.initialization_evaluations))
|
||||||
|
candidates.append((row, fit.x.copy()))
|
||||||
|
except (ValueError, np.linalg.LinAlgError) as error:
|
||||||
|
row['reason'] = str(error)
|
||||||
|
audit.append(row)
|
||||||
|
candidate_completed(row)
|
||||||
|
# A reviewable draft needs a bounded representative search, not a
|
||||||
|
# release claim that all geometric alternatives have been excluded.
|
||||||
|
if len(candidates) == 4:
|
||||||
|
break
|
||||||
|
if not candidates:
|
||||||
|
raise ValueError('draft_no_converged_training_candidate')
|
||||||
|
selected, parameters = min(candidates, key=lambda pair: pair[0]['coordinate_rms_px'])
|
||||||
|
jacobian = bundle.jacobian(parameters)*bundle.scale[None, :]
|
||||||
|
_u, singular, vectors = np.linalg.svd(jacobian, full_matrices=jacobian.shape[0] < jacobian.shape[1])
|
||||||
|
rows = getattr(bundle, 'original_residual_count', jacobian.shape[0])
|
||||||
|
rank = int(np.count_nonzero(singular > singular[0]*max(rows, jacobian.shape[1])*np.finfo(float).eps))
|
||||||
|
nullspace = vectors[rank:]
|
||||||
|
unresolved = [name for name, column in bundle.zero_columns.items()
|
||||||
|
if len(nullspace) and np.linalg.norm(nullspace[:, column]) > 1e-6]
|
||||||
|
if unresolved:
|
||||||
|
raise ValueError('draft_estimated_zero_unobservable:' + ','.join(unresolved))
|
||||||
|
zeros = {name: dict(estimated_deg=float(np.degrees(bundle.zero_offsets(parameters)[name])),
|
||||||
|
candidate_spread_deg=float(np.degrees(np.ptp([bundle.zero_offsets(value)[name]
|
||||||
|
for _, value in candidates])))) for name in bundle.zero_columns}
|
||||||
|
return parameters, dict(selected_seed=selected['seed'], selection='minimum_training_coordinate_rms',
|
||||||
|
validation_used_for_selection=False, precision_certified=False, candidates=audit,
|
||||||
|
solver_stopping_ftol=getattr(bundle, 'solver_ftol', 1e-9),
|
||||||
|
candidate_search_policy='uncertified_draft_up_to_four_converged_v1',
|
||||||
|
initializations_evaluated=len(audit), available_initializations=2**seed_width,
|
||||||
|
candidate_search_exhaustive=len(audit) == 2**seed_width,
|
||||||
|
estimated_zeros=zeros, local_parameter_rank=rank, parameter_count=len(bundle.scale)), candidates
|
||||||
@@ -0,0 +1,176 @@
|
|||||||
|
"""Training-only candidate sets for the CAD-constrained image graph."""
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from ..geometry.tag_pose.finger_chain_images import chain_candidate_seeds
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RawJointCandidate:
|
||||||
|
seed: tuple
|
||||||
|
parameters: np.ndarray
|
||||||
|
covariance: np.ndarray
|
||||||
|
rms_px: float
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
for field in ('parameters', 'covariance'):
|
||||||
|
value = np.array(getattr(self, field), copy=True)
|
||||||
|
value.flags.writeable = False
|
||||||
|
object.__setattr__(self, field, value)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RawJointFit:
|
||||||
|
candidates: tuple[RawJointCandidate, ...]
|
||||||
|
representative: int
|
||||||
|
uncertainty: dict
|
||||||
|
training_audit: tuple
|
||||||
|
training_selection: dict | None = None
|
||||||
|
|
||||||
|
|
||||||
|
def physical_outputs(bundle, parameters):
|
||||||
|
"""Physical quantities only: differing installation poses are not vetoes."""
|
||||||
|
offsets = bundle.zero_offsets(parameters)
|
||||||
|
values = bundle.mappings(parameters)
|
||||||
|
zeros = np.asarray([offsets[name] for name in bundle.joints])
|
||||||
|
# Absolute angles retain covariance between the zero and command curve.
|
||||||
|
commands = np.concatenate([(values[name]+offsets[name]).ravel() for name in bundle.joints])
|
||||||
|
axes, points = [], []
|
||||||
|
probes = [list(bundle.profile.command.baseline_values)]
|
||||||
|
for name, channel in bundle.channels.items():
|
||||||
|
for knot in bundle.knots[name]:
|
||||||
|
probe = probes[0].copy(); probe[channel] = knot; probes.append(probe)
|
||||||
|
probes = np.asarray(probes)
|
||||||
|
fk_points, fk_rotations = [], []
|
||||||
|
for direction in (0, 1):
|
||||||
|
poses = bundle.link_poses(parameters, probes, np.full(probes.shape, direction))
|
||||||
|
if direction == 0:
|
||||||
|
for name in bundle.joints:
|
||||||
|
joint = bundle.model.joints[bundle.cad_names[name]]
|
||||||
|
frame = poses[joint.parent][0] @ joint.origin
|
||||||
|
axes.append(frame[:3, :3] @ joint.axis)
|
||||||
|
points.append(frame[:3, 3])
|
||||||
|
for link in sorted(set(bundle.tag_links.values())):
|
||||||
|
fk_points.extend(poses[link][:, :3, 3])
|
||||||
|
fk_rotations.extend(poses[link][:, :3, :3].reshape(-1, 9))
|
||||||
|
return dict(zero_rad=zeros, command_rad=commands, axis=np.asarray(axes),
|
||||||
|
axis_point_m=np.asarray(points), fk_point_m=np.asarray(fk_points),
|
||||||
|
fk_rotation=np.asarray(fk_rotations))
|
||||||
|
|
||||||
|
|
||||||
|
def candidate_uncertainty(bundle, candidate):
|
||||||
|
from .raw_joint_uncertainty import physical_intervals
|
||||||
|
return physical_outputs(bundle, candidate.parameters), physical_intervals(bundle, candidate)
|
||||||
|
|
||||||
|
|
||||||
|
def certify_candidate_union(bundle, candidates):
|
||||||
|
if not candidates:
|
||||||
|
raise ValueError('raw_joint_no_training_candidate')
|
||||||
|
evaluated = [candidate_uncertainty(bundle, candidate) for candidate in candidates]
|
||||||
|
limits = dict(zero_rad=np.radians(1), command_rad=np.radians(1),
|
||||||
|
axis=2*np.sin(np.radians(3)/2), axis_point_m=.0015,
|
||||||
|
fk_point_m=.003, fk_rotation=2*np.sqrt(2)*np.sin(np.radians(1)/2))
|
||||||
|
audit, failures, radii = {}, [], {}
|
||||||
|
for key, limit in limits.items():
|
||||||
|
values = np.asarray([item[0][key] for item in evaluated])
|
||||||
|
widths = np.asarray([item[1][key] for item in evaluated])
|
||||||
|
radii[key] = []
|
||||||
|
for first in range(len(candidates)):
|
||||||
|
difference = values-values[first]
|
||||||
|
if difference.ndim == 3:
|
||||||
|
difference = np.linalg.norm(difference, axis=-1)
|
||||||
|
else:
|
||||||
|
difference = np.abs(difference)
|
||||||
|
radii[key].append(float(np.max(difference+widths)))
|
||||||
|
# Choose a physical minimax representative only AFTER forming the union.
|
||||||
|
# Pixel scores and installation uniqueness never choose the exported zero.
|
||||||
|
representative = int(np.argmin(np.max(np.asarray([np.asarray(radii[k])/v
|
||||||
|
for k, v in limits.items()]), axis=0)))
|
||||||
|
for key, limit in limits.items():
|
||||||
|
radius = radii[key][representative]
|
||||||
|
audit[key] = dict(maximum_union_radius=radius, limit=float(limit), passed=bool(radius <= limit))
|
||||||
|
if radius > limit:
|
||||||
|
failures.append(key)
|
||||||
|
values = np.asarray([item[0][key] for item in evaluated])
|
||||||
|
widths = np.asarray([item[1][key] for item in evaluated])
|
||||||
|
difference = values-values[representative]
|
||||||
|
difference = np.linalg.norm(difference,axis=-1) if difference.ndim == 3 else np.abs(difference)
|
||||||
|
affected = np.flatnonzero(np.max(difference+widths,axis=0) > limit)
|
||||||
|
if key in {'zero_rad','axis','axis_point_m'}:
|
||||||
|
names = {bundle.joints[int(index)] for index in affected}
|
||||||
|
elif key == 'command_rad':
|
||||||
|
labels = [name for name in bundle.joints for _ in range(2*len(bundle.knots[name]))]
|
||||||
|
names = {labels[int(index)] for index in affected}
|
||||||
|
else:
|
||||||
|
probes = 1+sum(len(nodes) for nodes in bundle.knots.values())
|
||||||
|
links = sorted(set(bundle.tag_links.values()))
|
||||||
|
affected_links = {links[(int(index)//probes)%len(links)] for index in affected}
|
||||||
|
names = {name for name in bundle.joints if bundle.model.joints[bundle.cad_names[name]].child in affected_links}
|
||||||
|
audit[key]['affected_joints'] = sorted(names)
|
||||||
|
audit[key]['required_observations'] = [dict(joint=name,
|
||||||
|
task=next(task.key for task in bundle.profile.motion.tasks if name in task.joints),
|
||||||
|
view=bundle.profile.measurement.measurements[name].view,
|
||||||
|
tag_roles=[bundle.profile.measurement.measurements[name].parent_role,
|
||||||
|
bundle.profile.measurement.measurements[name].child_role],
|
||||||
|
need=('bidirectional_steady_command_support' if key == 'command_rad' else
|
||||||
|
'simultaneous_link_observations_with_distinct_axis_or_held_pose_geometry'),
|
||||||
|
repeat_identical_trajectory_automatically=False) for name in sorted(names)]
|
||||||
|
if any(abs(v) > np.radians(20) for candidate in candidates
|
||||||
|
for v in bundle.zero_offsets(candidate.parameters).values()):
|
||||||
|
failures.append('absolute_zero_exceeds_20deg')
|
||||||
|
audit['passed'] = not failures
|
||||||
|
audit['failures'] = failures
|
||||||
|
audit['scope'] = 'training_command_grid_and_single_joint_fk; not_arbitrary_multiaxis'
|
||||||
|
return audit, representative
|
||||||
|
|
||||||
|
|
||||||
|
def fit_raw_joint_candidates(bundle, *, cancelled=lambda: False, candidate_completed=lambda _row: None,
|
||||||
|
workers=1):
|
||||||
|
"""The existing 64 seeds / 200 evaluations; validation is not an input."""
|
||||||
|
from concurrent.futures import ThreadPoolExecutor
|
||||||
|
if type(workers) is not int or not 1 <= workers <= 4:
|
||||||
|
raise ValueError('raw_joint_worker_budget_invalid')
|
||||||
|
candidates, audit = [], []
|
||||||
|
def solve(seed):
|
||||||
|
if cancelled():
|
||||||
|
raise ValueError('operator_abort:no_artifact_publication')
|
||||||
|
row = dict(seed=list(seed), converged=False, accepted_for_uncertainty=False)
|
||||||
|
try:
|
||||||
|
fit = bundle.fit(seed)
|
||||||
|
row.update(converged=bool(fit.success), evaluations=int(fit.nfev)+fit.initialization_evaluations,
|
||||||
|
joint_evaluations=int(fit.nfev), initialization_evaluations=fit.initialization_evaluations,
|
||||||
|
rms_px=float(np.sqrt(np.mean(fit.fun**2))))
|
||||||
|
if not fit.success:
|
||||||
|
raise ValueError('optimizer_budget_exhausted')
|
||||||
|
from ..geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS
|
||||||
|
errors = np.sqrt(np.mean(fit.fun.reshape(-1, 4, 2)**2, axis=(1, 2))*2)
|
||||||
|
row['maximum_training_tag_rms_px'] = float(np.max(errors))
|
||||||
|
if np.max(errors) > DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px:
|
||||||
|
row['training_excluded'] = 'declared_image_reprojection_limit'
|
||||||
|
return None, row
|
||||||
|
covariance = bundle.covariance(fit)
|
||||||
|
bundle.output_mappings(fit.x) # Reject nonmonotonic physical curves.
|
||||||
|
candidate = RawJointCandidate(tuple(seed), fit.x, covariance, row['rms_px'])
|
||||||
|
row['accepted_for_uncertainty'] = True
|
||||||
|
except (ValueError, np.linalg.LinAlgError) as error:
|
||||||
|
# Failure to initialize/converge cannot prove an alternative false.
|
||||||
|
row['unresolved_reason'] = str(error)
|
||||||
|
candidate = None
|
||||||
|
return candidate, row
|
||||||
|
with ThreadPoolExecutor(max_workers=workers) as pool:
|
||||||
|
for candidate, row in pool.map(solve, chain_candidate_seeds()):
|
||||||
|
if candidate is not None:
|
||||||
|
candidates.append(candidate)
|
||||||
|
audit.append(row)
|
||||||
|
candidate_completed(row)
|
||||||
|
if any(not row['accepted_for_uncertainty'] and not row.get('training_excluded') for row in audit):
|
||||||
|
raise ValueError('raw_joint_unresolved_training_candidates:see_raw_joint_training.json')
|
||||||
|
from .raw_joint_selection import reject_training_distinguishable_candidates
|
||||||
|
candidates, selection = reject_training_distinguishable_candidates(bundle, tuple(candidates))
|
||||||
|
excluded = {tuple(seed) for seed in selection.get('excluded_seeds', ())}
|
||||||
|
for row in audit:
|
||||||
|
if tuple(row['seed']) in excluded:
|
||||||
|
row.update(accepted_for_uncertainty=False, training_excluded='paired_training_loss_holm')
|
||||||
|
uncertainty, representative = certify_candidate_union(bundle, candidates)
|
||||||
|
return RawJointFit(candidates, representative, uncertainty, tuple(audit), selection)
|
||||||
@@ -0,0 +1,80 @@
|
|||||||
|
"""Training-only rejection of distinguishable image hypotheses.
|
||||||
|
|
||||||
|
Repeated holds and optimizer seeds are not independent observations. Compare
|
||||||
|
one median paired loss per distinct command/direction pose, with the existing
|
||||||
|
0.03 px resolution margin and Holm familywise correction. Survivors, including
|
||||||
|
physically different tied solutions, all proceed to covariance propagation.
|
||||||
|
"""
|
||||||
|
from collections import defaultdict
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.stats import ttest_1samp, wilcoxon
|
||||||
|
|
||||||
|
from ..geometry.tag_pose.image_model_selection import holm_adjusted_probabilities
|
||||||
|
from ..geometry.tag_pose.parameters import DEFAULT_REPROJECTION_TIE_PX
|
||||||
|
|
||||||
|
|
||||||
|
def _families(bundle, candidates):
|
||||||
|
families = []
|
||||||
|
for index, candidate in enumerate(candidates):
|
||||||
|
for family in families:
|
||||||
|
if all(np.max(np.abs(candidate.parameters-candidates[other].parameters)/bundle.scale) <= 1e-5
|
||||||
|
for other in family):
|
||||||
|
family.append(index)
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
families.append([index])
|
||||||
|
return families
|
||||||
|
|
||||||
|
|
||||||
|
def _probability(differences):
|
||||||
|
if len(differences) < 6 or not np.all(np.isfinite(differences)) or np.mean(differences) <= 0:
|
||||||
|
return 1.
|
||||||
|
if np.ptp(differences) < 1e-12:
|
||||||
|
return 2.**-len(differences)
|
||||||
|
mean = float(ttest_1samp(differences,0.,alternative='greater').pvalue)
|
||||||
|
rank = float(wilcoxon(differences,alternative='greater',zero_method='wilcox').pvalue)
|
||||||
|
return max(mean,rank) if np.isfinite(mean) and np.isfinite(rank) else 1.
|
||||||
|
|
||||||
|
|
||||||
|
def reject_training_distinguishable_candidates(bundle, candidates):
|
||||||
|
if not candidates:
|
||||||
|
return (), dict(policy='training_paired_pose_losses_holm_v1', families=[], comparisons=[])
|
||||||
|
families = _families(bundle,candidates)
|
||||||
|
anchor = min(range(len(candidates)),key=lambda i:candidates[i].rms_px)
|
||||||
|
predictions = [bundle.project_observations(c.parameters,bundle.commands,bundle.directions,bundle.observations)
|
||||||
|
for c in candidates]
|
||||||
|
probabilities, comparisons = [], []
|
||||||
|
for family_index,family in enumerate(families):
|
||||||
|
if anchor in family:
|
||||||
|
continue
|
||||||
|
for role,(indices,measured,_matrices,_objects) in bundle.observations.items():
|
||||||
|
base = np.sqrt(np.mean(np.sum((predictions[anchor][role]-measured)**2,axis=2),axis=1))
|
||||||
|
# Only commanded ancestors move this role. Other fingers' repeated
|
||||||
|
# holds cannot multiply the evidence for its installation branch.
|
||||||
|
ancestors = {joint.name for joint in bundle.chains[role]}
|
||||||
|
channels = sorted(bundle.channels[name] for name in bundle.joints if bundle.cad_names[name] in ancestors)
|
||||||
|
strata = defaultdict(list)
|
||||||
|
for local,index in enumerate(indices):
|
||||||
|
key = tuple((float(bundle.commands[index,ch]),int(bundle.directions[index,ch])) for ch in channels)
|
||||||
|
strata[key].append(local)
|
||||||
|
pvalues = []
|
||||||
|
for member in family:
|
||||||
|
other = np.sqrt(np.mean(np.sum((predictions[member][role]-measured)**2,axis=2),axis=1))
|
||||||
|
losses = other**2-(base+DEFAULT_REPROJECTION_TIE_PX)**2
|
||||||
|
differences = np.asarray([np.median(losses[items]) for items in strata.values()])
|
||||||
|
pvalues.append(_probability(differences))
|
||||||
|
index = len(comparisons)
|
||||||
|
probabilities.append((index,max(pvalues)))
|
||||||
|
comparisons.append(dict(family=family_index,role=role,pose_strata=len(strata),
|
||||||
|
probability=max(pvalues)))
|
||||||
|
excluded = set()
|
||||||
|
for index,adjusted in holm_adjusted_probabilities(probabilities):
|
||||||
|
comparisons[index]['holm_probability'] = adjusted
|
||||||
|
if adjusted < .01:
|
||||||
|
excluded.update(families[comparisons[index]['family']])
|
||||||
|
retained = tuple(c for i,c in enumerate(candidates) if i not in excluded)
|
||||||
|
return retained, dict(policy='training_paired_pose_losses_holm_v1',
|
||||||
|
training_comparison_anchor=list(candidates[anchor].seed), pixel_margin=DEFAULT_REPROJECTION_TIE_PX,
|
||||||
|
familywise_alpha=.01, families=[[list(candidates[i].seed) for i in family] for family in families],
|
||||||
|
excluded_seeds=[list(candidates[i].seed) for i in sorted(excluded)], comparisons=comparisons)
|
||||||
+91
@@ -0,0 +1,91 @@
|
|||||||
|
"""Exact first derivatives of physical outputs, including full covariance."""
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from ..geometry.tag_pose.urdf_command_images import rigid, right_jacobian, skew
|
||||||
|
|
||||||
|
|
||||||
|
def angle_derivatives(bundle, commands, directions):
|
||||||
|
derivatives = {}
|
||||||
|
width = len(bundle.scale)
|
||||||
|
for name, channel in bundle.channels.items():
|
||||||
|
jac = np.zeros((len(commands), width))
|
||||||
|
if name in bundle.zero_columns:
|
||||||
|
jac[:, bundle.zero_columns[name]] = 1.
|
||||||
|
weights = bundle.curve_weights(name, commands[:,channel], directions[:,channel])
|
||||||
|
for direction in range(2):
|
||||||
|
for node, column in enumerate(bundle.curve_columns[name][direction]):
|
||||||
|
if column >= 0:
|
||||||
|
jac[:,column] += weights[:,direction,node]
|
||||||
|
derivatives[bundle.cad_names[name]] = jac
|
||||||
|
return derivatives
|
||||||
|
|
||||||
|
|
||||||
|
def pose_derivatives(bundle, parameters, commands, directions):
|
||||||
|
poses = bundle.link_poses(parameters, commands, directions)
|
||||||
|
angles = angle_derivatives(bundle, commands, directions)
|
||||||
|
shape = (len(commands), 3, len(parameters))
|
||||||
|
position, rotation = np.zeros(shape), np.zeros(shape)
|
||||||
|
position[:,:,3:6] = np.eye(3)
|
||||||
|
rotation[:,:,:3] = rigid(parameters[:6])[:3,:3] @ right_jacobian(parameters[:3])
|
||||||
|
jacobians = {bundle.model.root_link: (position, rotation)}
|
||||||
|
frames = {}
|
||||||
|
for chain in bundle.chains.values():
|
||||||
|
for joint in chain:
|
||||||
|
if joint.child in jacobians:
|
||||||
|
continue
|
||||||
|
parent_position, parent_rotation = jacobians[joint.parent]
|
||||||
|
frame = poses[joint.parent] @ joint.origin
|
||||||
|
displacement = frame[:,:3,3]-poses[joint.parent][:,:3,3]
|
||||||
|
point = parent_position-skew(displacement) @ parent_rotation
|
||||||
|
angular = parent_rotation.copy()
|
||||||
|
if joint.kind != 'fixed':
|
||||||
|
axis = frame[:,:3,:3] @ joint.axis
|
||||||
|
angular += axis[:,:,None]*angles[joint.name][:,None,:]
|
||||||
|
frames[joint.name] = (point, -skew(axis) @ parent_rotation)
|
||||||
|
jacobians[joint.child] = point, angular
|
||||||
|
return poses, jacobians, frames
|
||||||
|
|
||||||
|
|
||||||
|
def physical_intervals(bundle, candidate):
|
||||||
|
parameters, covariance = candidate.parameters, candidate.covariance
|
||||||
|
def interval(jac):
|
||||||
|
# Scalar or vector three-sigma radius, preserving every correlation.
|
||||||
|
shape = jac.shape[:-1]
|
||||||
|
flat = jac.reshape(-1, len(parameters))
|
||||||
|
from scipy.sparse import csr_matrix
|
||||||
|
sparse = csr_matrix(flat)
|
||||||
|
variance = np.asarray(sparse.multiply(sparse @ covariance).sum(axis=1)).ravel().reshape(shape)
|
||||||
|
return 3*np.sqrt(np.maximum(variance, 0.).sum(axis=-1) if len(shape)>1 else np.maximum(variance, 0.))
|
||||||
|
zeros = np.zeros((len(bundle.joints), len(parameters)))
|
||||||
|
commands = []
|
||||||
|
for i, name in enumerate(bundle.joints):
|
||||||
|
if name in bundle.zero_columns:
|
||||||
|
zeros[i,bundle.zero_columns[name]] = 1.
|
||||||
|
for direction in (0, 1):
|
||||||
|
basis = bundle.curve_weights(name, bundle.knots[name],
|
||||||
|
np.full(len(bundle.knots[name]), direction))
|
||||||
|
for weights in basis:
|
||||||
|
row = zeros[i].copy()
|
||||||
|
for column, weight in zip(bundle.curve_columns[name].ravel(), weights.ravel()):
|
||||||
|
if column >= 0:
|
||||||
|
row[column] += weight
|
||||||
|
commands.append(row)
|
||||||
|
result = dict(zero_rad=interval(zeros), command_rad=interval(np.asarray(commands)))
|
||||||
|
probes = [list(bundle.profile.command.baseline_values)]
|
||||||
|
for name, channel in bundle.channels.items():
|
||||||
|
for knot in bundle.knots[name]:
|
||||||
|
probe = probes[0].copy(); probe[channel] = knot; probes.append(probe)
|
||||||
|
probes = np.asarray(probes)
|
||||||
|
points, rotations = [], []
|
||||||
|
for direction in (0,1):
|
||||||
|
poses, jacobians, frames = pose_derivatives(bundle, parameters, probes, np.full(probes.shape, direction))
|
||||||
|
if direction == 0:
|
||||||
|
result['axis_point_m'] = interval(np.asarray([frames[bundle.cad_names[n]][0][0] for n in bundle.joints]))
|
||||||
|
result['axis'] = interval(np.asarray([frames[bundle.cad_names[n]][1][0] for n in bundle.joints]))
|
||||||
|
for link in sorted(set(bundle.tag_links.values())):
|
||||||
|
point, angular = jacobians[link]
|
||||||
|
points.extend(interval(point).tolist())
|
||||||
|
# ||dR||_F = sqrt(2)||angular velocity||, exact rotation metric.
|
||||||
|
rotations.extend((np.sqrt(2)*interval(angular)).tolist())
|
||||||
|
result['fk_point_m'], result['fk_rotation'] = np.asarray(points), np.asarray(rotations)
|
||||||
|
return result
|
||||||
@@ -0,0 +1,88 @@
|
|||||||
|
"""Constrained batch updates of the existing image objective, without motion.
|
||||||
|
|
||||||
|
The caller supplies the new batch's camera geometry and immutable reference
|
||||||
|
parameters. Unselected joints and tag installations cannot move in the fit.
|
||||||
|
"""
|
||||||
|
import numpy as np
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
|
||||||
|
|
||||||
|
def check_update_zero_observability(bundle, parameters, active, joints):
|
||||||
|
"""Reject zero gauges, conditional on the explicitly frozen reference."""
|
||||||
|
jacobian = bundle.jacobian(parameters)[:, active]*bundle.scale[active]
|
||||||
|
_, singular, vectors = np.linalg.svd(jacobian,
|
||||||
|
full_matrices=jacobian.shape[0] < jacobian.shape[1])
|
||||||
|
rows = getattr(bundle, 'original_residual_count', jacobian.shape[0])
|
||||||
|
rank = int(np.count_nonzero(singular > singular[0]*max(rows, len(active))*np.finfo(float).eps))
|
||||||
|
nullspace = vectors[rank:]
|
||||||
|
positions = {column: i for i, column in enumerate(active)}
|
||||||
|
unresolved = [name for name in joints if name in bundle.zero_columns
|
||||||
|
and (bundle.zero_columns[name] not in positions or (len(nullspace)
|
||||||
|
and np.linalg.norm(nullspace[:, positions[bundle.zero_columns[name]]]) > 1e-6))]
|
||||||
|
if unresolved:
|
||||||
|
raise ValueError('batch_update_zero_unobservable:' + ','.join(unresolved))
|
||||||
|
return rank
|
||||||
|
|
||||||
|
|
||||||
|
def transfer_parameters(source, parameters, target):
|
||||||
|
"""Copy semantic parameters; array positions may differ between scopes."""
|
||||||
|
result = np.zeros(len(target.scale))
|
||||||
|
result[:6] = parameters[:6]
|
||||||
|
for name, columns in target.curve_columns.items():
|
||||||
|
if not np.array_equal(source.knots[name], target.knots[name]):
|
||||||
|
raise ValueError('batch_update_training_knots_changed:' + name)
|
||||||
|
mask = columns >= 0
|
||||||
|
original = source.curve_columns[name]
|
||||||
|
if not np.array_equal(mask, original >= 0):
|
||||||
|
raise ValueError('batch_update_baseline_changed:' + name)
|
||||||
|
result[columns[mask]] = parameters[original[mask]]
|
||||||
|
for name, column in target.zero_columns.items():
|
||||||
|
result[column] = source.zero_offsets(parameters)[name]
|
||||||
|
for role, column in target.mount_columns.items():
|
||||||
|
if source.tag_links[role] != target.tag_links[role]:
|
||||||
|
raise ValueError('batch_update_tag_link_changed:' + role)
|
||||||
|
original = source.mount_columns[role]
|
||||||
|
result[column:column+6] = parameters[original:original+6]
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def fit_joint_update(bundle, initial, joints, mount_roles, *, fit_palm=True):
|
||||||
|
"""Use the shared bounded solver while keeping the reference hand fixed."""
|
||||||
|
if not set(joints) <= set(bundle.joints) or not set(mount_roles) <= set(bundle.tags):
|
||||||
|
raise ValueError('batch_update_scope_invalid')
|
||||||
|
columns = list(range(6)) if fit_palm else []
|
||||||
|
for name in joints:
|
||||||
|
if name in bundle.zero_columns:
|
||||||
|
columns.append(bundle.zero_columns[name])
|
||||||
|
columns.extend(bundle.curve_columns[name][bundle.curve_columns[name] >= 0])
|
||||||
|
for role in mount_roles:
|
||||||
|
start = bundle.mount_columns[role]
|
||||||
|
columns.extend(range(start, start+6))
|
||||||
|
proposed = np.asarray(sorted(set(columns)), dtype=int)
|
||||||
|
# Unobserved increments cannot acquire invented travel through optimizer
|
||||||
|
# drift. Retain their input values and report that they were not fitted.
|
||||||
|
supported = np.linalg.norm(bundle.jacobian(initial)[:, proposed], axis=0) > 1e-8
|
||||||
|
active = proposed[supported]
|
||||||
|
if not len(active):
|
||||||
|
raise ValueError('batch_update_no_observed_parameters')
|
||||||
|
def unpack(values):
|
||||||
|
parameters = initial.copy()
|
||||||
|
parameters[active] = values
|
||||||
|
return parameters
|
||||||
|
options = dict(bundle.solver_options)
|
||||||
|
lower, upper = options.pop('bounds')
|
||||||
|
fit = least_squares(lambda x: bundle.residual(unpack(x)), initial[active],
|
||||||
|
jac=lambda x: bundle.solver_jacobian(unpack(x))[:, active],
|
||||||
|
bounds=(lower[active], upper[active]), x_scale=bundle.scale[active],
|
||||||
|
max_nfev=100, ftol=bundle.solver_ftol, **options)
|
||||||
|
parameters = unpack(fit.x)
|
||||||
|
rank = check_update_zero_observability(bundle, parameters, active, joints)
|
||||||
|
derived = {row['column'] for rows in getattr(bundle, 'turnaround_constraints', {}).values() for row in rows}
|
||||||
|
audit = dict(converged=bool(fit.success), evaluations=int(fit.nfev),
|
||||||
|
coordinate_rms_px=float(np.sqrt(np.mean(bundle.original_residual(parameters)**2))),
|
||||||
|
active_columns=active.tolist(),
|
||||||
|
derived_turnaround_columns=sorted(derived.intersection(proposed.tolist())),
|
||||||
|
unobserved_retained_columns=[int(column) for column in proposed[~supported] if column not in derived],
|
||||||
|
validation_used=False, fixed_cad_axes=True, conditional_parameter_rank=rank,
|
||||||
|
observability_condition='unselected_joints_and_tag_mounts_fixed')
|
||||||
|
return parameters, audit
|
||||||
+141
@@ -0,0 +1,141 @@
|
|||||||
|
"""Independent per-image angles; shared geometry and command maps stay frozen.
|
||||||
|
|
||||||
|
Images with the same observation layout are evaluated together. Their angle
|
||||||
|
variables remain separate blocks, so vectorization shares no measurements and
|
||||||
|
cannot turn the third round into a geometry or mapping fit.
|
||||||
|
"""
|
||||||
|
from collections import defaultdict
|
||||||
|
from dataclasses import asdict
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.optimize import least_squares
|
||||||
|
from scipy.sparse import coo_matrix
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
from ..domain.motion_path import task_segment
|
||||||
|
from ..urdf.acceptance import angular_metrics
|
||||||
|
from ..urdf.tag_acceptance import POSITION_P95_LIMIT_M
|
||||||
|
from ..geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS
|
||||||
|
from ..geometry.tag_pose.ippe import square_object_points
|
||||||
|
|
||||||
|
|
||||||
|
def _groups(bundle, rows):
|
||||||
|
training_ids = {(r['view'], r['image_stamp_ns']) for r in bundle.rows}
|
||||||
|
matrices = {r['view']: np.asarray(r['camera_matrix']) for r in bundle.rows}
|
||||||
|
tasks = {task.key: task for task in bundle.profile.motion.tasks}
|
||||||
|
seen, groups = set(), defaultdict(list)
|
||||||
|
for row in rows:
|
||||||
|
if row.get('sample_phase') != 'steady' or row.get('accepted_sample_window_open') is not True:
|
||||||
|
continue
|
||||||
|
identity = row['view'], row['image_stamp_ns']
|
||||||
|
if identity in training_ids or identity in seen or row.get('cycle') != 3:
|
||||||
|
raise ValueError('raw_joint_holdout_not_independent')
|
||||||
|
seen.add(identity)
|
||||||
|
if not np.array_equal(row['camera_matrix'], matrices.get(row['view'])):
|
||||||
|
raise ValueError('raw_joint_holdout_intrinsics_changed')
|
||||||
|
task = tasks[row['task_name']]
|
||||||
|
segment = task_segment(task, row['segment_key']) if row.get('segment_key') else None
|
||||||
|
roles = tuple(role for role, (view, _) in bundle.tags.items()
|
||||||
|
if view == row['view'] and role in row.get('tags', {}))
|
||||||
|
ancestors = {joint.name for role in roles for joint in bundle.chains[role]}
|
||||||
|
names = tuple(name for name in (segment.joints if segment else task.joints)
|
||||||
|
if bundle.cad_names[name] in ancestors)
|
||||||
|
if roles:
|
||||||
|
groups[row['view'], names, roles].append(row)
|
||||||
|
return groups
|
||||||
|
|
||||||
|
|
||||||
|
def _measure_group(bundle, candidate, rows, names, roles, cancelled):
|
||||||
|
count, width = len(rows), len(names)
|
||||||
|
commands = np.asarray([row['command_vector'] for row in rows])
|
||||||
|
directions = bundle._directions(rows)
|
||||||
|
observations = {}
|
||||||
|
for role in roles:
|
||||||
|
pixels = np.asarray([row['tags'][role]['corners_xy'] for row in rows], dtype=float)
|
||||||
|
if pixels.shape != (count,4,2) or not np.all(np.isfinite(pixels)):
|
||||||
|
raise ValueError('raw_joint_holdout_corners_invalid')
|
||||||
|
observations[role] = (np.arange(count), pixels,
|
||||||
|
np.asarray([row['camera_matrix'] for row in rows]), square_object_points(bundle.tags[role][1].size_m))
|
||||||
|
def corrections(values):
|
||||||
|
matrix = values.reshape(count,width)
|
||||||
|
return {name: matrix[:,index] for index,name in enumerate(names)}
|
||||||
|
def residual(values):
|
||||||
|
if cancelled():
|
||||||
|
raise ValueError('operator_abort:no_artifact_publication')
|
||||||
|
predicted = bundle.project_observations(candidate.parameters, commands, directions,
|
||||||
|
observations, corrections(values))
|
||||||
|
return np.concatenate([(predicted[role]-data[1]).ravel() for role,data in observations.items()])
|
||||||
|
forward = residual(np.zeros(count*width)).reshape(len(roles),count,8)
|
||||||
|
if not width:
|
||||||
|
return None, forward, forward, None, None, None
|
||||||
|
# Every image's pixels depend only on that image's active angle variables.
|
||||||
|
row_indices = np.arange(len(roles)*count*8)
|
||||||
|
columns = np.tile(np.repeat(np.arange(count),8),len(roles))
|
||||||
|
sparsity = coo_matrix((np.ones(len(row_indices)*width),
|
||||||
|
(np.repeat(row_indices,width), (columns[:,None]*width+np.arange(width)).ravel())),
|
||||||
|
shape=(len(row_indices),count*width)).tocsr()
|
||||||
|
fit = least_squares(residual, np.zeros(count*width), jac_sparsity=sparsity,
|
||||||
|
max_nfev=20, ftol=1e-9, xtol=1e-9, gtol=1e-8,
|
||||||
|
tr_options=dict(atol=1e-12,btol=1e-12))
|
||||||
|
if not fit.success:
|
||||||
|
raise ValueError('raw_joint_holdout_angle_measurement_failed')
|
||||||
|
local_jac = np.zeros((count,len(roles)*8,width))
|
||||||
|
for role_index in range(len(roles)):
|
||||||
|
indices = role_index*count*8+np.arange(count*8).reshape(count,8)
|
||||||
|
for column in range(width):
|
||||||
|
selected = fit.jac[indices, (np.arange(count)*width+column)[:,None]]
|
||||||
|
local_jac[:,role_index*8:(role_index+1)*8,column] = (
|
||||||
|
selected.toarray() if hasattr(selected, 'toarray') else np.asarray(selected))
|
||||||
|
information = np.einsum('nki,nkj->nij',local_jac,local_jac)
|
||||||
|
eigen = np.linalg.eigvalsh(information)
|
||||||
|
if np.any(eigen[:,0] <= np.maximum(1.,eigen[:,-1])*np.finfo(float).eps*width):
|
||||||
|
raise ValueError('raw_joint_holdout_angle_unobservable')
|
||||||
|
measured = fit.fun.reshape(len(roles),count,8)
|
||||||
|
variance = np.maximum(.03**2,np.sum(measured**2,axis=(0,2))/max(1,len(roles)*8-width))
|
||||||
|
confidence = 3*np.sqrt(np.diagonal(np.linalg.inv(information),axis1=1,axis2=2)*variance[:,None])
|
||||||
|
predicted_poses = bundle.link_poses(candidate.parameters,commands,directions)
|
||||||
|
measured_poses = bundle.link_poses(candidate.parameters,commands,directions,corrections(fit.x))
|
||||||
|
return fit.x.reshape(count,width), forward, measured, confidence, predicted_poses, measured_poses
|
||||||
|
|
||||||
|
|
||||||
|
def validate_raw_joint_candidate(bundle, candidate, rows, *, cancelled=lambda: False,
|
||||||
|
report_joints=None):
|
||||||
|
names_to_report = tuple(bundle.joints if report_joints is None else report_joints)
|
||||||
|
if not names_to_report or not set(names_to_report) <= set(bundle.joints):
|
||||||
|
raise ValueError('raw_joint_holdout_report_scope_invalid')
|
||||||
|
angles, positions, rotations = defaultdict(list), defaultdict(list), defaultdict(list)
|
||||||
|
primary_counts, pixels, forward_pixels, uncertainty = defaultdict(int), [], [], []
|
||||||
|
for (view,names,roles), group in _groups(bundle,rows).items():
|
||||||
|
values, forward, measured, confidence, predicted, observed = _measure_group(
|
||||||
|
bundle,candidate,group,names,roles,cancelled)
|
||||||
|
forward_pixels.extend(np.sqrt(np.mean(forward**2,axis=2)*2).ravel().tolist())
|
||||||
|
pixels.extend(np.sqrt(np.mean(measured**2,axis=2)*2).ravel().tolist())
|
||||||
|
if values is None:
|
||||||
|
continue
|
||||||
|
uncertainty.extend(confidence.ravel().tolist())
|
||||||
|
for column,name in enumerate(names):
|
||||||
|
angles[name].extend(values[:,column].tolist())
|
||||||
|
if bundle.profile.measurement.measurements[name].view == view:
|
||||||
|
primary_counts[name] += len(group)
|
||||||
|
link = bundle.model.joints[bundle.cad_names[name]].child
|
||||||
|
a,b = predicted[link],observed[link]
|
||||||
|
positions[name].extend(np.linalg.norm(a[:,:3,3]-b[:,:3,3],axis=1).tolist())
|
||||||
|
rotations[name].extend(Rotation.from_matrix(a[:,:3,:3].transpose(0,2,1)@b[:,:3,:3]).magnitude().tolist())
|
||||||
|
reports = {}
|
||||||
|
for name in names_to_report:
|
||||||
|
if primary_counts[name] < bundle.profile.acquisition.minimum_valid_samples:
|
||||||
|
raise ValueError('raw_joint_holdout_measurements_missing:'+name)
|
||||||
|
angular,orientation = angular_metrics(angles[name]),angular_metrics(rotations[name])
|
||||||
|
position = float(np.percentile(positions[name],95))
|
||||||
|
reports[name] = dict(angular=asdict(angular),orientation=asdict(orientation),
|
||||||
|
position_p95_m=position,samples=len(angles[name]),primary_samples=primary_counts[name],
|
||||||
|
passed=angular.passed and orientation.passed and position <= POSITION_P95_LIMIT_M)
|
||||||
|
passed = (bool(reports) and all(r['passed'] for r in reports.values())
|
||||||
|
and max(uncertainty,default=np.inf) <= np.radians(1)
|
||||||
|
and max(pixels,default=np.inf) <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px)
|
||||||
|
return dict(passed=bool(passed),joints=reports,
|
||||||
|
maximum_measurement_confidence_rad=max(uncertainty,default=None),
|
||||||
|
maximum_measurement_reprojection_px=max(pixels,default=None),
|
||||||
|
frozen_forward_reprojection_p95_px=float(np.percentile(forward_pixels,95)) if forward_pixels else None,
|
||||||
|
holdout_cycle=3,shared_parameters_updated=False,
|
||||||
|
policy='frozen_forward_commands_and_independent_image_angles_v1')
|
||||||
@@ -15,6 +15,7 @@ import numpy as np
|
|||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
from .curve import isotonic_nonincreasing
|
from .curve import isotonic_nonincreasing
|
||||||
|
from ..domain.capture_plan import LEGACY_TRAINING, LEGACY_HOLDOUT
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -37,6 +38,8 @@ class RotationCurve:
|
|||||||
sdk_to_joint_direction: int
|
sdk_to_joint_direction: int
|
||||||
training_sample_ids: frozenset[str]
|
training_sample_ids: frozenset[str]
|
||||||
maximum_monotonic_correction_rad: float = 0.0
|
maximum_monotonic_correction_rad: float = 0.0
|
||||||
|
training_cycles: tuple[int, ...] = LEGACY_TRAINING
|
||||||
|
holdout_cycle: int = LEGACY_HOLDOUT
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def angle_rad(self) -> tuple[float, ...]:
|
def angle_rad(self) -> tuple[float, ...]:
|
||||||
@@ -59,7 +62,7 @@ class RotationCurve:
|
|||||||
return angle, off_axis_error
|
return angle, off_axis_error
|
||||||
|
|
||||||
def holdout_errors(self, observations: Sequence[RotationObservation]) -> tuple[float, ...]:
|
def holdout_errors(self, observations: Sequence[RotationObservation]) -> tuple[float, ...]:
|
||||||
_validate_observations(observations, cycles={3})
|
_validate_observations(observations, cycles={self.holdout_cycle})
|
||||||
if any(row.sample_id in self.training_sample_ids for row in observations):
|
if any(row.sample_id in self.training_sample_ids for row in observations):
|
||||||
raise ValueError("holdout image identity overlaps training")
|
raise ValueError("holdout image identity overlaps training")
|
||||||
reference = Rotation.from_quat(self.reference_xyzw)
|
reference = Rotation.from_quat(self.reference_xyzw)
|
||||||
@@ -78,6 +81,25 @@ def _rotation(quaternion: Sequence[float]) -> Rotation:
|
|||||||
return Rotation.from_quat(values)
|
return Rotation.from_quat(values)
|
||||||
|
|
||||||
|
|
||||||
|
def common_directional_input_support(values, directions) -> tuple[float, float]:
|
||||||
|
"""Training-only support shared by fitting and live acquisition checks."""
|
||||||
|
inputs = np.asarray(values, dtype=float)
|
||||||
|
if inputs.ndim != 1 or len(inputs) != len(directions) or not np.all(np.isfinite(inputs)):
|
||||||
|
raise ValueError("directional SDK inputs must be finite matching vectors")
|
||||||
|
if any(direction not in {"increasing", "decreasing"} for direction in directions):
|
||||||
|
raise ValueError("invalid SDK support direction")
|
||||||
|
supports = []
|
||||||
|
for direction in ("increasing", "decreasing"):
|
||||||
|
selected = inputs[np.asarray([value == direction for value in directions], dtype=bool)]
|
||||||
|
if not selected.size:
|
||||||
|
raise ValueError("both directional SDK supports are required")
|
||||||
|
supports.append((float(np.min(selected)), float(np.max(selected))))
|
||||||
|
lower, upper = max(s[0] for s in supports), min(s[1] for s in supports)
|
||||||
|
if upper <= lower:
|
||||||
|
raise ValueError("directional SDK supports do not overlap")
|
||||||
|
return lower, upper
|
||||||
|
|
||||||
|
|
||||||
def _validate_observations(rows: Sequence[RotationObservation], *, cycles: set[int]) -> None:
|
def _validate_observations(rows: Sequence[RotationObservation], *, cycles: set[int]) -> None:
|
||||||
if not rows:
|
if not rows:
|
||||||
raise ValueError("rotation observations are missing")
|
raise ValueError("rotation observations are missing")
|
||||||
@@ -95,6 +117,9 @@ def _validate_observations(rows: Sequence[RotationObservation], *, cycles: set[i
|
|||||||
def fit_rotation_curve(
|
def fit_rotation_curve(
|
||||||
observations: Sequence[RotationObservation], *, input_domain: str,
|
observations: Sequence[RotationObservation], *, input_domain: str,
|
||||||
sdk_to_joint_direction: int, knot_count: int = 65,
|
sdk_to_joint_direction: int, knot_count: int = 65,
|
||||||
|
reference_xyzw: Sequence[float] | None = None,
|
||||||
|
training_cycles: tuple[int, ...] = LEGACY_TRAINING,
|
||||||
|
holdout_cycle: int = LEGACY_HOLDOUT,
|
||||||
) -> RotationCurve:
|
) -> RotationCurve:
|
||||||
"""Fit three complete training cycles without integerizing physical inputs.
|
"""Fit three complete training cycles without integerizing physical inputs.
|
||||||
|
|
||||||
@@ -106,41 +131,25 @@ def fit_rotation_curve(
|
|||||||
raise ValueError("an explicit native SDK input domain is required")
|
raise ValueError("an explicit native SDK input domain is required")
|
||||||
if sdk_to_joint_direction not in {-1, 1} or knot_count < 32:
|
if sdk_to_joint_direction not in {-1, 1} or knot_count < 32:
|
||||||
raise ValueError("invalid direction or curve knot count")
|
raise ValueError("invalid direction or curve knot count")
|
||||||
_validate_observations(observations, cycles={0, 1, 2})
|
if len(training_cycles) < 2 or holdout_cycle in training_cycles:
|
||||||
required = {(cycle, direction) for cycle in range(3) for direction in ("increasing", "decreasing")}
|
raise ValueError("at least two independent training cycles and an isolated holdout are required")
|
||||||
|
_validate_observations(observations, cycles=set(training_cycles))
|
||||||
|
required = {(cycle, direction) for cycle in training_cycles for direction in ("increasing", "decreasing")}
|
||||||
groups = {(row.cycle, row.direction) for row in observations}
|
groups = {(row.cycle, row.direction) for row in observations}
|
||||||
if groups != required:
|
if groups != required:
|
||||||
raise ValueError("three complete bidirectional training cycles are required")
|
raise ValueError("complete declared bidirectional training cycles are required")
|
||||||
for cycle, direction in required:
|
for cycle, direction in required:
|
||||||
if sum(row.cycle == cycle and row.direction == direction for row in observations) < 40:
|
if sum(row.cycle == cycle and row.direction == direction for row in observations) < 40:
|
||||||
raise ValueError("each training direction requires 40 unique observations")
|
raise ValueError("each training direction requires 40 unique observations")
|
||||||
inputs = np.asarray([row.input_value for row in observations])
|
inputs = np.asarray([row.input_value for row in observations])
|
||||||
if input_domain.endswith("u8") and (np.min(inputs) < 0 or np.max(inputs) > 255):
|
if input_domain.endswith("u8") and (np.min(inputs) < 0 or np.max(inputs) > 255):
|
||||||
raise ValueError("byte observations are outside the SDK domain")
|
raise ValueError("byte observations are outside the SDK domain")
|
||||||
rotations = Rotation.from_quat([row.quaternion_xyzw for row in observations])
|
frame = fit_rotation_frame(observations, sdk_to_joint_direction=sdk_to_joint_direction,
|
||||||
# Use one observed reference, never assert that its input is a CAD zero.
|
reference_xyzw=reference_xyzw, training_cycles=training_cycles)
|
||||||
reference = rotations[int(np.argmin(inputs))]
|
reference = Rotation.from_quat(frame.reference_xyzw)
|
||||||
vectors = (reference.inv() * rotations).as_rotvec()
|
axis = np.asarray(frame.axis_xyz)
|
||||||
_, singular, vh = np.linalg.svd(vectors, full_matrices=False)
|
angles = (reference.inv() * Rotation.from_quat([r.quaternion_xyzw for r in observations])).as_rotvec() @ axis
|
||||||
if not singular.size or singular[0] < math.radians(1):
|
lower, upper = common_directional_input_support(inputs, [row.direction for row in observations])
|
||||||
raise ValueError("hinge rotation is not observable")
|
|
||||||
axis = vh[0]
|
|
||||||
angles = vectors @ axis
|
|
||||||
trend = float((inputs - np.mean(inputs)) @ (angles - np.mean(angles)))
|
|
||||||
if abs(trend) <= 1e-12:
|
|
||||||
raise ValueError("SDK/visual motion direction is not observable")
|
|
||||||
if math.copysign(1, trend) != sdk_to_joint_direction:
|
|
||||||
axis, angles = -axis, -angles
|
|
||||||
if np.ptp(angles) >= math.pi - math.radians(1):
|
|
||||||
raise ValueError("hinge travel exceeds this rotation primitive's unambiguous interval")
|
|
||||||
supports = []
|
|
||||||
for direction in ("increasing", "decreasing"):
|
|
||||||
selected = [i for i, row in enumerate(observations) if row.direction == direction]
|
|
||||||
x = inputs[selected]
|
|
||||||
supports.append((float(np.min(x)), float(np.max(x))))
|
|
||||||
lower, upper = max(s[0] for s in supports), min(s[1] for s in supports)
|
|
||||||
if upper <= lower:
|
|
||||||
raise ValueError("directional SDK supports do not overlap")
|
|
||||||
knots = np.linspace(lower, upper, knot_count)
|
knots = np.linspace(lower, upper, knot_count)
|
||||||
branches = {}
|
branches = {}
|
||||||
maximum_correction = 0.0
|
maximum_correction = 0.0
|
||||||
@@ -156,4 +165,50 @@ def fit_rotation_curve(
|
|||||||
branches[direction] = tuple(float(v) for v in np.interp(knots, unique, projected))
|
branches[direction] = tuple(float(v) for v in np.interp(knots, unique, projected))
|
||||||
return RotationCurve(input_domain, tuple(float(v) for v in knots),
|
return RotationCurve(input_domain, tuple(float(v) for v in knots),
|
||||||
branches["increasing"], branches["decreasing"], tuple(reference.as_quat()),
|
branches["increasing"], branches["decreasing"], tuple(reference.as_quat()),
|
||||||
tuple(axis), sdk_to_joint_direction, frozenset(row.sample_id for row in observations), maximum_correction)
|
tuple(axis), sdk_to_joint_direction, frozenset(row.sample_id for row in observations), maximum_correction,
|
||||||
|
tuple(training_cycles), holdout_cycle)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class VisualRotationFrame:
|
||||||
|
"""Training-only hinge frame, independent of any SDK-to-angle regression."""
|
||||||
|
reference_xyzw: tuple[float, ...]
|
||||||
|
axis_xyz: tuple[float, ...]
|
||||||
|
training_sample_ids: tuple[str, ...]
|
||||||
|
training_cycles: tuple[int, ...] = LEGACY_TRAINING
|
||||||
|
|
||||||
|
@property
|
||||||
|
def circle(self):
|
||||||
|
return {"space": "relative_rotation_3d", "reference_quaternion_xyzw": self.reference_xyzw,
|
||||||
|
"axis_xyz": self.axis_xyz, "training_cycles": list(self.training_cycles),
|
||||||
|
"training_sample_ids": self.training_sample_ids}
|
||||||
|
|
||||||
|
|
||||||
|
def fit_rotation_frame(observations, *, sdk_to_joint_direction, reference_xyzw=None,
|
||||||
|
training_cycles=LEGACY_TRAINING):
|
||||||
|
_validate_observations(observations, cycles=set(training_cycles))
|
||||||
|
for cycle in training_cycles:
|
||||||
|
for direction in ("increasing", "decreasing"):
|
||||||
|
if sum(r.cycle == cycle and r.direction == direction for r in observations) < 40:
|
||||||
|
raise ValueError("visual frame requires 40 training images per cycle/direction")
|
||||||
|
if sdk_to_joint_direction not in {-1, 1}:
|
||||||
|
raise ValueError("visual frame requires a signed physical direction")
|
||||||
|
inputs = np.asarray([row.input_value for row in observations])
|
||||||
|
rotations = Rotation.from_quat([row.quaternion_xyzw for row in observations])
|
||||||
|
# Use one observed reference, never assert that its input is a CAD zero.
|
||||||
|
reference = rotations[int(np.argmin(inputs))] if reference_xyzw is None else _rotation(reference_xyzw)
|
||||||
|
vectors = (reference.inv() * rotations).as_rotvec()
|
||||||
|
_, singular, vh = np.linalg.svd(vectors, full_matrices=False)
|
||||||
|
if not singular.size or singular[0] < math.radians(1):
|
||||||
|
raise ValueError("hinge rotation is not observable")
|
||||||
|
axis = vh[0]
|
||||||
|
angles = vectors @ axis
|
||||||
|
trend = float((inputs - np.mean(inputs)) @ (angles - np.mean(angles)))
|
||||||
|
if abs(trend) <= 1e-12:
|
||||||
|
raise ValueError("SDK/visual motion direction is not observable")
|
||||||
|
if math.copysign(1, trend) != sdk_to_joint_direction:
|
||||||
|
axis, angles = -axis, -angles
|
||||||
|
if np.ptp(angles) >= math.pi - math.radians(1):
|
||||||
|
raise ValueError("hinge travel exceeds this rotation primitive's unambiguous interval")
|
||||||
|
return VisualRotationFrame(tuple(reference.as_quat()), tuple(axis),
|
||||||
|
tuple(sorted(row.sample_id for row in observations)), tuple(training_cycles))
|
||||||
|
|||||||
+78
@@ -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])))
|
||||||
@@ -2,13 +2,15 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field, replace
|
||||||
import math
|
import math
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Mapping
|
from typing import Mapping
|
||||||
|
|
||||||
from ..domain.profile import CalibrationProfile
|
from ..domain.profile import CalibrationProfile, validate_profile
|
||||||
from ..domain.result import CalibrationResult, JointMapping
|
from ..domain.result import CalibrationResult, JointMapping
|
||||||
|
from ..domain.capture_plan import (CapturePlan, ADAPTIVE, STAGED, TASKWISE_STAGED, TASKWISE_TWO_ROUND,
|
||||||
|
training_cycles)
|
||||||
from ..urdf.kinematics import UrdfKinematicModel
|
from ..urdf.kinematics import UrdfKinematicModel
|
||||||
from .motion_fit import channel_for_joint, fit_profile_motion, joint_input_direction
|
from .motion_fit import channel_for_joint, fit_profile_motion, joint_input_direction
|
||||||
from .spatial import ZeroCalibrationProfile, fit_joint_axis_measurement, solve_urdf_zero_offsets, with_depth_free_axis_projection
|
from .spatial import ZeroCalibrationProfile, fit_joint_axis_measurement, solve_urdf_zero_offsets, with_depth_free_axis_projection
|
||||||
@@ -32,7 +34,7 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi
|
|||||||
"directed_base_axis_joints", "depth_free_axis_projection", "axis_order",
|
"directed_base_axis_joints", "depth_free_axis_projection", "axis_order",
|
||||||
"same_view_axis_pair_by_offset",
|
"same_view_axis_pair_by_offset",
|
||||||
"parallel_root_pattern",
|
"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:
|
if not data or set(data) - allowed:
|
||||||
raise ValueError("spatial observation graph is missing or has unsupported fields")
|
raise ValueError("spatial observation graph is missing or has unsupported fields")
|
||||||
direct = tuple(profile.zero.direct_zero_joints)
|
direct = tuple(profile.zero.direct_zero_joints)
|
||||||
@@ -42,34 +44,93 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi
|
|||||||
axes = tuple(data.get("axis_order", profile.zero.axis_joints))
|
axes = tuple(data.get("axis_order", profile.zero.axis_joints))
|
||||||
if set(axes) != set(profile.zero.axis_joints) or len(set(axes)) != len(axes):
|
if set(axes) != set(profile.zero.axis_joints) or len(set(axes)) != len(axes):
|
||||||
raise ValueError("spatial axis order differs from declared observations")
|
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(
|
return ZeroCalibrationProfile(
|
||||||
hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints),
|
hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints - profile.retained_joints),
|
||||||
stable_cross_view_cone_bias=profile.measurement.stable_cross_view_cone_bias),
|
stable_cross_view_cone_bias=profile.measurement.stable_cross_view_cone_bias),
|
||||||
direct_zero_joints=direct, axis_joints=axes,
|
direct_zero_joints=direct, axis_joints=axes,
|
||||||
inherited_zero_joints=dict(profile.zero.transferred_zero_sources),
|
inherited_zero_joints={} if profile.artifacts.output_schema_version >= 3 else dict(profile.zero.transferred_zero_sources),
|
||||||
inherited_static_zero_joints=dict(profile.zero.transferred_zero_sources),
|
inherited_static_zero_joints={} if profile.artifacts.output_schema_version >= 3 else dict(profile.zero.transferred_zero_sources),
|
||||||
constrained_circle_joints=frozenset(axes),
|
constrained_circle_joints=frozenset(axes),
|
||||||
root_anchor_joints=frozenset(data.get("root_anchor_joints", ())),
|
root_anchor_joints=frozenset(data.get("root_anchor_joints", ())),
|
||||||
axis_parent_joint=dict(data.get("axis_parent_joint", {})),
|
axis_parent_joint=dict(data.get("axis_parent_joint", {})),
|
||||||
phase_parent_joint=dict(data.get("phase_parent_joint", {})),
|
phase_parent_joint=dict(data.get("phase_parent_joint", {})),
|
||||||
offset_observer_joint=observers,
|
offset_observer_joint=observers,
|
||||||
same_view_axis_pair_by_offset={name: tuple(pair) for name, pair in data.get("same_view_axis_pair_by_offset", {}).items()},
|
same_view_axis_pair_by_offset={name: tuple(pair) for name, pair in data.get("same_view_axis_pair_by_offset", {}).items()},
|
||||||
fixed_direct_zero_offsets_rad={}, static_output_zero_offsets_rad={},
|
fixed_direct_zero_offsets_rad={name: value.cad_angle_rad for name, value in profile.zero.known_baseline_geometry.items()
|
||||||
|
if name in direct}, static_output_zero_offsets_rad={},
|
||||||
base_pose_strategy=str(data.get("base_pose_strategy", "full_hand")),
|
base_pose_strategy=str(data.get("base_pose_strategy", "full_hand")),
|
||||||
orientation_anchor_joint=data.get("orientation_anchor_joint"),
|
orientation_anchor_joint=data.get("orientation_anchor_joint"),
|
||||||
directed_base_axis_joints=frozenset(data.get("directed_base_axis_joints", ())),
|
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)),
|
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)),
|
||||||
parallel_root_pattern=bool(data.get("parallel_root_pattern", False)))
|
independent_observed_motion=profile.artifacts.output_schema_version >= 3)
|
||||||
|
|
||||||
|
|
||||||
|
def build_axis_observations(profile, model, geometry_records, motion, zero_profile, *, include_holdout=True):
|
||||||
|
"""Build only the axis observations present in the declared partition."""
|
||||||
|
measured = profile.artifacts.output_schema_version >= 3
|
||||||
|
geometry_domain = f"command_{profile.command.unit}" if profile.command_based_release else profile.curve_input_domain
|
||||||
|
observations = []
|
||||||
|
plan = CapturePlan.from_profile(profile)
|
||||||
|
phase = "steady" if profile.command_based_release else "sweep"
|
||||||
|
for cycle in sorted({c for part in plan.tasks for c in part.cycles(phase)}):
|
||||||
|
if not include_holdout and cycle in {
|
||||||
|
part.command_holdout if phase == "steady" else part.holdout for part in plan.tasks}:
|
||||||
|
continue
|
||||||
|
by_joint = {}
|
||||||
|
for joint in zero_profile.axis_joints:
|
||||||
|
task = next(task for task in profile.motion.tasks if joint in task.joints)
|
||||||
|
if cycle not in plan.task(task.key).cycles(phase):
|
||||||
|
continue
|
||||||
|
rows = geometry_records[joint]
|
||||||
|
if profile.command_based_release:
|
||||||
|
rows = [dict(row, input_value=float(row[geometry_domain]),
|
||||||
|
state_values=row[f"state_{profile.command.unit}"]) for row in rows]
|
||||||
|
elif profile.command.unit == "rad":
|
||||||
|
rows = [dict(row, input_value=float(row[profile.curve_input_domain]), state_values=row["state_rad"])
|
||||||
|
for row in rows]
|
||||||
|
else:
|
||||||
|
rows = [dict(row, command_u8=int(round(float(row[profile.curve_input_domain])))) for row in rows]
|
||||||
|
parent = zero_profile.phase_parent_joint.get(joint)
|
||||||
|
if parent is not None and parent not in by_joint:
|
||||||
|
if (cycle != plan.task(task.key).holdout
|
||||||
|
and profile.quality.training_policy in {ADAPTIVE, STAGED, TASKWISE_STAGED, TASKWISE_TWO_ROUND}):
|
||||||
|
continue # No same-cycle parent observation: do not fabricate a geometric sample.
|
||||||
|
raise ValueError("spatial axis order must place phase parents first")
|
||||||
|
task = next(task for task in profile.motion.tasks if joint in task.joints)
|
||||||
|
baseline = (motion.reference_inputs[joint] if measured else task.start_value if profile.command.unit == "rad"
|
||||||
|
else profile.command.baseline_values[channel_for_joint(profile, joint)])
|
||||||
|
axis = fit_joint_axis_measurement(joint, rows, cycle=cycle, zero_command_u8=baseline,
|
||||||
|
input_to_joint_direction=joint_input_direction(profile, model, joint),
|
||||||
|
constrained_circle_joints=zero_profile.constrained_circle_joints,
|
||||||
|
view_normal_common_xyz=rows[0]["view_normal_common_xyz"],
|
||||||
|
canonical_zero_direction="decreasing",
|
||||||
|
axis_common_constraint=None if parent is None else by_joint[parent].axis_common_xyz,
|
||||||
|
separate_axial_residual=True,
|
||||||
|
condition_command_field=f"command_vector_{profile.command.unit}" if profile.command_based_release else None)
|
||||||
|
if profile.zero.spatial.get("depth_free_axis_projection", False):
|
||||||
|
axis = with_depth_free_axis_projection(axis, rows[0]["camera_center_common_xyz_m"])
|
||||||
|
observations.append(axis)
|
||||||
|
by_joint[joint] = axis
|
||||||
|
return observations
|
||||||
|
|
||||||
|
|
||||||
def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, records_by_joint,
|
def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, records_by_joint,
|
||||||
*, cross_view_records=None) -> CalibrationResult:
|
*, cross_view_records=None, zero_references=None, steady_records=None, motion_fitted=lambda _motion: None,
|
||||||
|
frozen_training=None) -> CalibrationResult:
|
||||||
"""Shared native-domain motion, spatial zero and standard mimic fit.
|
"""Shared native-domain motion, spatial zero and standard mimic fit.
|
||||||
|
|
||||||
No mechanical endpoint or range-centre fallback can replace a requested
|
No mechanical endpoint or range-centre fallback can replace a requested
|
||||||
but unobservable spatial zero. The fourth cycle never updates the fit.
|
but unobservable spatial zero. The fourth cycle never updates the fit.
|
||||||
"""
|
"""
|
||||||
|
if profile.artifacts.output_schema_version >= 3:
|
||||||
|
validate_profile(profile)
|
||||||
if profile.zero.endpoint_anchor_by_joint:
|
if profile.zero.endpoint_anchor_by_joint:
|
||||||
raise ValueError("unverified mechanical endpoints cannot define CAD zeros")
|
raise ValueError("unverified mechanical endpoints cannot define CAD zeros")
|
||||||
required_pose_fields = {"parent_pose_common", "child_pose_common",
|
required_pose_fields = {"parent_pose_common", "child_pose_common",
|
||||||
@@ -80,39 +141,51 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
|
|||||||
if not rows or any(not required_pose_fields.issubset(row) for row in rows):
|
if not rows or any(not required_pose_fields.issubset(row) for row in rows):
|
||||||
raise ValueError(f"spatial calibration requires identified common-frame pose observations:{joint}")
|
raise ValueError(f"spatial calibration requires identified common-frame pose observations:{joint}")
|
||||||
model = UrdfKinematicModel(source_urdf)
|
model = UrdfKinematicModel(source_urdf)
|
||||||
|
if profile.retained_joints:
|
||||||
|
from ..urdf.partial_scope import require_profile_held_pose
|
||||||
|
from ...profiles.observations import compile_tag_links
|
||||||
|
links = compile_tag_links(profile, model)
|
||||||
|
for name, rows in records_by_joint.items():
|
||||||
|
spec = profile.measurement.measurements[name]
|
||||||
|
for row in rows:
|
||||||
|
for role in (spec.parent_role, spec.child_role):
|
||||||
|
require_profile_held_pose(profile, model, row[f"command_vector_{profile.command.unit}"], links[role])
|
||||||
zero_profile = compile_spatial_profile(profile)
|
zero_profile = compile_spatial_profile(profile)
|
||||||
motion = fit_profile_motion(profile, model, records_by_joint, cross_view_records=cross_view_records)
|
if profile.command_based_release:
|
||||||
|
from .command_motion import fit_command_motion
|
||||||
|
motion = fit_command_motion(profile, model, records_by_joint, steady_records or {}, zero_references or {},
|
||||||
|
frozen_motion=None if frozen_training is None else frozen_training["motion"])
|
||||||
|
geometry_records = steady_records
|
||||||
|
geometry_domain = f"command_{profile.command.unit}"
|
||||||
|
else:
|
||||||
|
motion = fit_profile_motion(profile, model, records_by_joint, cross_view_records=cross_view_records,
|
||||||
|
zero_references=zero_references)
|
||||||
|
geometry_records = records_by_joint
|
||||||
|
geometry_domain = profile.curve_input_domain
|
||||||
|
for joint in profile.zero.axis_joints:
|
||||||
|
if not geometry_records.get(joint) or any(not required_pose_fields.issubset(row)
|
||||||
|
for row in geometry_records[joint]):
|
||||||
|
raise ValueError(f"spatial calibration requires identified geometry poses:{joint}")
|
||||||
|
motion_fitted(motion)
|
||||||
curves = motion.coordinate_curves
|
curves = motion.coordinate_curves
|
||||||
observations = []
|
measured = profile.artifacts.output_schema_version >= 3
|
||||||
for cycle in range(4):
|
baseline_geometry = {name: value.cad_angle_rad for name, value in profile.zero.known_baseline_geometry.items()}
|
||||||
by_joint = {}
|
if measured:
|
||||||
for joint in zero_profile.axis_joints:
|
baseline_geometry.update({name: 0.0 for name in profile.zero.cad_zero_assumptions})
|
||||||
rows = records_by_joint[joint]
|
spatial_curves = {name: replace(curve, circle={**curve.circle,
|
||||||
if profile.command.unit == "rad":
|
"known_cad_baseline_rad": baseline_geometry[name]})
|
||||||
rows = [dict(row, input_value=float(row[profile.curve_input_domain]), state_values=row["state_rad"])
|
if measured and name in baseline_geometry and name not in zero_profile.direct_zero_joints
|
||||||
for row in rows]
|
else curve for name, curve in curves.items()}
|
||||||
else:
|
observations = build_axis_observations(profile, model, geometry_records, motion, zero_profile)
|
||||||
rows = [dict(row, command_u8=int(round(float(row[profile.curve_input_domain])))) for row in rows]
|
plan = CapturePlan.from_profile(profile)
|
||||||
parent = zero_profile.phase_parent_joint.get(joint)
|
geometry_training = tuple(sorted({cycle for part in plan.tasks for cycle in
|
||||||
if parent is not None and parent not in by_joint:
|
(part.command_training if profile.command_based_release else part.training)}))
|
||||||
raise ValueError("spatial axis order must place phase parents first")
|
geometry_holdout = (plan.tasks[0].command_holdout if profile.command_based_release else profile.quality.holdout_cycle)
|
||||||
task = next(task for task in profile.motion.tasks if joint in task.joints)
|
|
||||||
baseline = (task.start_value if profile.command.unit == "rad"
|
|
||||||
else profile.command.baseline_values[channel_for_joint(profile, joint)])
|
|
||||||
axis = fit_joint_axis_measurement(joint, rows, cycle=cycle, zero_command_u8=baseline,
|
|
||||||
input_to_joint_direction=joint_input_direction(profile, model, joint),
|
|
||||||
constrained_circle_joints=zero_profile.constrained_circle_joints,
|
|
||||||
view_normal_common_xyz=rows[0]["view_normal_common_xyz"],
|
|
||||||
canonical_zero_direction="decreasing",
|
|
||||||
axis_common_constraint=None if parent is None else by_joint[parent].axis_common_xyz,
|
|
||||||
separate_axial_residual=True)
|
|
||||||
if profile.zero.spatial.get("depth_free_axis_projection", False):
|
|
||||||
axis = with_depth_free_axis_projection(axis, rows[0]["camera_center_common_xyz_m"])
|
|
||||||
observations.append(axis)
|
|
||||||
by_joint[joint] = axis
|
|
||||||
zero = solve_urdf_zero_offsets(source_urdf=source_urdf, measurements=observations,
|
zero = solve_urdf_zero_offsets(source_urdf=source_urdf, measurements=observations,
|
||||||
curves=curves, motor_by_joint={joint: channel_for_joint(profile, joint) for joint in curves},
|
curves=spatial_curves, motor_by_joint={joint: channel_for_joint(profile, joint) for joint in curves},
|
||||||
zero_profile=zero_profile, training_cycles=(0, 1, 2), validation_cycle=3,
|
zero_profile=zero_profile, training_cycles=geometry_training, validation_cycle=geometry_holdout,
|
||||||
|
allow_partial_training_cycles=profile.quality.training_policy in {ADAPTIVE, STAGED, TASKWISE_STAGED, TASKWISE_TWO_ROUND},
|
||||||
|
frozen_training=None if frozen_training is None else frozen_training["zero"],
|
||||||
maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20),
|
maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20),
|
||||||
maximum_validation_mae_rad=math.radians(1), maximum_validation_p95_rad=math.radians(2),
|
maximum_validation_mae_rad=math.radians(1), maximum_validation_p95_rad=math.radians(2),
|
||||||
maximum_validation_error_rad=math.radians(3), maximum_confidence_half_width_rad=math.radians(1),
|
maximum_validation_error_rad=math.radians(3), maximum_confidence_half_width_rad=math.radians(1),
|
||||||
@@ -121,6 +194,20 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
|
|||||||
raise ValueError("spatial_zero_not_observable_or_inaccurate:" + str(zero.failure_reasons))
|
raise ValueError("spatial_zero_not_observable_or_inaccurate:" + str(zero.failure_reasons))
|
||||||
offsets = {name: float(zero.direct_offsets_rad[name]) for name in profile.zero.direct_zero_joints}
|
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}
|
methods = {name: "urdf_serial_axis_geometry" for name in offsets}
|
||||||
|
if measured:
|
||||||
|
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:
|
||||||
|
offsets[name], methods[name] = 0.0, "assumed_source_cad_zero"
|
||||||
|
mappings = {name: JointMapping(name, channel_for_joint(profile, name), geometry_domain,
|
||||||
|
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),
|
||||||
|
command_mappings=motion.mappings if profile.command_based_release else {},
|
||||||
|
command_holdout_metrics=motion.command_metrics if profile.command_based_release else {},
|
||||||
|
command_applicability=motion.applicability if profile.command_based_release else {},
|
||||||
|
feedback_diagnostics=motion.feedback_diagnostics if profile.command_based_release else {})
|
||||||
for name in profile.zero.active_joints - offsets.keys() - profile.zero.transferred_zero_sources.keys():
|
for name in profile.zero.active_joints - offsets.keys() - profile.zero.transferred_zero_sources.keys():
|
||||||
if name not in profile.zero.cad_frozen_joints:
|
if name not in profile.zero.cad_frozen_joints:
|
||||||
raise ValueError(f"active static zero has no measured or CAD-retention policy:{name}")
|
raise ValueError(f"active static zero has no measured or CAD-retention policy:{name}")
|
||||||
@@ -142,7 +229,7 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
training = records_by_joint[donor]
|
training = records_by_joint[donor]
|
||||||
support = [[float(r[profile.curve_input_domain]) for r in training
|
support = [[float(r[profile.curve_input_domain]) for r in training
|
||||||
if r["cycle"] in {0, 1, 2} and r["direction"] == direction]
|
if r["cycle"] in training_cycles(profile, joint=donor) and r["direction"] == direction]
|
||||||
for direction in ("increasing", "decreasing")]
|
for direction in ("increasing", "decreasing")]
|
||||||
lo, hi = math.ceil(max(min(s) for s in support)), math.floor(min(max(s) for s in support))
|
lo, hi = math.ceil(max(min(s) for s in support)), math.floor(min(max(s) for s in support))
|
||||||
knots = tuple(float(v) for v in range(lo, hi+1))
|
knots = tuple(float(v) for v in range(lo, hi+1))
|
||||||
|
|||||||
+73
-196
@@ -21,187 +21,80 @@ from .stages import (
|
|||||||
from .types import ZeroSolveResult
|
from .types import ZeroSolveResult
|
||||||
|
|
||||||
|
|
||||||
def decide_acceptance(
|
def decide_training_acceptance(*, cycles, fit, geometry_evidence, limits, observability, options, problem):
|
||||||
*,
|
"""All training gates shared by early stopping and final acceptance."""
|
||||||
cycles: CycleEvidence,
|
|
||||||
fit: TrainingFit,
|
|
||||||
geometry_evidence: GeometryEvidence,
|
|
||||||
holdout: HoldoutEvidence,
|
|
||||||
limits: OffsetLimits,
|
|
||||||
observability: ObservabilityEvidence,
|
|
||||||
options: ZeroSolveOptions,
|
|
||||||
problem: ZeroProblem,
|
|
||||||
) -> AcceptanceDecision:
|
|
||||||
applied_training = cycles.applied_training
|
|
||||||
confidence_half_widths = cycles.confidence_half_widths
|
|
||||||
cycle_consistent = cycles.cycle_consistent
|
|
||||||
diagnostic_offset_limits = limits.diagnostic_offset_limits
|
|
||||||
fixed_offsets = problem.fixed_offsets
|
|
||||||
improvement_confidence_lower = holdout.improvement_confidence_lower
|
|
||||||
improvement_passed = holdout.improvement_passed
|
|
||||||
inconsistent_cycles = cycles.inconsistent_cycles
|
|
||||||
insignificant_large = cycles.insignificant_large
|
|
||||||
maximum_confidence_half_width_rad = options.maximum_confidence_half_width_rad
|
|
||||||
maximum_observability_condition_number = options.maximum_observability_condition_number
|
|
||||||
maximum_offset_rad = options.maximum_offset_rad
|
|
||||||
maximum_validation_error_rad = options.maximum_validation_error_rad
|
|
||||||
maximum_validation_mae_rad = options.maximum_validation_mae_rad
|
|
||||||
maximum_validation_p95_rad = options.maximum_validation_p95_rad
|
|
||||||
observability_condition_number = observability.observability_condition_number
|
|
||||||
observability_parameter_count = observability.observability_parameter_count
|
|
||||||
observability_rank = observability.observability_rank
|
|
||||||
observation_failures = geometry_evidence.observation_failures
|
|
||||||
offset_limits = limits.offset_limits
|
|
||||||
output_offsets = problem.output_offsets
|
|
||||||
palm_orientation_validation_passed = holdout.palm_orientation_validation_passed
|
|
||||||
product_finger_rolls = limits.product_finger_rolls
|
|
||||||
profile = problem.profile
|
profile = problem.profile
|
||||||
training_offsets = fit.training_offsets
|
common_mode = (float(np.median([fit.training_offsets[name] for name in limits.product_finger_rolls]))
|
||||||
validation_errors = holdout.validation_errors
|
if profile.parallel_root_pattern and limits.product_finger_rolls else 0.)
|
||||||
|
failures = {}
|
||||||
finger_roll_common_mode = (
|
for name, limit, diagnostic_limit in zip(profile.direct_zero_joints,
|
||||||
float(
|
limits.offset_limits, limits.diagnostic_offset_limits):
|
||||||
np.median(
|
if name in problem.fixed_offsets:
|
||||||
[training_offsets[name] for name in product_finger_rolls]
|
|
||||||
)
|
|
||||||
)
|
|
||||||
if profile.parallel_root_pattern
|
|
||||||
and product_finger_rolls
|
|
||||||
else 0.0
|
|
||||||
)
|
|
||||||
configured_limit_exceeded: list[str] = []
|
|
||||||
for name, limit in zip(profile.direct_zero_joints, offset_limits):
|
|
||||||
if name in fixed_offsets:
|
|
||||||
continue
|
continue
|
||||||
# A post-solve mechanical endpoint datum is the value that will be
|
checked = problem.output_offsets.get(name, fit.training_offsets[name])
|
||||||
# published for this joint. The visual root-axis scalar remains a
|
if name in limits.product_finger_rolls:
|
||||||
# nuisance gauge used for holdout geometry and must not be compared
|
checked -= common_mode
|
||||||
# with the safety bound of a different, endpoint-anchored output.
|
if abs(checked) > limit + math.radians(.01):
|
||||||
checked_offset = output_offsets.get(name, training_offsets[name])
|
failures[name] = "zero_offset_exceeds_configured_limit"
|
||||||
if name in product_finger_rolls:
|
if abs(checked) >= diagnostic_limit - math.radians(.01):
|
||||||
# The four roll motors share the same electrical centre and the
|
failures[name] = "zero_offset_reached_diagnostic_bound"
|
||||||
# absolute palm axial datum is recovered from the root-line
|
if limits.product_finger_rolls and abs(common_mode) > options.maximum_offset_rad + math.radians(.01):
|
||||||
# pattern. Protect the independently assembled finger-to-finger
|
failures["finger_mcp_roll_common_mode"] = "zero_offset_exceeds_configured_limit"
|
||||||
# deviations with the strict finger bound; protect their shared
|
if profile.parallel_root_pattern and profile.base_pose_strategy == "full_hand":
|
||||||
# common mode with the unchanged global zero bound. Treating the
|
if observability.observability_rank < observability.observability_parameter_count:
|
||||||
# same common datum as four independent failures is both
|
failures["palm_and_static_zero"] = "zero_observation_jacobian_rank_deficient"
|
||||||
# over-counting and sensitive to the palm-frame gauge.
|
elif observability.observability_condition_number > options.maximum_observability_condition_number:
|
||||||
checked_offset -= finger_roll_common_mode
|
failures["palm_and_static_zero"] = "zero_observation_jacobian_ill_conditioned"
|
||||||
if abs(checked_offset) > limit + math.radians(0.01):
|
for name in cycles.inconsistent_cycles:
|
||||||
configured_limit_exceeded.append(name)
|
failures[name] = "zero_offset_cycle_difference_too_large"
|
||||||
if (
|
for name in cycles.insignificant_large:
|
||||||
product_finger_rolls
|
failures[name] = "zero_offset_not_statistically_significant"
|
||||||
and abs(finger_roll_common_mode)
|
if options.maximum_confidence_half_width_rad is not None:
|
||||||
> maximum_offset_rad + math.radians(0.01)
|
for name, width in cycles.confidence_half_widths.items():
|
||||||
):
|
if width > options.maximum_confidence_half_width_rad:
|
||||||
configured_limit_exceeded.append("finger_mcp_roll_common_mode")
|
failures[name] = "zero_offset_confidence_interval_too_wide"
|
||||||
diagnostic_bound_hits: list[str] = []
|
failures.update(geometry_evidence.observation_failures)
|
||||||
for name, limit in zip(
|
return AcceptanceDecision(finger_roll_common_mode=common_mode, failure_reasons=failures,
|
||||||
profile.direct_zero_joints, diagnostic_offset_limits
|
passed=cycles.cycle_consistent and not failures)
|
||||||
):
|
|
||||||
if name in fixed_offsets:
|
|
||||||
continue
|
|
||||||
checked_offset = output_offsets.get(name, training_offsets[name])
|
|
||||||
if name in product_finger_rolls:
|
|
||||||
# Match the configured-limit and publication convention above.
|
|
||||||
# The raw common roll is a fitted-palm-frame gauge; only the
|
|
||||||
# finger-to-finger deviation is a physical zero correction.
|
|
||||||
checked_offset -= finger_roll_common_mode
|
|
||||||
if abs(checked_offset) >= limit - math.radians(0.01):
|
|
||||||
diagnostic_bound_hits.append(name)
|
|
||||||
failure_reasons: dict[str, str] = {}
|
|
||||||
if not palm_orientation_validation_passed:
|
|
||||||
failure_reasons["palm_orientation"] = (
|
|
||||||
"palm_orientation_holdout_too_large"
|
|
||||||
)
|
|
||||||
requires_full_observability = bool(
|
|
||||||
profile.parallel_root_pattern
|
|
||||||
and profile.base_pose_strategy == "full_hand"
|
|
||||||
)
|
|
||||||
if (
|
|
||||||
requires_full_observability
|
|
||||||
and observability_rank < observability_parameter_count
|
|
||||||
):
|
|
||||||
failure_reasons["palm_and_static_zero"] = (
|
|
||||||
"zero_observation_jacobian_rank_deficient"
|
|
||||||
)
|
|
||||||
elif (
|
|
||||||
requires_full_observability
|
|
||||||
and observability_condition_number
|
|
||||||
> maximum_observability_condition_number
|
|
||||||
):
|
|
||||||
failure_reasons["palm_and_static_zero"] = (
|
|
||||||
"zero_observation_jacobian_ill_conditioned"
|
|
||||||
)
|
|
||||||
for name in configured_limit_exceeded:
|
|
||||||
failure_reasons[name] = "zero_offset_exceeds_configured_limit"
|
|
||||||
for name in diagnostic_bound_hits:
|
|
||||||
failure_reasons[name] = "zero_offset_reached_diagnostic_bound"
|
|
||||||
for name in inconsistent_cycles:
|
|
||||||
failure_reasons[name] = "zero_offset_cycle_difference_too_large"
|
|
||||||
for name in insignificant_large:
|
|
||||||
failure_reasons[name] = "zero_offset_not_statistically_significant"
|
|
||||||
if maximum_confidence_half_width_rad is not None:
|
|
||||||
for name, half_width in confidence_half_widths.items():
|
|
||||||
if half_width > maximum_confidence_half_width_rad:
|
|
||||||
failure_reasons[name] = (
|
|
||||||
"zero_offset_confidence_interval_too_wide"
|
|
||||||
)
|
|
||||||
# This solver publishes rotational encoder zeros only. A post-fit CAD to
|
|
||||||
# measured axis-line displacement is invariant to the joint's own zero
|
|
||||||
# and cannot be repaired by changing that rotational parameter. Keep the
|
|
||||||
# per-joint and aggregate values in ZeroSolveResult for geometry audit,
|
|
||||||
# but do not misclassify a fixed link-origin/Tag-depth discrepancy as a
|
|
||||||
# failed rotational holdout. Axis-point *fit* quality is still guarded
|
|
||||||
# above for every phase observation that actually uses line position.
|
|
||||||
if not improvement_passed:
|
|
||||||
for name, value in applied_training.items():
|
|
||||||
if (
|
|
||||||
name not in fixed_offsets
|
|
||||||
and value != 0.0
|
|
||||||
and improvement_confidence_lower.get(name, 0.0) <= 0.0
|
|
||||||
):
|
|
||||||
failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence"
|
|
||||||
# Geometry failures are the root cause and must not be hidden by the
|
|
||||||
# downstream validation symptom produced by the same bad observation.
|
|
||||||
failure_reasons.update(observation_failures)
|
|
||||||
passed = bool(
|
|
||||||
validation_errors.size
|
|
||||||
== len(profile.direct_zero_joints) - len(fixed_offsets)
|
|
||||||
and float(np.mean(validation_errors)) <= maximum_validation_mae_rad
|
|
||||||
and float(np.percentile(validation_errors, 95.0))
|
|
||||||
<= maximum_validation_p95_rad
|
|
||||||
and (
|
|
||||||
maximum_validation_error_rad is None
|
|
||||||
or float(np.max(validation_errors))
|
|
||||||
<= maximum_validation_error_rad
|
|
||||||
)
|
|
||||||
and cycle_consistent
|
|
||||||
and not insignificant_large
|
|
||||||
and not configured_limit_exceeded
|
|
||||||
and not diagnostic_bound_hits
|
|
||||||
and not observation_failures
|
|
||||||
and (
|
|
||||||
not requires_full_observability
|
|
||||||
or (
|
|
||||||
observability_rank == observability_parameter_count
|
|
||||||
and observability_condition_number
|
|
||||||
<= maximum_observability_condition_number
|
|
||||||
)
|
|
||||||
)
|
|
||||||
and not any(
|
|
||||||
reason == "zero_offset_confidence_interval_too_wide"
|
|
||||||
for reason in failure_reasons.values()
|
|
||||||
)
|
|
||||||
and improvement_passed
|
|
||||||
and palm_orientation_validation_passed
|
|
||||||
)
|
|
||||||
|
|
||||||
return AcceptanceDecision(
|
|
||||||
finger_roll_common_mode=finger_roll_common_mode,
|
def decide_acceptance(
|
||||||
failure_reasons=failure_reasons,
|
*, cycles: CycleEvidence, fit: TrainingFit, geometry_evidence: GeometryEvidence,
|
||||||
passed=passed,
|
holdout: HoldoutEvidence, limits: OffsetLimits, observability: ObservabilityEvidence,
|
||||||
)
|
options: ZeroSolveOptions, problem: ZeroProblem,
|
||||||
|
) -> AcceptanceDecision:
|
||||||
|
training = decide_training_acceptance(cycles=cycles, fit=fit, geometry_evidence=geometry_evidence,
|
||||||
|
limits=limits, observability=observability, options=options, problem=problem)
|
||||||
|
failures = dict(training.failure_reasons)
|
||||||
|
if not holdout.palm_orientation_validation_passed:
|
||||||
|
failures["palm_orientation"] = "palm_orientation_holdout_too_large"
|
||||||
|
if not holdout.improvement_passed:
|
||||||
|
for name, value in cycles.applied_training.items():
|
||||||
|
if (name not in problem.fixed_offsets and value != 0.
|
||||||
|
and holdout.improvement_confidence_lower.get(name, 0.) <= 0.):
|
||||||
|
failures[name] = "zero_offset_did_not_improve_with_95pct_confidence"
|
||||||
|
# Geometry is the root cause; retain its reason over downstream symptoms.
|
||||||
|
failures.update(geometry_evidence.observation_failures)
|
||||||
|
errors = holdout.validation_errors
|
||||||
|
passed = bool(training.passed and holdout.improvement_passed
|
||||||
|
and holdout.palm_orientation_validation_passed
|
||||||
|
and errors.size == len(problem.profile.direct_zero_joints) - len(problem.fixed_offsets)
|
||||||
|
and errors.size > 0
|
||||||
|
and float(np.mean(errors)) <= options.maximum_validation_mae_rad
|
||||||
|
and float(np.percentile(errors, 95.)) <= options.maximum_validation_p95_rad
|
||||||
|
and (options.maximum_validation_error_rad is None
|
||||||
|
or float(np.max(errors)) <= options.maximum_validation_error_rad))
|
||||||
|
return AcceptanceDecision(finger_roll_common_mode=training.finger_roll_common_mode,
|
||||||
|
failure_reasons=failures, passed=passed)
|
||||||
|
|
||||||
|
|
||||||
|
def applied_output_offsets(profile, applied_training, output_offsets, product_finger_rolls, common_mode):
|
||||||
|
"""The same training-frozen coordinate convention for Tag fit and release."""
|
||||||
|
offsets = {**applied_training, **output_offsets}
|
||||||
|
if profile.parallel_root_pattern:
|
||||||
|
for name in product_finger_rolls:
|
||||||
|
offsets[name] = float(offsets[name]) - common_mode
|
||||||
|
return offsets
|
||||||
|
|
||||||
|
|
||||||
def assemble_result(
|
def assemble_result(
|
||||||
@@ -247,24 +140,8 @@ def assemble_result(
|
|||||||
# Never refit a model that has passed its holdout with the validation
|
# Never refit a model that has passed its holdout with the validation
|
||||||
# cycle. The published offsets are exactly the frozen training result
|
# cycle. The published offsets are exactly the frozen training result
|
||||||
# that produced ``validation_errors`` above.
|
# that produced ``validation_errors`` above.
|
||||||
final_offsets = dict(applied_training)
|
final_offsets = applied_output_offsets(profile, applied_training, output_offsets,
|
||||||
# Apply independently validated assembly datums only after trajectory
|
product_finger_rolls, finger_roll_common_mode)
|
||||||
# fitting and holdout validation. A mechanical prior must define the
|
|
||||||
# written artifact without perturbing downstream yaw/pitch estimates.
|
|
||||||
final_offsets.update(output_offsets)
|
|
||||||
if profile.parallel_root_pattern and product_finger_rolls:
|
|
||||||
# The camera solve observes the four roll axes in a fitted palm frame.
|
|
||||||
# Rotation of that frame about their shared datum is a gauge, not four
|
|
||||||
# independent finger assembly errors. The product command 127/CAD
|
|
||||||
# pose defines the common straight-ahead datum; publish only each
|
|
||||||
# finger's robust deviation from the four-finger median. Validation
|
|
||||||
# above remains in the observation gauge, so no measured residual is
|
|
||||||
# discarded.
|
|
||||||
for name in product_finger_rolls:
|
|
||||||
final_offsets[name] = (
|
|
||||||
float(final_offsets[name]) - finger_roll_common_mode
|
|
||||||
)
|
|
||||||
|
|
||||||
# Every active joint must be present in the runtime payload/URDF writer,
|
# Every active joint must be present in the runtime payload/URDF writer,
|
||||||
# but absence of an absolute observation is not evidence for the
|
# but absence of an absolute observation is not evidence for the
|
||||||
# reference finger's assembly offset. Preserve the source-CAD zero for
|
# reference finger's assembly offset. Preserve the source-CAD zero for
|
||||||
|
|||||||
+14
-6
@@ -399,6 +399,7 @@ def fit_joint_axis_measurement(
|
|||||||
canonical_zero_direction: str | None = None,
|
canonical_zero_direction: str | None = None,
|
||||||
separate_axial_residual: bool = False,
|
separate_axial_residual: bool = False,
|
||||||
input_to_joint_direction: int = -1,
|
input_to_joint_direction: int = -1,
|
||||||
|
condition_command_field: str | None = None,
|
||||||
) -> JointAxisMeasurement:
|
) -> JointAxisMeasurement:
|
||||||
"""Fit one physical screw axis from one complete scan cycle."""
|
"""Fit one physical screw axis from one complete scan cycle."""
|
||||||
samples = [
|
samples = [
|
||||||
@@ -527,17 +528,21 @@ def fit_joint_axis_measurement(
|
|||||||
)
|
)
|
||||||
)
|
)
|
||||||
point_common = parent_rotation.apply(point_parent) + parent_translation
|
point_common = parent_rotation.apply(point_parent) + parent_translation
|
||||||
state = np.median(
|
states = [record.get("state_values", record.get("state_u8")) for record in zero_records]
|
||||||
np.asarray([record["state_values"] if "state_values" in record else record["state_u8"]
|
# Command-conditioned geometry never invents an encoder condition. Keep
|
||||||
for record in zero_records], dtype=float),
|
# unavailable diagnostics explicit while preserving legacy requirements.
|
||||||
axis=0,
|
if any(value is None for value in states):
|
||||||
)
|
if condition_command_field is None:
|
||||||
|
raise ValueError("axis_condition_feedback_missing")
|
||||||
|
state = None
|
||||||
|
else:
|
||||||
|
state = tuple(float(value) for value in np.median(np.asarray(states, dtype=float), axis=0))
|
||||||
return JointAxisMeasurement(
|
return JointAxisMeasurement(
|
||||||
joint=str(joint),
|
joint=str(joint),
|
||||||
cycle=int(cycle),
|
cycle=int(cycle),
|
||||||
axis_common_xyz=tuple(float(value) for value in axis_common),
|
axis_common_xyz=tuple(float(value) for value in axis_common),
|
||||||
point_common_xyz_m=tuple(float(value) for value in point_common),
|
point_common_xyz_m=tuple(float(value) for value in point_common),
|
||||||
condition_state_u8=tuple(float(value) for value in state),
|
condition_state_u8=state,
|
||||||
plane_rms_m=float(plane_rms),
|
plane_rms_m=float(plane_rms),
|
||||||
radial_rms_m=float(circle["radial_rms_m"]),
|
radial_rms_m=float(circle["radial_rms_m"]),
|
||||||
rotation_circle_axis_difference_rad=float(disagreement),
|
rotation_circle_axis_difference_rad=float(disagreement),
|
||||||
@@ -560,4 +565,7 @@ def fit_joint_axis_measurement(
|
|||||||
pose_axis_line_transverse_rms_m=residual_diagnostics["transverse_rms_m"],
|
pose_axis_line_transverse_rms_m=residual_diagnostics["transverse_rms_m"],
|
||||||
axis_point_axial_component_separated=separate_axial,
|
axis_point_axial_component_separated=separate_axial,
|
||||||
pose_axis_line_source_joints=(str(joint),),
|
pose_axis_line_source_joints=(str(joint),),
|
||||||
|
condition_command_u8=(tuple(float(v) for v in np.median(
|
||||||
|
np.asarray([r[condition_command_field] for r in zero_records], dtype=float), axis=0))
|
||||||
|
if condition_command_field else None),
|
||||||
)
|
)
|
||||||
|
|||||||
+1
@@ -192,6 +192,7 @@ def _angles_from_state(
|
|||||||
if not knots[0] <= value <= knots[-1]:
|
if not knots[0] <= value <= knots[-1]:
|
||||||
raise ValueError(f"spatial condition outside measured native support:{joint}:{value}")
|
raise ValueError(f"spatial condition outside measured native support:{joint}:{value}")
|
||||||
result[joint] = float(np.interp(value, knots, curve.angle_rad))
|
result[joint] = float(np.interp(value, knots, curve.angle_rad))
|
||||||
|
result[joint] += float(curve.circle.get("known_cad_baseline_rad", 0.0))
|
||||||
else:
|
else:
|
||||||
if not 0 <= value <= 255:
|
if not 0 <= value <= 255:
|
||||||
raise ValueError(f"spatial condition outside byte domain:{joint}")
|
raise ValueError(f"spatial condition outside byte domain:{joint}")
|
||||||
|
|||||||
+4
-1
@@ -25,6 +25,7 @@ def optimise_offsets(
|
|||||||
limits: OffsetLimits,
|
limits: OffsetLimits,
|
||||||
problem: TrainingProblem,
|
problem: TrainingProblem,
|
||||||
geometry: ObservationGeometry,
|
geometry: ObservationGeometry,
|
||||||
|
fitted_names=None,
|
||||||
) -> dict[str, float]:
|
) -> dict[str, float]:
|
||||||
diagnostic_offset_limits = limits.diagnostic_offset_limits
|
diagnostic_offset_limits = limits.diagnostic_offset_limits
|
||||||
fixed_offsets = problem.fixed_offsets
|
fixed_offsets = problem.fixed_offsets
|
||||||
@@ -48,6 +49,8 @@ def optimise_offsets(
|
|||||||
):
|
):
|
||||||
if name in fixed_offsets:
|
if name in fixed_offsets:
|
||||||
continue
|
continue
|
||||||
|
if fitted_names is not None and name not in fitted_names:
|
||||||
|
continue
|
||||||
has_axis_pair = name in profile.same_view_axis_pair_by_offset
|
has_axis_pair = name in profile.same_view_axis_pair_by_offset
|
||||||
limit = float(
|
limit = float(
|
||||||
configured_limit if has_axis_pair else diagnostic_limit
|
configured_limit if has_axis_pair else diagnostic_limit
|
||||||
@@ -57,7 +60,7 @@ def optimise_offsets(
|
|||||||
candidate = dict(result)
|
candidate = dict(result)
|
||||||
candidate[name] = float(value[0])
|
candidate[name] = float(value[0])
|
||||||
samples = angular_error_samples(
|
samples = angular_error_samples(
|
||||||
candidate, selected, base_rotation, base_translation
|
candidate, selected, base_rotation, base_translation, only_offsets={name}
|
||||||
)[name]
|
)[name]
|
||||||
return np.asarray(
|
return np.asarray(
|
||||||
[
|
[
|
||||||
|
|||||||
+18
-5
@@ -5,6 +5,7 @@ from __future__ import annotations
|
|||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Mapping, Sequence
|
from typing import Mapping, Sequence
|
||||||
import math
|
import math
|
||||||
|
import json
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
@@ -51,13 +52,25 @@ def prepare_zero_problem(
|
|||||||
raise ValueError("zero profile layout does not match tag_layout")
|
raise ValueError("zero profile layout does not match tag_layout")
|
||||||
model = UrdfKinematicModel(source_urdf)
|
model = UrdfKinematicModel(source_urdf)
|
||||||
training = [m for m in measurements if m.cycle in set(training_cycles)]
|
training = [m for m in measurements if m.cycle in set(training_cycles)]
|
||||||
validation = [m for m in measurements if m.cycle == int(validation_cycle)]
|
validation = [m for m in measurements if validation_cycle is not None and m.cycle == int(validation_cycle)]
|
||||||
expected = set(profile.axis_joints)
|
expected = set(profile.axis_joints)
|
||||||
if (
|
if (
|
||||||
{m.joint for m in training} != expected
|
{m.joint for m in training} != expected
|
||||||
or {m.joint for m in validation} != expected
|
or (validation_cycle is not None and {m.joint for m in validation} != expected)
|
||||||
):
|
):
|
||||||
raise ValueError("axis measurements do not contain all required joints/cycles")
|
observed_training = {m.joint for m in training}
|
||||||
|
observed_validation = {m.joint for m in validation}
|
||||||
|
coverage = {
|
||||||
|
"training_missing": sorted(expected-observed_training),
|
||||||
|
"training_unexpected": sorted(observed_training-expected),
|
||||||
|
"validation_missing": sorted(expected-observed_validation) if validation_cycle is not None else [],
|
||||||
|
"validation_unexpected": sorted(observed_validation-expected) if validation_cycle is not None else [],
|
||||||
|
"training_missing_root_anchors": sorted(set(profile.root_anchor_joints)-observed_training),
|
||||||
|
"validation_missing_root_anchors": (sorted(set(profile.root_anchor_joints)-observed_validation)
|
||||||
|
if validation_cycle is not None else []),
|
||||||
|
}
|
||||||
|
raise ValueError("axis measurements do not contain all required joints/cycles:"
|
||||||
|
+json.dumps(coverage, sort_keys=True))
|
||||||
configured_palm_sources = {
|
configured_palm_sources = {
|
||||||
str(name): str(model_joint)
|
str(name): str(model_joint)
|
||||||
for name, model_joint in (
|
for name, model_joint in (
|
||||||
@@ -86,7 +99,7 @@ def prepare_zero_problem(
|
|||||||
)
|
)
|
||||||
required_orientation_cycles = {
|
required_orientation_cycles = {
|
||||||
*(int(value) for value in training_cycles),
|
*(int(value) for value in training_cycles),
|
||||||
int(validation_cycle),
|
*((int(validation_cycle),) if validation_cycle is not None else ()),
|
||||||
}
|
}
|
||||||
for cycle in required_orientation_cycles:
|
for cycle in required_orientation_cycles:
|
||||||
cycle_sources = {
|
cycle_sources = {
|
||||||
@@ -107,7 +120,7 @@ def prepare_zero_problem(
|
|||||||
orientation_validation = tuple(
|
orientation_validation = tuple(
|
||||||
item
|
item
|
||||||
for item in orientation_measurements
|
for item in orientation_measurements
|
||||||
if int(item.cycle) == int(validation_cycle)
|
if validation_cycle is not None and int(item.cycle) == int(validation_cycle)
|
||||||
)
|
)
|
||||||
orientation_by_model_cycle: dict[
|
orientation_by_model_cycle: dict[
|
||||||
tuple[str, int], PalmOrientationMeasurement
|
tuple[str, int], PalmOrientationMeasurement
|
||||||
|
|||||||
+33
-13
@@ -27,6 +27,24 @@ class ObservationGeometry:
|
|||||||
curves: Mapping[str, JointCurveFit]
|
curves: Mapping[str, JointCurveFit]
|
||||||
motor_by_joint: Mapping[str, int]
|
motor_by_joint: Mapping[str, int]
|
||||||
orientation_by_model_cycle: Mapping[tuple[str, int], PalmOrientationMeasurement]
|
orientation_by_model_cycle: Mapping[tuple[str, int], PalmOrientationMeasurement]
|
||||||
|
incomplete_training_cycles: frozenset[int] = frozenset()
|
||||||
|
|
||||||
|
def complete_offsets(self, selected):
|
||||||
|
"""Offsets whose same-cycle observation dependencies actually exist."""
|
||||||
|
keys = {(item.joint, item.cycle) for item in selected}
|
||||||
|
available = set()
|
||||||
|
for name, observer in self.profile.offset_observer_joint.items():
|
||||||
|
for joint, cycle in keys:
|
||||||
|
if joint != observer:
|
||||||
|
continue
|
||||||
|
pair = self.profile.same_view_axis_pair_by_offset.get(name)
|
||||||
|
if pair and all((axis, cycle) in self.orientation_by_model_cycle for axis in pair):
|
||||||
|
available.add(name)
|
||||||
|
elif observer in self.profile.axis_parent_joint:
|
||||||
|
available.add(name)
|
||||||
|
elif (self.profile.phase_parent_joint.get(observer), cycle) in keys:
|
||||||
|
available.add(name)
|
||||||
|
return available
|
||||||
|
|
||||||
def for_cycles(self, cycles: Sequence[int]) -> ObservationGeometry:
|
def for_cycles(self, cycles: Sequence[int]) -> ObservationGeometry:
|
||||||
selected = set(cycles)
|
selected = set(cycles)
|
||||||
@@ -57,6 +75,7 @@ class ObservationGeometry:
|
|||||||
measurement.joint,
|
measurement.joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
|
|
||||||
def predicted_palm_orientation_local(
|
def predicted_palm_orientation_local(
|
||||||
@@ -78,6 +97,7 @@ class ObservationGeometry:
|
|||||||
measurement.model_joint,
|
measurement.model_joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
return axis / np.linalg.norm(axis)
|
return axis / np.linalg.norm(axis)
|
||||||
|
|
||||||
@@ -91,7 +111,6 @@ class ObservationGeometry:
|
|||||||
observed_child_point: np.ndarray,
|
observed_child_point: np.ndarray,
|
||||||
view_normal_common: Sequence[float] | None = None,
|
view_normal_common: Sequence[float] | None = None,
|
||||||
) -> float:
|
) -> float:
|
||||||
profile = self.profile
|
|
||||||
parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis)
|
parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis)
|
||||||
observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis)
|
observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis)
|
||||||
if float(parent_axis @ observed_axis) < 0.0:
|
if float(parent_axis @ observed_axis) < 0.0:
|
||||||
@@ -127,19 +146,11 @@ class ObservationGeometry:
|
|||||||
# mounts therefore choose different along-axis gauges.
|
# mounts therefore choose different along-axis gauges.
|
||||||
# Reusing delta here would reintroduce that arbitrary
|
# Reusing delta here would reintroduce that arbitrary
|
||||||
# coordinate and turn it into a phase at oblique views.
|
# coordinate and turn it into a phase at oblique views.
|
||||||
predicted_image_input = (
|
predicted_radial = predicted_radial - view_normal * float(
|
||||||
predicted_radial if profile.project_axis_gauge_before_image
|
predicted_radial @ view_normal
|
||||||
else predicted_delta
|
|
||||||
)
|
)
|
||||||
observed_image_input = (
|
observed_radial = observed_radial - view_normal * float(
|
||||||
observed_radial if profile.project_axis_gauge_before_image
|
observed_radial @ view_normal
|
||||||
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
|
|
||||||
)
|
)
|
||||||
angle_axis = view_normal
|
angle_axis = view_normal
|
||||||
if float(angle_axis @ parent_axis) < 0.0:
|
if float(angle_axis @ parent_axis) < 0.0:
|
||||||
@@ -201,11 +212,13 @@ class ObservationGeometry:
|
|||||||
anchor_joint,
|
anchor_joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
predicted_observer, _ = model.axis_line(
|
predicted_observer, _ = model.axis_line(
|
||||||
observer_joint,
|
observer_joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
observed_anchor = np.asarray(
|
observed_anchor = np.asarray(
|
||||||
anchor.axis_common_xyz, dtype=float
|
anchor.axis_common_xyz, dtype=float
|
||||||
@@ -225,6 +238,7 @@ class ObservationGeometry:
|
|||||||
selected: Sequence[JointAxisMeasurement],
|
selected: Sequence[JointAxisMeasurement],
|
||||||
base_rotation: Rotation,
|
base_rotation: Rotation,
|
||||||
base_translation: np.ndarray,
|
base_translation: np.ndarray,
|
||||||
|
*, only_offsets=None,
|
||||||
) -> dict[str, tuple[float, ...]]:
|
) -> dict[str, tuple[float, ...]]:
|
||||||
curves = self.curves
|
curves = self.curves
|
||||||
model = self.model
|
model = self.model
|
||||||
@@ -238,6 +252,8 @@ class ObservationGeometry:
|
|||||||
name: [] for name in profile.direct_zero_joints
|
name: [] for name in profile.direct_zero_joints
|
||||||
}
|
}
|
||||||
for offset_joint, observer_joint in profile.offset_observer_joint.items():
|
for offset_joint, observer_joint in profile.offset_observer_joint.items():
|
||||||
|
if only_offsets is not None and offset_joint not in only_offsets:
|
||||||
|
continue
|
||||||
pair_errors = same_view_axis_pair_errors(
|
pair_errors = same_view_axis_pair_errors(
|
||||||
offset_joint, selected, offsets
|
offset_joint, selected, offsets
|
||||||
)
|
)
|
||||||
@@ -272,6 +288,7 @@ class ObservationGeometry:
|
|||||||
parent_joint,
|
parent_joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
parent_axis = base_rotation.apply(parent_axis)
|
parent_axis = base_rotation.apply(parent_axis)
|
||||||
error = _zero_sensitive_axis_error_rad(
|
error = _zero_sensitive_axis_error_rad(
|
||||||
@@ -281,6 +298,8 @@ class ObservationGeometry:
|
|||||||
parent_joint = profile.phase_parent_joint[observer_joint]
|
parent_joint = profile.phase_parent_joint[observer_joint]
|
||||||
observed_parent = by_key.get((parent_joint, item.cycle))
|
observed_parent = by_key.get((parent_joint, item.cycle))
|
||||||
if observed_parent is None:
|
if observed_parent is None:
|
||||||
|
if item.cycle in self.incomplete_training_cycles:
|
||||||
|
continue
|
||||||
raise ValueError(
|
raise ValueError(
|
||||||
f"phase parent is missing: {parent_joint} cycle {item.cycle}"
|
f"phase parent is missing: {parent_joint} cycle {item.cycle}"
|
||||||
)
|
)
|
||||||
@@ -298,6 +317,7 @@ class ObservationGeometry:
|
|||||||
parent_joint,
|
parent_joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
parent_axis = base_rotation.apply(parent_axis)
|
parent_axis = base_rotation.apply(parent_axis)
|
||||||
parent_point = (
|
parent_point = (
|
||||||
|
|||||||
+72
-12
@@ -2,16 +2,17 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Mapping, Sequence
|
from typing import Mapping, Sequence
|
||||||
import math
|
import math
|
||||||
|
|
||||||
from ...domain.measurement import JointCurveFit
|
from ...domain.measurement import JointCurveFit
|
||||||
from .acceptance import assemble_result, decide_acceptance
|
from .acceptance import assemble_result, decide_acceptance, decide_training_acceptance
|
||||||
from .optimization import solve_selected
|
from .optimization import solve_selected
|
||||||
from .preparation import prepare_offset_limits, prepare_zero_problem
|
from .preparation import prepare_offset_limits, prepare_zero_problem
|
||||||
from .residuals import ObservationGeometry
|
from .residuals import ObservationGeometry
|
||||||
from .stages import TrainingFit, TrainingProblem, ZeroSolveOptions
|
from .stages import TrainingFit, TrainingProblem, ZeroSolveOptions, ObservabilityEvidence, CycleEvidence
|
||||||
from .statistics import estimate_cycle_evidence, estimate_observability
|
from .statistics import estimate_cycle_evidence, estimate_observability
|
||||||
from .types import (
|
from .types import (
|
||||||
JointAxisMeasurement,
|
JointAxisMeasurement,
|
||||||
@@ -22,6 +23,36 @@ from .types import (
|
|||||||
from .validation import check_observation_geometry, validate_axis_lines, validate_holdout
|
from .validation import check_observation_geometry, validate_axis_lines, validate_holdout
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class FrozenZeroTraining:
|
||||||
|
input_sha256: str
|
||||||
|
fit: TrainingFit
|
||||||
|
observability: ObservabilityEvidence
|
||||||
|
cycles: CycleEvidence
|
||||||
|
applied_offsets: Mapping[str, float]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ZeroTrainingResult:
|
||||||
|
offsets: Mapping[str, float]
|
||||||
|
confidence_half_widths: Mapping[str, float]
|
||||||
|
cycle_values: Mapping[str, Sequence[float]]
|
||||||
|
failures: tuple[str, ...]
|
||||||
|
frozen: FrozenZeroTraining | None = None
|
||||||
|
failure_reasons: Mapping[str, str] | None = None
|
||||||
|
|
||||||
|
|
||||||
|
def fit_urdf_zero_training(*, measurements, training_cycles, **kwargs):
|
||||||
|
"""Training-only entry: independent validation observations cannot enter."""
|
||||||
|
if any(item.cycle not in training_cycles for item in (
|
||||||
|
*measurements, *kwargs.get("palm_orientation_measurements", ()))):
|
||||||
|
raise ValueError("zero_training_received_nontraining_data")
|
||||||
|
if "validation_cycle" in kwargs:
|
||||||
|
raise ValueError("zero_training_has_no_validation_cycle")
|
||||||
|
return solve_urdf_zero_offsets(measurements=measurements, training_cycles=training_cycles,
|
||||||
|
validation_cycle=None, **kwargs)
|
||||||
|
|
||||||
|
|
||||||
def solve_urdf_zero_offsets(
|
def solve_urdf_zero_offsets(
|
||||||
*,
|
*,
|
||||||
source_urdf: str | Path,
|
source_urdf: str | Path,
|
||||||
@@ -30,7 +61,7 @@ def solve_urdf_zero_offsets(
|
|||||||
curves: Mapping[str, JointCurveFit],
|
curves: Mapping[str, JointCurveFit],
|
||||||
motor_by_joint: Mapping[str, int],
|
motor_by_joint: Mapping[str, int],
|
||||||
training_cycles: Sequence[int] = (0, 1),
|
training_cycles: Sequence[int] = (0, 1),
|
||||||
validation_cycle: int = 2,
|
validation_cycle: int | None = 2,
|
||||||
maximum_offset_rad: float = math.radians(20.0),
|
maximum_offset_rad: float = math.radians(20.0),
|
||||||
finger_maximum_offset_rad: float = math.radians(3.0),
|
finger_maximum_offset_rad: float = math.radians(3.0),
|
||||||
joint_maximum_offset_rad: Mapping[str, float] | None = None,
|
joint_maximum_offset_rad: Mapping[str, float] | None = None,
|
||||||
@@ -50,7 +81,9 @@ def solve_urdf_zero_offsets(
|
|||||||
fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None,
|
fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None,
|
||||||
static_output_zero_offsets_rad: Mapping[str, float] | None = None,
|
static_output_zero_offsets_rad: Mapping[str, float] | None = None,
|
||||||
zero_profile: ZeroCalibrationProfile | None = None,
|
zero_profile: ZeroCalibrationProfile | None = None,
|
||||||
) -> ZeroSolveResult:
|
allow_partial_training_cycles: bool = False,
|
||||||
|
frozen_training: FrozenZeroTraining | None = None,
|
||||||
|
) -> ZeroSolveResult | ZeroTrainingResult:
|
||||||
"""Prepare, fit training only, validate the frozen result, then assemble."""
|
"""Prepare, fit training only, validate the frozen result, then assemble."""
|
||||||
options = ZeroSolveOptions(
|
options = ZeroSolveOptions(
|
||||||
maximum_offset_rad=maximum_offset_rad,
|
maximum_offset_rad=maximum_offset_rad,
|
||||||
@@ -81,23 +114,50 @@ def solve_urdf_zero_offsets(
|
|||||||
static_output_zero_offsets_rad=static_output_zero_offsets_rad,
|
static_output_zero_offsets_rad=static_output_zero_offsets_rad,
|
||||||
zero_profile=zero_profile,
|
zero_profile=zero_profile,
|
||||||
)
|
)
|
||||||
geometry = ObservationGeometry(problem.profile, problem.model, curves, motor_by_joint, problem.orientation_by_model_cycle)
|
geometry = ObservationGeometry(problem.profile, problem.model, curves, motor_by_joint, problem.orientation_by_model_cycle,
|
||||||
|
frozenset(training_cycles) if allow_partial_training_cycles else frozenset())
|
||||||
training_geometry = geometry.for_cycles(training_cycles)
|
training_geometry = geometry.for_cycles(training_cycles)
|
||||||
training_problem = TrainingProblem(problem.profile, problem.training, problem.fixed_offsets, problem.zero_offsets)
|
training_problem = TrainingProblem(problem.profile, problem.training, problem.fixed_offsets, problem.zero_offsets)
|
||||||
limits = prepare_offset_limits(problem=problem, options=options)
|
limits = prepare_offset_limits(problem=problem, options=options)
|
||||||
rotation, translation, offsets = solve_selected(
|
from ..frozen import encode
|
||||||
problem=training_problem, limits=limits, geometry=training_geometry, selected=problem.training)
|
from ...domain.capture_plan import evidence_digest
|
||||||
fit = TrainingFit(rotation, translation, offsets)
|
import hashlib
|
||||||
observability = estimate_observability(problem=training_problem, fit=fit, geometry=training_geometry)
|
input_sha256 = evidence_digest(encode((tuple(problem.training), tuple(training_cycles),
|
||||||
|
curves, motor_by_joint, options, problem.profile,
|
||||||
|
hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest(),
|
||||||
|
training_geometry.orientation_by_model_cycle, allow_partial_training_cycles))) if (
|
||||||
|
validation_cycle is None or frozen_training is not None) else ""
|
||||||
|
if frozen_training is not None:
|
||||||
|
if frozen_training.input_sha256 != input_sha256:
|
||||||
|
raise ValueError("frozen_zero_training_inputs_changed")
|
||||||
|
fit, observability, cycles = (frozen_training.fit, frozen_training.observability, frozen_training.cycles)
|
||||||
|
rotation, offsets = fit.base_rotation, fit.training_offsets
|
||||||
|
else:
|
||||||
|
rotation, translation, offsets = solve_selected(
|
||||||
|
problem=training_problem, limits=limits, geometry=training_geometry, selected=problem.training)
|
||||||
|
fit = TrainingFit(rotation, translation, offsets)
|
||||||
|
observability = estimate_observability(problem=training_problem, fit=fit, geometry=training_geometry)
|
||||||
|
cycles = estimate_cycle_evidence(problem=training_problem, limits=limits, fit=fit, options=options,
|
||||||
|
geometry=training_geometry, measurements=problem.training, training_cycles=training_cycles)
|
||||||
geometry_evidence = check_observation_geometry(problem=problem, options=options,
|
geometry_evidence = check_observation_geometry(problem=problem, options=options,
|
||||||
geometry=geometry, selected=list(measurements), offsets=offsets, rotation=rotation)
|
geometry=geometry, selected=list(measurements), offsets=offsets, rotation=rotation)
|
||||||
cycles = estimate_cycle_evidence(problem=training_problem, limits=limits, fit=fit, options=options,
|
if validation_cycle is None:
|
||||||
geometry=training_geometry, measurements=problem.training, training_cycles=training_cycles)
|
decision = decide_training_acceptance(problem=problem, limits=limits, fit=fit, options=options,
|
||||||
|
observability=observability, geometry_evidence=geometry_evidence, cycles=cycles)
|
||||||
|
from .acceptance import applied_output_offsets
|
||||||
|
applied = applied_output_offsets(problem.profile, cycles.applied_training, problem.output_offsets,
|
||||||
|
limits.product_finger_rolls, decision.finger_roll_common_mode)
|
||||||
|
return ZeroTrainingResult(offsets, cycles.confidence_half_widths, cycles.cycle_values,
|
||||||
|
tuple(f"{reason}:{name}" for name, reason in sorted(decision.failure_reasons.items())),
|
||||||
|
FrozenZeroTraining(input_sha256, fit, observability, cycles, applied), dict(decision.failure_reasons))
|
||||||
holdout = validate_holdout(problem=problem, fit=fit, cycles=cycles, options=options,
|
holdout = validate_holdout(problem=problem, fit=fit, cycles=cycles, options=options,
|
||||||
geometry=geometry, measurements=measurements)
|
geometry=geometry, measurements=measurements)
|
||||||
lines = validate_axis_lines(problem=problem, fit=fit, cycles=cycles, geometry=geometry)
|
lines = validate_axis_lines(problem=problem, fit=fit, cycles=cycles, geometry=geometry)
|
||||||
decision = decide_acceptance(problem=problem, limits=limits, fit=fit, options=options,
|
decision = decide_acceptance(problem=problem, limits=limits, fit=fit, options=options,
|
||||||
observability=observability, geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout)
|
observability=observability, geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout)
|
||||||
return assemble_result(problem=problem, limits=limits, fit=fit, observability=observability,
|
result = assemble_result(problem=problem, limits=limits, fit=fit, observability=observability,
|
||||||
geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout, lines=lines,
|
geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout, lines=lines,
|
||||||
decision=decision, validation_cycle=validation_cycle)
|
decision=decision, validation_cycle=validation_cycle)
|
||||||
|
if frozen_training is not None and dict(result.direct_offsets_rad) != dict(frozen_training.applied_offsets):
|
||||||
|
raise ValueError('frozen_zero_applied_offsets_changed')
|
||||||
|
return result
|
||||||
|
|||||||
+7
-2
@@ -174,6 +174,9 @@ def estimate_cycle_evidence(
|
|||||||
training_cycle_ids = tuple(sorted({int(value) for value in training_cycles}))
|
training_cycle_ids = tuple(sorted({int(value) for value in training_cycles}))
|
||||||
for cycle in training_cycle_ids:
|
for cycle in training_cycle_ids:
|
||||||
selected = [item for item in measurements if item.cycle == cycle]
|
selected = [item for item in measurements if item.cycle == cycle]
|
||||||
|
available = geometry.complete_offsets(selected)
|
||||||
|
if not available:
|
||||||
|
continue
|
||||||
# Keep one palm pose while comparing cycles. Refitting a base pose from
|
# Keep one palm pose while comparing cycles. Refitting a base pose from
|
||||||
# only two nearly parallel root axes per cycle makes harmless root-line
|
# only two nearly parallel root axes per cycle makes harmless root-line
|
||||||
# noise appear as a large encoder-zero change.
|
# noise appear as a large encoder-zero change.
|
||||||
@@ -183,17 +186,19 @@ def estimate_cycle_evidence(
|
|||||||
cycle_rotation,
|
cycle_rotation,
|
||||||
cycle_translation,
|
cycle_translation,
|
||||||
initial=training_offsets,
|
initial=training_offsets,
|
||||||
|
fitted_names=available,
|
||||||
)
|
)
|
||||||
cycle_models[cycle] = (cycle_rotation, cycle_translation)
|
cycle_models[cycle] = (cycle_rotation, cycle_translation)
|
||||||
for name, value in cycle_offsets.items():
|
for name, value in cycle_offsets.items():
|
||||||
cycle_values[name].append(value)
|
if name in available:
|
||||||
|
cycle_values[name].append(value)
|
||||||
uncertainties: dict[str, float] = {}
|
uncertainties: dict[str, float] = {}
|
||||||
confidence_half_widths: dict[str, float] = {}
|
confidence_half_widths: dict[str, float] = {}
|
||||||
cycle_consistent = True
|
cycle_consistent = True
|
||||||
inconsistent_cycles: list[str] = []
|
inconsistent_cycles: list[str] = []
|
||||||
for name, values in cycle_values.items():
|
for name, values in cycle_values.items():
|
||||||
array = np.asarray(values, dtype=float)
|
array = np.asarray(values, dtype=float)
|
||||||
spread = float(np.max(array) - np.min(array))
|
spread = float(np.ptp(array)) if array.size else float("inf")
|
||||||
if spread > maximum_cycle_difference_rad:
|
if spread > maximum_cycle_difference_rad:
|
||||||
cycle_consistent = False
|
cycle_consistent = False
|
||||||
inconsistent_cycles.append(name)
|
inconsistent_cycles.append(name)
|
||||||
|
|||||||
+2
-6
@@ -58,14 +58,10 @@ class ZeroCalibrationProfile:
|
|||||||
# contains it. The frozen zero still faces every geometry/cycle/holdout
|
# contains it. The frozen zero still faces every geometry/cycle/holdout
|
||||||
# check below. Default False preserves existing G20/L6/O6 decisions.
|
# check below. Default False preserves existing G20/L6/O6 decisions.
|
||||||
accept_validated_zero_in_confidence_interval: bool = False
|
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
|
parallel_root_pattern: bool = False
|
||||||
common_mode_zero_joints: tuple[str, ...] = ()
|
common_mode_zero_joints: tuple[str, ...] = ()
|
||||||
small_offset_joints: frozenset[str] = frozenset()
|
small_offset_joints: frozenset[str] = frozenset()
|
||||||
|
independent_observed_motion: bool = False
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def reference_finger(self) -> str:
|
def reference_finger(self) -> str:
|
||||||
@@ -78,7 +74,7 @@ class JointAxisMeasurement:
|
|||||||
cycle: int
|
cycle: int
|
||||||
axis_common_xyz: tuple[float, float, float]
|
axis_common_xyz: tuple[float, float, float]
|
||||||
point_common_xyz_m: tuple[float, float, float]
|
point_common_xyz_m: tuple[float, float, float]
|
||||||
condition_state_u8: tuple[float, ...]
|
condition_state_u8: tuple[float, ...] | None
|
||||||
plane_rms_m: float
|
plane_rms_m: float
|
||||||
radial_rms_m: float
|
radial_rms_m: float
|
||||||
rotation_circle_axis_difference_rad: float
|
rotation_circle_axis_difference_rad: float
|
||||||
|
|||||||
+2
@@ -75,11 +75,13 @@ def check_observation_geometry(
|
|||||||
observer_joint,
|
observer_joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
parent_axis, _ = model.axis_line(
|
parent_axis, _ = model.axis_line(
|
||||||
parent_joint,
|
parent_joint,
|
||||||
zero_offsets=offsets,
|
zero_offsets=offsets,
|
||||||
joint_angles=angles,
|
joint_angles=angles,
|
||||||
|
independent_mimic_angles=profile.independent_observed_motion,
|
||||||
)
|
)
|
||||||
cone_mismatches.append(
|
cone_mismatches.append(
|
||||||
_axis_cone_mismatch_rad(
|
_axis_cone_mismatch_rad(
|
||||||
|
|||||||
+31
-10
@@ -14,6 +14,7 @@ import numpy as np
|
|||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
from ..geometry.rotation import robust_rotation_summary
|
from ..geometry.rotation import robust_rotation_summary
|
||||||
|
from ..domain.capture_plan import LEGACY_TRAINING
|
||||||
from ..urdf.kinematics import UrdfKinematicModel
|
from ..urdf.kinematics import UrdfKinematicModel
|
||||||
|
|
||||||
|
|
||||||
@@ -59,35 +60,52 @@ class FrozenTagInstallation:
|
|||||||
training_sample_ids: tuple[str, ...]
|
training_sample_ids: tuple[str, ...]
|
||||||
training_rotation_p95_deg: float
|
training_rotation_p95_deg: float
|
||||||
training_translation_p95_m: float
|
training_translation_p95_m: float
|
||||||
|
training_cycles: tuple[int, ...] = LEGACY_TRAINING
|
||||||
|
|
||||||
|
|
||||||
|
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(
|
def fit_tag_installations(
|
||||||
*, source_model: UrdfKinematicModel, common_from_base,
|
*, source_model: UrdfKinematicModel, common_from_base,
|
||||||
link_by_role: Mapping[str, str], observations: Sequence[TagTrainingPose],
|
link_by_role: Mapping[str, str], observations: Sequence[TagTrainingPose],
|
||||||
minimum_samples: int = 40,
|
minimum_samples: int = 40,
|
||||||
|
training_cycles_by_role: Mapping[str, Sequence[int]] | None = None,
|
||||||
) -> dict[str, FrozenTagInstallation]:
|
) -> dict[str, FrozenTagInstallation]:
|
||||||
"""Fit mounting transforms once across all tasks and three cycles.
|
"""Fit mounting transforms once across each role's declared training cycles.
|
||||||
|
|
||||||
Repeated records of one image/Tag are deduplicated only when their pose
|
Repeated records of one image/Tag are deduplicated only when their pose
|
||||||
and joint conditions agree. Conflicting duplicates are invalid evidence.
|
and joint conditions agree. Conflicting duplicates are invalid evidence.
|
||||||
Training residuals check rigid installation, not a per-frame PnP gate.
|
Training residuals check rigid installation, not a per-frame PnP gate.
|
||||||
"""
|
"""
|
||||||
base_from_common = np.linalg.inv(rigid_matrix(common_from_base))
|
base_from_common = np.linalg.inv(rigid_matrix(common_from_base))
|
||||||
|
partitions = {role: tuple(training_cycles_by_role[role]) if training_cycles_by_role is not None
|
||||||
|
else LEGACY_TRAINING for role in link_by_role}
|
||||||
|
if any(cycles not in (LEGACY_TRAINING, LEGACY_TRAINING[:2]) for cycles in partitions.values()):
|
||||||
|
raise ValueError("Tag installation requires declared independent training cycles")
|
||||||
grouped: dict[str, dict[str, tuple[int, np.ndarray]]] = {role: {} for role in link_by_role}
|
grouped: dict[str, dict[str, tuple[int, np.ndarray]]] = {role: {} for role in link_by_role}
|
||||||
for row in observations:
|
for row in observations:
|
||||||
if row.cycle not in {0, 1, 2} or not row.sample_id:
|
|
||||||
raise ValueError("only identified training images may fit Tag installations")
|
|
||||||
if row.role not in link_by_role:
|
if row.role not in link_by_role:
|
||||||
raise ValueError(f"undeclared Tag installation:{row.role}")
|
raise ValueError(f"undeclared Tag installation:{row.role}")
|
||||||
|
if row.cycle not in partitions[row.role] or not row.sample_id:
|
||||||
|
raise ValueError("only identified training images may fit Tag installations")
|
||||||
candidate = np.linalg.inv(link_pose(source_model, link_by_role[row.role], row.cad_angles)) @ base_from_common @ rigid_matrix(row.common_from_tag)
|
candidate = np.linalg.inv(link_pose(source_model, link_by_role[row.role], row.cad_angles)) @ base_from_common @ rigid_matrix(row.common_from_tag)
|
||||||
previous = grouped[row.role].get(row.sample_id)
|
previous = grouped[row.role].get(row.sample_id)
|
||||||
if previous is not None and (previous[0] != row.cycle or not np.allclose(previous[1], candidate, atol=1e-8, rtol=0)):
|
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}")
|
raise ValueError(f"conflicting duplicate Tag image:{row.role}:{row.sample_id}")
|
||||||
grouped[row.role][row.sample_id] = (row.cycle, candidate)
|
grouped[row.role][row.sample_id] = (row.cycle, candidate)
|
||||||
output = {}
|
output, diagnostics = {}, {}
|
||||||
for role, unique in grouped.items():
|
for role, unique in sorted(grouped.items()):
|
||||||
if len(unique) < minimum_samples or {value[0] for value in unique.values()} != {0, 1, 2}:
|
if len(unique) < minimum_samples or {value[0] for value in unique.values()} != set(partitions[role]):
|
||||||
raise ValueError(f"Tag installation lacks three-cycle training evidence:{role}")
|
raise ValueError(f"Tag installation lacks declared training evidence:{role}")
|
||||||
matrices = np.asarray([value[1] for value in unique.values()])
|
matrices = np.asarray([value[1] for value in unique.values()])
|
||||||
rotation = Rotation.from_quat(robust_rotation_summary(
|
rotation = Rotation.from_quat(robust_rotation_summary(
|
||||||
Rotation.from_matrix(matrices[:, :3, :3]).as_quat().tolist())[0])
|
Rotation.from_matrix(matrices[:, :3, :3]).as_quat().tolist())[0])
|
||||||
@@ -96,12 +114,15 @@ def fit_tag_installations(
|
|||||||
distances = np.linalg.norm(matrices[:, :3, 3] - translation, axis=1)
|
distances = np.linalg.norm(matrices[:, :3, 3] - translation, axis=1)
|
||||||
rotation_p95 = float(np.rad2deg(np.percentile(errors, 95)))
|
rotation_p95 = float(np.rad2deg(np.percentile(errors, 95)))
|
||||||
translation_p95 = float(np.percentile(distances, 95))
|
translation_p95 = float(np.percentile(distances, 95))
|
||||||
if rotation_p95 > 2.0 or translation_p95 > 0.003:
|
diagnostics[role] = {"count": len(unique), "rotation_p95_deg": rotation_p95,
|
||||||
raise ValueError(f"Tag installation is not rigid across training tasks:{role}:rotation_p95_deg={rotation_p95}:translation_p95_m={translation_p95}")
|
"translation_p95_m": translation_p95,
|
||||||
|
"passed": rotation_p95 <= 2.0 and translation_p95 <= 0.003}
|
||||||
transform = np.eye(4)
|
transform = np.eye(4)
|
||||||
transform[:3, :3], transform[:3, 3] = rotation.as_matrix(), translation
|
transform[:3, :3], transform[:3, 3] = rotation.as_matrix(), translation
|
||||||
output[role] = FrozenTagInstallation(role, link_by_role[role], matrix_tuple(transform),
|
output[role] = FrozenTagInstallation(role, link_by_role[role], matrix_tuple(transform),
|
||||||
tuple(sorted(unique)), rotation_p95, translation_p95)
|
tuple(sorted(unique)), rotation_p95, translation_p95, partitions[role])
|
||||||
|
if any(not row["passed"] for row in diagnostics.values()):
|
||||||
|
raise TagInstallationFailure(diagnostics)
|
||||||
return output
|
return output
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,174 @@
|
|||||||
|
"""Bounded training decisions; no holdout input and no hardware interface."""
|
||||||
|
|
||||||
|
from dataclasses import asdict, replace
|
||||||
|
import hashlib
|
||||||
|
from itertools import combinations
|
||||||
|
import math
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.stats import t as student_t
|
||||||
|
|
||||||
|
from ..domain.capture_plan import (CapturePlan, TASKWISE_STAGED, evidence_digest,
|
||||||
|
select_task_training, training_cycles)
|
||||||
|
from ..domain.motion_path import record_scan_identity
|
||||||
|
from ..domain.sampling import command_nodes
|
||||||
|
from ..urdf.kinematics import UrdfKinematicModel
|
||||||
|
from ..urdf.acceptance import angular_metrics
|
||||||
|
from .command_motion import fit_command_training
|
||||||
|
from .spatial import measure_joint_curve_observation
|
||||||
|
|
||||||
|
|
||||||
|
TRAINING_DECISION_VERSION = "training_decision_v1"
|
||||||
|
# Zero confidence uses the production spatial solver's unchanged limit.
|
||||||
|
MAXIMUM_CONFIDENCE_HALF_WIDTH_RAD = math.radians(1.)
|
||||||
|
TRAINING_FIELDS = ("kind", "joint", "view", "task_name", "cycle", "direction", "segment_key",
|
||||||
|
"attempt", "sample_phase", "image_stamp_ns", "motor_index", "command_u8", "command_rad",
|
||||||
|
"feedback_u8", "feedback_rad", "steady_index", "steady_target", "relative_quaternion_xyzw",
|
||||||
|
"relative_translation_xyz_m", "parent_pose_common", "child_pose_common", "view_normal_common_xyz",
|
||||||
|
"camera_center_common_xyz_m", "state_u8", "state_rad", "command_vector_u8", "command_vector_rad")
|
||||||
|
|
||||||
|
|
||||||
|
def training_snapshot(profile, records, task_key, cycles):
|
||||||
|
"""Latest attempts only. Validation rows are excluded before all statistics."""
|
||||||
|
chosen = select_task_training(profile, task_key, cycles)
|
||||||
|
allowed = {task.key: training_cycles(chosen, task) for task in chosen.motion.tasks}
|
||||||
|
latest = {}
|
||||||
|
for row in records:
|
||||||
|
if row.get("task_name") in allowed and row.get("cycle") in allowed[row["task_name"]]:
|
||||||
|
key = record_scan_identity(row)
|
||||||
|
latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1)))
|
||||||
|
rows = []
|
||||||
|
for row in records:
|
||||||
|
if (row.get("kind") != "joint_sample" or row.get("task_name") not in allowed
|
||||||
|
or row.get("cycle") not in allowed[row["task_name"]]
|
||||||
|
or row.get("sample_phase", "sweep") not in {"sweep", "steady"}
|
||||||
|
or int(row.get("attempt", 1)) != latest[record_scan_identity(row)]):
|
||||||
|
continue
|
||||||
|
rows.append({key: row[key] for key in TRAINING_FIELDS if key in row})
|
||||||
|
return tuple(rows)
|
||||||
|
|
||||||
|
|
||||||
|
def _by_joint(rows, phase):
|
||||||
|
result = {}
|
||||||
|
for row in rows:
|
||||||
|
if row.get("sample_phase", "sweep") == phase:
|
||||||
|
result.setdefault(row["joint"], []).append({**row,
|
||||||
|
"sample_id": f"{row['view']}:{row['image_stamp_ns']}"})
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def _curve_repeatability(profile, task, motion, steady, cycles):
|
||||||
|
failures, metrics = [], {}
|
||||||
|
for name in task.joints:
|
||||||
|
curve = motion.coordinate_curves[name]
|
||||||
|
values, by_direction = [], {}
|
||||||
|
for direction in ("increasing", "decreasing"):
|
||||||
|
directional_values = []
|
||||||
|
for node in command_nodes(profile, task):
|
||||||
|
repeated = []
|
||||||
|
for cycle in cycles:
|
||||||
|
rows = [row for row in steady[name] if row["cycle"] == cycle
|
||||||
|
and row["direction"] == direction
|
||||||
|
and math.isclose(float(row["steady_target"]), node, abs_tol=1e-9)]
|
||||||
|
repeated.append(float(np.median([measure_joint_curve_observation(curve,
|
||||||
|
quaternion_xyzw=row["relative_quaternion_xyzw"]) for row in rows])))
|
||||||
|
values.append(repeated)
|
||||||
|
directional_values.append(repeated)
|
||||||
|
array = np.asarray(directional_values)
|
||||||
|
errors = np.concatenate([array[:, a]-array[:, b] for a, b in combinations(range(len(cycles)), 2)])
|
||||||
|
by_direction[direction] = angular_metrics(errors)
|
||||||
|
array = np.asarray(values)
|
||||||
|
spread = float(np.max(np.ptp(array, axis=1)))
|
||||||
|
width = float(np.max(student_t.ppf(.975, len(cycles)-1)
|
||||||
|
* np.std(array, axis=1, ddof=1) / math.sqrt(len(cycles))))
|
||||||
|
metrics[name] = {"maximum_cycle_difference_rad": spread,
|
||||||
|
"maximum_angle_confidence_half_width_rad": width,
|
||||||
|
"independent_cycle_count": len(cycles),
|
||||||
|
"repeatability_by_direction": {key: asdict(value) for key, value in by_direction.items()}}
|
||||||
|
# Curve consistency is an angular error gate, not a zero-offset CI.
|
||||||
|
# Applying the 1-degree zero CI limit at every command knot would
|
||||||
|
# introduce a stricter, unrelated rejection of otherwise sound data.
|
||||||
|
if not all(value.passed for value in by_direction.values()):
|
||||||
|
failures.append(f"training_repeatability:{name}")
|
||||||
|
return failures, metrics
|
||||||
|
|
||||||
|
|
||||||
|
def assess_training(profile, task_key, cycles, rows, references, source_urdf):
|
||||||
|
"""Assess one immutable task snapshot without reading validation data.
|
||||||
|
|
||||||
|
Adaptive collection keeps the older conservative behavior: missing
|
||||||
|
full-hand geometry requests a third round. Taskwise staged collection uses
|
||||||
|
local two-round repeatability to decide immediately; its full-hand spatial
|
||||||
|
fit remains a separate mandatory gate after every training task is frozen.
|
||||||
|
"""
|
||||||
|
cycles = tuple(cycles)
|
||||||
|
chosen = select_task_training(profile, task_key, cycles)
|
||||||
|
task = next(t for t in profile.motion.tasks if t.key == task_key)
|
||||||
|
taskwise = profile.quality.training_policy == TASKWISE_STAGED
|
||||||
|
if taskwise:
|
||||||
|
# A task decision is deliberately local and therefore remains stable
|
||||||
|
# when later fingers are acquired. Full-hand zero geometry is checked
|
||||||
|
# by the final training-only fit before any artifact can be published.
|
||||||
|
rows = tuple(row for row in rows if row.get("task_name") == task_key)
|
||||||
|
if any(row["cycle"] not in training_cycles(chosen, row["task_name"]) for row in rows):
|
||||||
|
raise ValueError("training_assessment_received_nontraining_data")
|
||||||
|
sweep, steady = _by_joint(rows, "sweep"), _by_joint(rows, "steady")
|
||||||
|
reference_inputs = {name: {"baseline_command": ref.baseline_command,
|
||||||
|
"quaternion": ref.quaternion(profile.measurement.measurements[name].view).tolist()}
|
||||||
|
for name, ref in references.items() if name in sweep}
|
||||||
|
source_hash = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest() if source_urdf is not None else ""
|
||||||
|
source = {"rows": rows, "references": reference_inputs, "source_urdf": source_hash}
|
||||||
|
failures, metrics = [], {}
|
||||||
|
fitted_model = None
|
||||||
|
geometry = {"status": "deferred", "reason": "spatial_dependencies_not_complete"}
|
||||||
|
try:
|
||||||
|
if source_urdf is None:
|
||||||
|
raise ValueError("training_source_urdf_missing")
|
||||||
|
model = UrdfKinematicModel(source_urdf)
|
||||||
|
local = fit_command_training(chosen, model,
|
||||||
|
{name: sweep[name] for name in task.joints}, {name: steady[name] for name in task.joints},
|
||||||
|
{name: references[name] for name in task.joints})
|
||||||
|
fitted_model = {"curves": {name: asdict(curve) for name, curve in local.coordinate_curves.items()},
|
||||||
|
"mappings": {name: asdict(mapping) for name, mapping in local.mappings.items()}}
|
||||||
|
failures, metrics = _curve_repeatability(chosen, task, local, steady, cycles)
|
||||||
|
required = set() if taskwise else set(profile.zero.axis_joints)
|
||||||
|
ready = bool(required) and required <= sweep.keys() & steady.keys() & references.keys()
|
||||||
|
if ready:
|
||||||
|
from .session import compile_spatial_profile, build_axis_observations
|
||||||
|
from .spatial_solver.solve import fit_urdf_zero_training
|
||||||
|
motion = fit_command_training(chosen, model, {name: sweep[name] for name in required},
|
||||||
|
{name: steady[name] for name in required}, {name: references[name] for name in required})
|
||||||
|
zero_profile = compile_spatial_profile(chosen)
|
||||||
|
axes = build_axis_observations(chosen, model, steady, motion, zero_profile, include_holdout=False)
|
||||||
|
baselines = {**{name: value.cad_angle_rad for name, value in profile.zero.known_baseline_geometry.items()},
|
||||||
|
**{name: 0. for name in profile.zero.cad_zero_assumptions}}
|
||||||
|
curves = {name: replace(curve, circle={**curve.circle, "known_cad_baseline_rad": baselines[name]})
|
||||||
|
if name in baselines and name not in zero_profile.direct_zero_joints else curve
|
||||||
|
for name, curve in motion.coordinate_curves.items()}
|
||||||
|
from .motion_fit import channel_for_joint
|
||||||
|
zero = fit_urdf_zero_training(source_urdf=source_urdf, measurements=axes,
|
||||||
|
training_cycles=tuple(sorted({a.cycle for a in axes})), curves=curves,
|
||||||
|
motor_by_joint={name: channel_for_joint(profile, name) for name in curves},
|
||||||
|
zero_profile=zero_profile, maximum_offset_rad=math.radians(20),
|
||||||
|
finger_maximum_offset_rad=math.radians(20),
|
||||||
|
maximum_confidence_half_width_rad=MAXIMUM_CONFIDENCE_HALF_WIDTH_RAD,
|
||||||
|
maximum_pose_axis_line_rms_m=.0015, allow_partial_training_cycles=True)
|
||||||
|
geometry = {"status": "checked", "zero_offsets_rad": dict(zero.offsets),
|
||||||
|
"zero_confidence_half_width_rad": {name: width if math.isfinite(width) else None
|
||||||
|
for name, width in zero.confidence_half_widths.items()},
|
||||||
|
"independent_cycles_by_joint": {name: len(values) for name, values in zero.cycle_values.items()}}
|
||||||
|
failures.extend(zero.failures)
|
||||||
|
except (ValueError, KeyError, np.linalg.LinAlgError) as error:
|
||||||
|
failures.append(str(error))
|
||||||
|
decision = ("add_training" if len(cycles) == 2 and (failures
|
||||||
|
or (geometry["status"] == "deferred" and not taskwise))
|
||||||
|
else "fail" if failures else "freeze")
|
||||||
|
record = {"kind": "training_decision", "version": TRAINING_DECISION_VERSION,
|
||||||
|
"task_name": task_key, "assessed_cycles": list(cycles), "decision": decision,
|
||||||
|
"failures": failures, "metrics": metrics, "geometry": geometry,
|
||||||
|
"frozen_training_sha256": (evidence_digest({"local_model": fitted_model, "geometry": geometry})
|
||||||
|
if decision == "freeze" else None),
|
||||||
|
"source_sha256": evidence_digest(source), "source_count": len(rows),
|
||||||
|
"input_plan_sha256": CapturePlan.from_profile(profile).sha256}
|
||||||
|
return {**record, "decision_sha256": evidence_digest(record)}
|
||||||
+147
-12
@@ -5,11 +5,18 @@ poses, substitute SDK angles, or relax the spatial solver's acceptance gates.
|
|||||||
"""
|
"""
|
||||||
from itertools import product
|
from itertools import product
|
||||||
import math
|
import math
|
||||||
|
from collections.abc import Mapping
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from scipy.spatial.transform import Rotation as R
|
from scipy.spatial.transform import Rotation as R
|
||||||
|
|
||||||
from .pnp import solve_square_tag_ippe
|
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 .tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence
|
||||||
|
from .image_motion_replay import IMAGE_POSE_SOURCE, has_image_model, replay_image_models, validate_image_sample
|
||||||
|
|
||||||
POLICY = 'training_frozen_articulated_candidates_v1'
|
POLICY = 'training_frozen_articulated_candidates_v1'
|
||||||
MATRIX_SOURCE = 'CameraInfo.P[:3,:3]'
|
MATRIX_SOURCE = 'CameraInfo.P[:3,:3]'
|
||||||
@@ -41,27 +48,127 @@ def _trajectory_score(parent, child):
|
|||||||
'score': rms/.0015 + off_axis/math.radians(1)}
|
'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', IMAGE_REFERENCE_SOURCE}
|
||||||
|
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, IMAGE_REFERENCE_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)
|
||||||
|
if source == IMAGE_REFERENCE_SOURCE:
|
||||||
|
data = frame['roles'][role]
|
||||||
|
reference = reference_from_evidence(item, stamp_ns=stamp, camera_matrix=frame['camera_matrix'])
|
||||||
|
recorded_ref = reference_from_evidence(data, stamp_ns=stamp, camera_matrix=frame['camera_matrix'])
|
||||||
|
if reference != recorded_ref or not np.array_equal(item.get('corners_xy'), data.get('corners_xy')):
|
||||||
|
raise ValueError(f'frozen_branch_image_reference_changed:{identity}:role={role}')
|
||||||
|
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,
|
def resolve_chain_observations(records, *, task_key, role_pairs,
|
||||||
projection_override=None, solve_pose=solve_square_tag_ippe):
|
projection_override=None, solve_pose=solve_square_tag_ippe, fixed_roles=(),
|
||||||
|
require_frozen_branches=False, training_cycles=(0, 1, 2), holdout_cycle=3):
|
||||||
"""Return copied observations and auditable candidate-selection evidence.
|
"""Return copied observations and auditable candidate-selection evidence.
|
||||||
|
|
||||||
Older captures did not save P. They remain on the legacy path; K is never
|
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
|
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.
|
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]
|
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))
|
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()):
|
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')
|
raise ValueError('candidate selection requires a bounded articulated observation graph')
|
||||||
joint_rows = [r for r in rows if r.get('joint') in role_pairs
|
joint_rows = [r for r in rows if r.get('joint') in role_pairs
|
||||||
and r.get('task_name') == task_key]
|
and r.get('task_name') == task_key and r.get('sample_phase') != 'joint_zero']
|
||||||
evidence = {int(r['image_stamp_ns']): r for r in rows
|
evidence = {int(r['image_stamp_ns']): r for r in rows
|
||||||
if ('roles' in r or str(r.get('kind', '')).endswith('pnp_candidate_frame'))
|
if ('roles' in r or str(r.get('kind', '')).endswith('pnp_candidate_frame'))
|
||||||
and r.get('task_name') == task_key}
|
and r.get('task_name') == task_key}
|
||||||
report = {'policy': POLICY, 'training_cycles': [0, 1, 2], 'holdout_cycle': 3,
|
report = {'policy': POLICY, 'training_cycles': list(training_cycles), 'holdout_cycle': holdout_cycle,
|
||||||
'projection_override_used': projection_override is not None,
|
'projection_override_used': projection_override is not None,
|
||||||
'is_accuracy_certificate': False}
|
'is_accuracy_certificate': False}
|
||||||
if not evidence:
|
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'}
|
return rows, {**report, 'status': 'legacy_no_corners'}
|
||||||
if projection_override is None and any(
|
if projection_override is None and any(
|
||||||
r.get('camera_matrix_source') != MATRIX_SOURCE for r in evidence.values()):
|
r.get('camera_matrix_source') != MATRIX_SOURCE for r in evidence.values()):
|
||||||
@@ -76,24 +183,49 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
|
|||||||
stamps = sorted({int(r['image_stamp_ns']) for r in chosen_rows})
|
stamps = sorted({int(r['image_stamp_ns']) for r in chosen_rows})
|
||||||
if not stamps or any(s not in evidence for s in stamps):
|
if not stamps or any(s not in evidence for s in stamps):
|
||||||
raise ValueError('corner replay lacks evidence for accepted observations')
|
raise ValueError('corner replay lacks evidence for accepted observations')
|
||||||
training = np.asarray([int(evidence[s]['cycle']) in (0, 1, 2) for s in stamps])
|
if any(int(evidence[s]['cycle']) not in (*training_cycles, holdout_cycle) for s in stamps):
|
||||||
if sum(training) < 40 or not any(int(evidence[s]['cycle']) == 3 for s in stamps):
|
raise ValueError('corner replay observation outside capture plan')
|
||||||
|
training = np.asarray([int(evidence[s]['cycle']) in training_cycles for s in stamps])
|
||||||
|
if sum(training) < 40 or not any(int(evidence[s]['cycle']) == holdout_cycle for s in stamps):
|
||||||
raise ValueError('corner replay requires training and independent holdout')
|
raise ValueError('corner replay requires training and independent holdout')
|
||||||
if np.flatnonzero(training)[-1] > np.flatnonzero(~training)[0]:
|
if np.flatnonzero(training)[-1] > np.flatnonzero(~training)[0]:
|
||||||
raise ValueError('holdout must follow the complete training trajectory')
|
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}
|
paths = {role: [[], []] for role in roles}
|
||||||
for stamp in stamps:
|
for stamp in stamps:
|
||||||
frame = evidence[stamp]
|
frame = evidence[stamp]
|
||||||
matrix = np.asarray(frame['camera_matrix'] if projection_override is None else projection_override)
|
matrix = np.asarray(frame['camera_matrix'] if projection_override is None else projection_override)
|
||||||
for role in roles:
|
for role in roles:
|
||||||
data = frame['roles'][role]
|
data = frame['roles'][role]
|
||||||
poses = solve_pose(data['corners_xy'], tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=matrix)
|
if data.get('locked_reference'):
|
||||||
limit = float(data.get('maximum_reprojection_error_px', 1.5))
|
if role not in fixed_roles or not data.get('selected'):
|
||||||
poses = [p for p in poses if p.reprojection_error_px <= limit]
|
raise ValueError('cached fixed reference has no frozen pose')
|
||||||
if not poses:
|
cs = [dict(data['selected'],
|
||||||
raise ValueError(f'corner replay has no eligible pose:{stamp}:{role}')
|
reprojection_error_px=float(data['selected'].get('reprojection_error_px', 0.0)))]
|
||||||
cs = [dict(quaternion_xyzw=list(p.quaternion_xyzw), translation_xyz_m=list(p.translation_xyz_m),
|
else:
|
||||||
reprojection_error_px=p.reprojection_error_px) for p in poses[:2]]
|
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))
|
||||||
|
poses = [p for p in poses if p.reprojection_error_px <= limit]
|
||||||
|
if not poses:
|
||||||
|
raise ValueError(f'corner replay has no eligible pose:{stamp}:{role}')
|
||||||
|
cs = [dict(quaternion_xyzw=list(p.quaternion_xyzw), translation_xyz_m=list(p.translation_xyz_m),
|
||||||
|
reprojection_error_px=p.reprojection_error_px) for p in poses[:2]]
|
||||||
if len(cs) == 1:
|
if len(cs) == 1:
|
||||||
cs.append(cs[0])
|
cs.append(cs[0])
|
||||||
if paths[role][0]:
|
if paths[role][0]:
|
||||||
@@ -115,6 +247,9 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
|
|||||||
selected = min(scores, key=lambda s: s['score'])
|
selected = min(scores, key=lambda s: s['score'])
|
||||||
lookup = {stamp: {role: paths[role][branch][i] for role, branch in zip(roles, selected['branches'])}
|
lookup = {stamp: {role: paths[role][branch][i] for role, branch in zip(roles, selected['branches'])}
|
||||||
for i, stamp in enumerate(stamps)}
|
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
|
changed = 0
|
||||||
for row in chosen_rows:
|
for row in chosen_rows:
|
||||||
stamp = int(row['image_stamp_ns'])
|
stamp = int(row['image_stamp_ns'])
|
||||||
|
|||||||
@@ -0,0 +1,105 @@
|
|||||||
|
"""Share immutable, content-bound image models without copying them per frame.
|
||||||
|
|
||||||
|
The JSON representation and its hash are unchanged. Per-image diagnostics stay
|
||||||
|
independent mutable dictionaries; only the already-frozen model is shared.
|
||||||
|
"""
|
||||||
|
from functools import cached_property, lru_cache
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
|
||||||
|
|
||||||
|
class FrozenEvidence(dict):
|
||||||
|
@cached_property
|
||||||
|
def canonical_json(self):
|
||||||
|
return json.dumps(self, sort_keys=True, separators=(',', ':'), allow_nan=False)
|
||||||
|
|
||||||
|
@cached_property
|
||||||
|
def sha256(self):
|
||||||
|
return hashlib.sha256(self.canonical_json.encode()).hexdigest()
|
||||||
|
|
||||||
|
def _immutable(self, *args, **kwargs):
|
||||||
|
raise TypeError('frozen evidence cannot be changed')
|
||||||
|
|
||||||
|
__setitem__ = __delitem__ = clear = pop = popitem = setdefault = update = __ior__ = _immutable
|
||||||
|
|
||||||
|
def __deepcopy__(self, memo):
|
||||||
|
return self
|
||||||
|
|
||||||
|
def __reduce__(self):
|
||||||
|
return (freeze_evidence, (dict(self),))
|
||||||
|
|
||||||
|
|
||||||
|
def freeze_evidence(value):
|
||||||
|
if isinstance(value, dict):
|
||||||
|
return FrozenEvidence((key, freeze_evidence(item)) for key, item in value.items())
|
||||||
|
if isinstance(value, (list, tuple)):
|
||||||
|
return tuple(freeze_evidence(item) for item in value)
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def canonical_evidence(value):
|
||||||
|
"""Cache serialization only for recursively immutable evidence."""
|
||||||
|
if isinstance(value, FrozenEvidence):
|
||||||
|
return value.canonical_json
|
||||||
|
return json.dumps(value, sort_keys=True, separators=(',', ':'), allow_nan=False)
|
||||||
|
|
||||||
|
|
||||||
|
def evidence_sha256(value):
|
||||||
|
if isinstance(value, FrozenEvidence):
|
||||||
|
return value.sha256
|
||||||
|
return hashlib.sha256(canonical_evidence(value).encode()).hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
@lru_cache(maxsize=64)
|
||||||
|
def image_model_evidence(model):
|
||||||
|
"""The model dataclasses contain immutable tuples and are value-hashable."""
|
||||||
|
from .tag_pose.image_motion_model import image_model_payload
|
||||||
|
return freeze_evidence(image_model_payload(model))
|
||||||
|
|
||||||
|
|
||||||
|
class EvidenceDecoder:
|
||||||
|
"""Intern verified models while streaming old and new JSONL records."""
|
||||||
|
def __init__(self, *, verify_hashes=True):
|
||||||
|
self.models = {}
|
||||||
|
self.references = {}
|
||||||
|
self.verify_hashes = verify_hashes
|
||||||
|
|
||||||
|
def _intern(self, payload, identity, cache, error):
|
||||||
|
if not self.verify_hashes:
|
||||||
|
# Passed-task resume defers replay to finalization. Sharing must
|
||||||
|
# still preserve the exact content, even for conflicting digests.
|
||||||
|
previous = cache.get(identity)
|
||||||
|
if previous is not None and payload == previous[0]:
|
||||||
|
return previous[1]
|
||||||
|
frozen = freeze_evidence(payload)
|
||||||
|
if previous is None:
|
||||||
|
cache[identity] = (payload, frozen)
|
||||||
|
return frozen
|
||||||
|
digest = hashlib.sha256(json.dumps(payload, sort_keys=True,
|
||||||
|
separators=(',', ':'), allow_nan=False).encode()).hexdigest()
|
||||||
|
if identity != digest:
|
||||||
|
raise ValueError(error)
|
||||||
|
if identity not in cache:
|
||||||
|
cache[identity] = freeze_evidence(payload)
|
||||||
|
return cache[identity]
|
||||||
|
|
||||||
|
def object_hook(self, row):
|
||||||
|
payload = row.get('frozen_image_model')
|
||||||
|
if payload is not None:
|
||||||
|
row['frozen_image_model'] = self._intern(payload, row.get('image_model_sha256'),
|
||||||
|
self.models, 'image_motion_provenance_model_hash_invalid')
|
||||||
|
reference = row.get('reference_frame')
|
||||||
|
if reference is not None:
|
||||||
|
row['reference_frame'] = self._intern(reference, row.get('reference_frame_sha256'),
|
||||||
|
self.references, 'image_reference_identity_changed')
|
||||||
|
return row
|
||||||
|
|
||||||
|
def loads(self, line):
|
||||||
|
# Most evidence rows contain observations only. Avoid recursively
|
||||||
|
# invoking the model interning hook for every nested pose/candidate
|
||||||
|
# dictionary when neither interned object can be present.
|
||||||
|
frozen_key = b'"frozen_image_model"' if isinstance(line, bytes) else '"frozen_image_model"'
|
||||||
|
reference_key = b'"reference_frame"' if isinstance(line, bytes) else '"reference_frame"'
|
||||||
|
if frozen_key not in line and reference_key not in line:
|
||||||
|
return json.loads(line)
|
||||||
|
return json.loads(line, object_hook=self.object_hook)
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user