Compare commits
22 Commits
366de4aad9
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
|
1bbcb535aa | ||
|
|
c1f29bc1ee | ||
|
|
dba2744687 | ||
|
|
33dcbe72bf | ||
|
|
fab7032d56 | ||
|
|
9bf49eff8d | ||
|
|
da2a5d5ae6 | ||
|
|
7818604c02 | ||
| 1db62e08df | |||
|
|
ce829d3875 | ||
| fe08ebc08c | |||
| b9df99540d | |||
| 2c0aeb718a | |||
| f9db05d073 | |||
|
|
aa78116837 | ||
|
|
d36ef6189d | ||
|
|
eb403dab36 | ||
|
|
5f8e1a0189 | ||
|
|
498499bf13 | ||
|
|
a3986beb6e | ||
|
|
855f917d24 | ||
|
|
f585c83689 |
49
data/homing/20260625_172504/aruco_marker_poses.csv
Normal file
49
data/homing/20260625_172504/aruco_marker_poses.csv
Normal file
@@ -0,0 +1,49 @@
|
||||
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
|
||||
0,unknown,,2,505.2,-100.88,-7.16,0.02189,0.03624,0.9991,,,,,,24.24
|
||||
46,Board,A0,2,537.83,185.51,-27.88,-0.08568,-0.05354,0.99488,537.44,185.2,-27.2,0.841,-0.679,23.2
|
||||
47,Board,A0,2,343.18,-286.29,-27.16,-0.03123,0.01004,0.99946,343.18,-286.05,-27.49,0.407,0.326,24.03
|
||||
50,Board,A0,2,574.07,210.38,-26.23,-0.00073,0.03209,0.99948,574.23,211.48,-27.15,1.448,0.925,24.06
|
||||
51,Board,A0,2,166.73,-171.08,-27.09,-0.03386,0.01172,0.99936,167.18,-170.93,-27.76,0.818,0.667,24.31
|
||||
53,Board,A0,2,487.37,212.32,-27.38,-0.07361,-0.01564,0.99716,487.08,212.19,-27.28,0.33,-0.103,23.53
|
||||
54,Board,A0,3,341.07,-330.3,-27.22,-0.03362,0.04017,0.99863,341.05,-330.09,-27.5,0.351,0.28,24.36
|
||||
55,Arm1,A0,2,282.65,-261.75,-26.65,-0.05274,0.01696,0.99846,,,,,,24.32
|
||||
58,Board,A0,2,48.62,-216.5,-27.86,-0.00162,-0.0094,0.99995,49.3,-216.52,-27.93,0.684,0.068,24.24
|
||||
62,Board,A0,3,404.15,-174.92,-26.96,0.01614,-0.01502,0.99976,404.07,-174.84,-27.4,0.454,0.439,23.97
|
||||
64,Board,A0,2,-22.59,-186.68,-26.97,-0.00648,0.01133,0.99991,-21.95,-186.37,-28.04,1.286,1.074,24.24
|
||||
66,Board,A0,2,208.51,-363.21,-27.64,-0.04961,0.03216,0.99825,208.41,-362.24,-27.7,0.98,0.055,24.38
|
||||
68,Board,A0,2,574.39,169.06,-26.11,-0.00044,0.05167,0.99866,574.4,170.26,-27.15,1.591,1.04,24.52
|
||||
73,Board,A0,2,221.97,337.22,-30.29,-0.05172,0.1462,0.9879,223.01,334.05,-27.67,4.242,-2.621,26.49
|
||||
76,Board,A0,2,686.21,165.02,-27.13,-0.04548,-0.03114,0.99848,685.86,166,-26.98,1.054,-0.149,23.28
|
||||
79,Board,A0,2,311.51,-157.95,-27.04,0.05023,-0.04899,0.99754,311.73,-158.5,-27.54,0.776,0.496,23.36
|
||||
82,Board,A0,2,219.37,300.46,-29.79,0.00751,0.1232,0.99235,220.31,298.19,-27.68,3.238,-2.111,25.14
|
||||
85,Board,A0,3,503.91,-313.51,-27.06,0.00515,-0.00866,0.99995,503.43,-312.87,-27.25,0.816,0.191,24.1
|
||||
90,Board,A0,2,644.93,316.2,-28.16,-0.02384,-0.04569,0.99867,644.39,315.79,-27.04,1.307,-1.118,23.32
|
||||
91,Board,A0,2,725.49,327.85,-27.51,-0.02708,0.03694,0.99895,724.61,327.11,-26.92,1.289,-0.587,24.21
|
||||
92,Board,A0,2,644.7,-186.93,-25.55,-0.05883,-0.0185,0.9981,644.42,-185.49,-27.04,2.094,1.489,23.69
|
||||
95,Board,A0,3,184.77,-273.26,-27.61,-0.03697,0.01835,0.99915,185.04,-272.99,-27.73,0.401,0.121,24.34
|
||||
96,Board,A0,3,369.2,-185.74,-27.61,-0.04836,0.02152,0.9986,369.1,-186.1,-27.46,0.398,-0.148,24.06
|
||||
103,Board,A0,3,104.63,-186.32,-27.25,-0.01721,0.02455,0.99955,105.03,-186.33,-27.85,0.721,0.6,24.42
|
||||
105,Board,A0,3,524.3,-267.15,-27.08,-0.016,0.01938,0.99968,523.86,-266.44,-27.22,0.848,0.137,23.92
|
||||
118,unknown,,3,322.99,-174.22,47.16,0.02335,-0.99462,0.10089,,,,,,24.14
|
||||
122,Ellbow,,3,359.95,-173.78,46.07,0.00204,-0.99361,0.11286,,,,,,24.26
|
||||
143,Arm2,,2,340.07,-138.22,229.66,-0.72695,-0.68151,0.08421,,,,,,24.03
|
||||
144,Arm2,,3,362.93,-157.15,158.16,-0.05456,-0.9917,0.11642,,,,,,24.31
|
||||
146,Arm2,,2,337.31,-147.71,160.02,-0.68833,-0.72303,0.05854,,,,,,24.0
|
||||
147,FingerA,,3,383.25,-144.05,226.5,0.43778,-0.89607,0.07357,,,,,,23.85
|
||||
148,Arm2,,3,367.21,-142.91,264.34,-0.04936,-0.99586,0.07639,,,,,,24.28
|
||||
178,FingerB,,2,287.73,-121.4,315.33,-0.67179,-0.7215,-0.16775,,,,,,23.25
|
||||
179,FingerB,,2,329.15,-139.64,311.4,-0.61251,-0.17201,0.77152,,,,,,23.53
|
||||
198,Arm1,,2,268.53,-53.98,84.4,0.00864,0.01319,0.99988,,,,,,24.79
|
||||
200,unknown,,2,199.66,-28.82,110.04,-0.15134,-0.01695,0.98834,,,,,,23.71
|
||||
204,unknown,,2,198.48,115.43,120.91,0.03546,0.04428,0.99839,,,,,,24.17
|
||||
208,Board,rail,2,626.35,-98.62,-6.79,-0.00084,0.01537,0.99988,631.01,-98.43,-7.71,4.748,0.917,24.04
|
||||
210,Board,rail,2,129.9,-7.86,-5.67,0.06219,0.02253,0.99781,122.63,-13.98,-0.72,10.715,-4.946,23.24
|
||||
214,unknown,,2,531.62,-8.59,-6.83,-0.02922,0.04196,0.99869,,,,,,24.25
|
||||
217,Board,rail,2,730.01,-8.51,-5.0,-0.08513,-0.01438,0.99627,732.39,-23.88,7.39,19.881,-12.386,23.12
|
||||
229,Arm1,,3,271.17,-142.11,79.1,0.01062,-0.04516,0.99892,,,,,,23.94
|
||||
243,Arm1,,2,270.3,-176.61,43.73,0.02044,-0.9996,-0.01934,,,,,,24.13
|
||||
|
||||
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
|
||||
cam0,335.33,-885.66,468.37,-0.06588,0.89247,-0.44628
|
||||
cam1,296.81,-462.82,780.61,0.09503,0.4331,-0.89632
|
||||
cam2,712.91,-665.53,724.31,-0.42887,0.59445,-0.68022
|
||||
|
2002
data/homing/20260625_172504/aruco_marker_poses.json
Normal file
2002
data/homing/20260625_172504/aruco_marker_poses.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
data/homing/20260625_172504/cam0.jpg
Normal file
BIN
data/homing/20260625_172504/cam0.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 98 KiB |
2645
data/homing/20260625_172504/cam0_aruco_detection.json
Normal file
2645
data/homing/20260625_172504/cam0_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
481
data/homing/20260625_172504/cam0_camera_pose.json
Normal file
481
data/homing/20260625_172504/cam0_camera_pose.json
Normal file
@@ -0,0 +1,481 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:09Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_172504/cam0_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam0",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1424.7584228515625,
|
||||
0.0,
|
||||
635.95947265625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1421.5770263671875,
|
||||
482.1744384765625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.05634751915931702,
|
||||
0.33765655755996704,
|
||||
0.002130246954038739,
|
||||
-0.004022662527859211,
|
||||
-1.182201862335205
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 20,
|
||||
"used_marker_ids": [
|
||||
97,
|
||||
66,
|
||||
85,
|
||||
54,
|
||||
105,
|
||||
69,
|
||||
47,
|
||||
95,
|
||||
58,
|
||||
64,
|
||||
103,
|
||||
62,
|
||||
96,
|
||||
208,
|
||||
51,
|
||||
79,
|
||||
210,
|
||||
68,
|
||||
50,
|
||||
91
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011403454671871994,
|
||||
0.001728926659833975,
|
||||
0.0016691755968926973,
|
||||
0.0016691753847773315
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 3.4096867660331136,
|
||||
"residual_median_px": 1.5691231791727809,
|
||||
"residual_max_px": 12.026369061549262,
|
||||
"sigma2_normalized": 3.2778193707572165e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.9973888397216797,
|
||||
0.04563671350479126,
|
||||
-0.05597161129117012
|
||||
],
|
||||
[
|
||||
-0.02958603762090206,
|
||||
-0.4488010108470917,
|
||||
-0.8931418061256409
|
||||
],
|
||||
[
|
||||
-0.0658801719546318,
|
||||
0.8924656510353088,
|
||||
-0.4462788999080658
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.2678244411945343,
|
||||
0.030760858207941055,
|
||||
1.0215346813201904
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.0344335845398724,
|
||||
0.011289329253343375,
|
||||
-0.08570511152514877
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.33533409237861633,
|
||||
-0.885656476020813,
|
||||
0.4683726131916046
|
||||
],
|
||||
"position_mm": [
|
||||
335.3341064453125,
|
||||
-885.656494140625,
|
||||
468.37261962890625
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 116.56741333007812,
|
||||
"pitch": 3.777391195297241,
|
||||
"yaw": -1.6990946531295776
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
9.702707532310775e-06,
|
||||
1.2273484057683694e-06,
|
||||
3.575388647845507e-06,
|
||||
-1.2199370555784652e-07,
|
||||
-2.1440042443296295e-06,
|
||||
1.039618282302021e-07
|
||||
],
|
||||
[
|
||||
1.22734840576837e-06,
|
||||
3.358446334387221e-06,
|
||||
1.8878909236797321e-07,
|
||||
4.651297819399596e-07,
|
||||
-8.835069052569738e-07,
|
||||
6.206962238461149e-07
|
||||
],
|
||||
[
|
||||
3.5753886478454758e-06,
|
||||
1.8878909236796024e-07,
|
||||
1.143731789507973e-05,
|
||||
-7.216885572151019e-07,
|
||||
-2.2145565854988443e-06,
|
||||
-3.0088157401175667e-06
|
||||
],
|
||||
[
|
||||
-1.219937055578466e-07,
|
||||
4.651297819399599e-07,
|
||||
-7.216885572151033e-07,
|
||||
2.2942348340442282e-07,
|
||||
3.6332485943604063e-08,
|
||||
3.2363949947789566e-07
|
||||
],
|
||||
[
|
||||
-2.1440042443296215e-06,
|
||||
-8.835069052569716e-07,
|
||||
-2.2145565854988506e-06,
|
||||
3.633248594360388e-08,
|
||||
9.86494679764419e-07,
|
||||
6.579597825341255e-07
|
||||
],
|
||||
[
|
||||
1.039618282302198e-07,
|
||||
6.206962238461218e-07,
|
||||
-3.0088157401175595e-06,
|
||||
3.2363949947789524e-07,
|
||||
6.579597825341204e-07,
|
||||
2.970696318843356e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.1784715940965861,
|
||||
0.10500061405868079,
|
||||
0.19376919211544102
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0004789817151044733,
|
||||
0.0009932243854056438,
|
||||
0.0017235708047084566
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0024386451901829424,
|
||||
0.0013863680263389562,
|
||||
0.002737631446074117
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.4386451901829425,
|
||||
1.3863680263389562,
|
||||
2.7376314460741167
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.19011944203297743,
|
||||
"pitch": 0.14270986978198957,
|
||||
"yaw": 0.1181609242753551
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
676.25,
|
||||
910.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
675.9551391601562,
|
||||
911.3131713867188
|
||||
],
|
||||
"reprojection_error_px": 0.8649801263910383,
|
||||
"confidence": 0.42385670146087856
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
480.5,
|
||||
921.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
480.5653381347656,
|
||||
919.2376098632812
|
||||
],
|
||||
"reprojection_error_px": 1.763600880544741,
|
||||
"confidence": 0.2797311732321247
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
1080.0,
|
||||
843.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1077.551025390625,
|
||||
842.6098022460938
|
||||
],
|
||||
"reprojection_error_px": 2.605749158768581,
|
||||
"confidence": 0.6761152978036918
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
753.5,
|
||||
868.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
753.16845703125,
|
||||
868.4518432617188
|
||||
],
|
||||
"reprojection_error_px": 0.33502210609070604,
|
||||
"confidence": 0.6745646371332027
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1098.25,
|
||||
783.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1096.3798828125,
|
||||
782.7687377929688
|
||||
],
|
||||
"reprojection_error_px": 2.0080046589625047,
|
||||
"confidence": 0.53843639257073
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
130.25,
|
||||
818.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
131.2628631591797,
|
||||
816.5780639648438
|
||||
],
|
||||
"reprojection_error_px": 1.9548048201489217,
|
||||
"confidence": 0.6309056746154269
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
755.0,
|
||||
810.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
754.8045043945312,
|
||||
810.226318359375
|
||||
],
|
||||
"reprojection_error_px": 0.19692473653729947,
|
||||
"confidence": 0.558404255319149
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
461.25,
|
||||
799.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
462.01556396484375,
|
||||
799.7348022460938
|
||||
],
|
||||
"reprojection_error_px": 0.7657148006869643,
|
||||
"confidence": 0.5573620390355706
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
243.0,
|
||||
742.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
244.86248779296875,
|
||||
742.42578125
|
||||
],
|
||||
"reprojection_error_px": 1.8639659872994379,
|
||||
"confidence": 0.4721784486231997
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
139.75,
|
||||
714.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
141.61831665039062,
|
||||
715.1568603515625
|
||||
],
|
||||
"reprojection_error_px": 2.197483328524737,
|
||||
"confidence": 0.4471706966400147
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
350.75,
|
||||
708.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.2232971191406,
|
||||
708.7933959960938
|
||||
],
|
||||
"reprojection_error_px": 1.6733444379103959,
|
||||
"confidence": 0.3922909999211629
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
851.5,
|
||||
685.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
851.7431640625,
|
||||
686.1382446289062
|
||||
],
|
||||
"reprojection_error_px": 0.9209274032584249,
|
||||
"confidence": 0.3564780454484243
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
793.25,
|
||||
698.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
793.7991333007812,
|
||||
698.2991943359375
|
||||
],
|
||||
"reprojection_error_px": 0.6253516072450701,
|
||||
"confidence": 0.3348906742607279
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
1202.5,
|
||||
583.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1209.5457763671875,
|
||||
583.9437866210938
|
||||
],
|
||||
"reprojection_error_px": 7.108705775496229,
|
||||
"confidence": 0.28965074531908364
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
458.0,
|
||||
690.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
459.341064453125,
|
||||
691.0894775390625
|
||||
],
|
||||
"reprojection_error_px": 1.4649019204351656,
|
||||
"confidence": 0.34674061533734285
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
696.25,
|
||||
672.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
697.0543212890625,
|
||||
673.733154296875
|
||||
],
|
||||
"reprojection_error_px": 1.6872105394342276,
|
||||
"confidence": 0.3256815826862768
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
439.25,
|
||||
532.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.3565673828125,
|
||||
530.716796875
|
||||
],
|
||||
"reprojection_error_px": 12.026369061549262,
|
||||
"confidence": 0.15571821530659996
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
1027.25,
|
||||
434.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1027.0755615234375,
|
||||
434.6865539550781
|
||||
],
|
||||
"reprojection_error_px": 0.47011502613700773,
|
||||
"confidence": 0.09323781394061914
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
1017.0,
|
||||
413.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1016.9035034179688,
|
||||
413.9068298339844
|
||||
],
|
||||
"reprojection_error_px": 0.41811733301008686,
|
||||
"confidence": 0.08307692198670814
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
1165.5,
|
||||
355.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1165.020751953125,
|
||||
355.35980224609375
|
||||
],
|
||||
"reprojection_error_px": 0.6180072633772071,
|
||||
"confidence": 0.06871920537654881
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_172504/cam0_debug.jpg
Normal file
BIN
data/homing/20260625_172504/cam0_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 209 KiB |
BIN
data/homing/20260625_172504/cam1.jpg
Normal file
BIN
data/homing/20260625_172504/cam1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 108 KiB |
3662
data/homing/20260625_172504/cam1_aruco_detection.json
Normal file
3662
data/homing/20260625_172504/cam1_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
761
data/homing/20260625_172504/cam1_camera_pose.json
Normal file
761
data/homing/20260625_172504/cam1_camera_pose.json
Normal file
@@ -0,0 +1,761 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:13Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_172504/cam1_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam1",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1367.5723876953125,
|
||||
0.0,
|
||||
672.1165771484375
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1372.3011474609375,
|
||||
445.8396911621094
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.01016925647854805,
|
||||
0.7656787633895874,
|
||||
-0.0031530377455055714,
|
||||
-0.00288817984983325,
|
||||
-2.490830183029175
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 40,
|
||||
"used_marker_ids": [
|
||||
54,
|
||||
95,
|
||||
58,
|
||||
85,
|
||||
47,
|
||||
103,
|
||||
59,
|
||||
48,
|
||||
105,
|
||||
51,
|
||||
102,
|
||||
96,
|
||||
62,
|
||||
71,
|
||||
92,
|
||||
63,
|
||||
208,
|
||||
210,
|
||||
217,
|
||||
74,
|
||||
75,
|
||||
52,
|
||||
68,
|
||||
76,
|
||||
46,
|
||||
53,
|
||||
101,
|
||||
50,
|
||||
100,
|
||||
82,
|
||||
60,
|
||||
67,
|
||||
73,
|
||||
94,
|
||||
70,
|
||||
104,
|
||||
98,
|
||||
90,
|
||||
91,
|
||||
88
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011372574297308506,
|
||||
0.0021949616225605375,
|
||||
0.002143709970633132,
|
||||
0.002143709690342172
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 4.23364353800265,
|
||||
"residual_median_px": 1.0679787737296105,
|
||||
"residual_max_px": 19.348939630537274,
|
||||
"sigma2_normalized": 4.968098633993014e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.9951881766319275,
|
||||
0.019756384193897247,
|
||||
-0.0959698036313057
|
||||
],
|
||||
[
|
||||
-0.02385592833161354,
|
||||
0.9011316895484924,
|
||||
0.4328886866569519
|
||||
],
|
||||
[
|
||||
0.09503374248743057,
|
||||
0.43309515714645386,
|
||||
-0.8963242769241333
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.37943923473358154,
|
||||
0.0862211212515831,
|
||||
0.8719164729118347
|
||||
],
|
||||
"rvec_rad": [
|
||||
0.0032072073338993575,
|
||||
-2.967110340660477,
|
||||
-0.6774876643558738
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.2968088686466217,
|
||||
-0.4628157615661621,
|
||||
0.7806104421615601
|
||||
],
|
||||
"position_mm": [
|
||||
296.8088684082031,
|
||||
-462.8157653808594,
|
||||
780.6104125976562
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 154.21060180664062,
|
||||
"pitch": -5.453261375427246,
|
||||
"yaw": -178.62680053710938
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
2.2460757709580997e-06,
|
||||
-1.0592111231264017e-07,
|
||||
-5.911479445197522e-07,
|
||||
9.807677828663091e-09,
|
||||
6.656021402664132e-07,
|
||||
1.8807173299942092e-07
|
||||
],
|
||||
[
|
||||
-1.059211123126279e-07,
|
||||
7.819602866884685e-06,
|
||||
-8.896995228371429e-07,
|
||||
-8.928123497147304e-07,
|
||||
8.447874310868068e-07,
|
||||
-3.883503279890717e-06
|
||||
],
|
||||
[
|
||||
-5.911479445197253e-07,
|
||||
-8.896995228370399e-07,
|
||||
1.8982339503369444e-05,
|
||||
-6.420456812663701e-07,
|
||||
-7.934435641861599e-07,
|
||||
3.082335762957178e-07
|
||||
],
|
||||
[
|
||||
9.807677828661003e-09,
|
||||
-8.928123497147341e-07,
|
||||
-6.42045681266356e-07,
|
||||
2.4448642118127635e-07,
|
||||
-7.775978286233749e-08,
|
||||
3.809119252748882e-07
|
||||
],
|
||||
[
|
||||
6.656021402664136e-07,
|
||||
8.447874310868029e-07,
|
||||
-7.934435641861794e-07,
|
||||
-7.775978286233639e-08,
|
||||
4.178437168687293e-07,
|
||||
-3.614395664399821e-07
|
||||
],
|
||||
[
|
||||
1.880717329994141e-07,
|
||||
-3.883503279890717e-06,
|
||||
3.08233576295777e-07,
|
||||
3.809119252748862e-07,
|
||||
-3.614395664399844e-07,
|
||||
2.809559727575109e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.08586868930820345,
|
||||
0.16021935571577825,
|
||||
0.24963041613501347
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0004944556817160425,
|
||||
0.000646408320544166,
|
||||
0.001676174134025194
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0028646163342597826,
|
||||
0.0025089246532298257,
|
||||
0.001967934179216506
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.8646163342597823,
|
||||
2.5089246532298257,
|
||||
1.967934179216506
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.1994418033453169,
|
||||
"pitch": 0.16062277696661237,
|
||||
"yaw": 0.07987033548886921
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
735.25,
|
||||
38.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
735.0857543945312,
|
||||
38.939788818359375
|
||||
],
|
||||
"reprojection_error_px": 0.4694580105501786,
|
||||
"confidence": 0.2985929580100399
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
1005.5,
|
||||
139.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1004.7282104492188,
|
||||
139.58082580566406
|
||||
],
|
||||
"reprojection_error_px": 0.9659284275868637,
|
||||
"confidence": 0.9357112460666233
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
1234.75,
|
||||
235.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1234.0849609375,
|
||||
235.2240447998047
|
||||
],
|
||||
"reprojection_error_px": 0.6655453606389707,
|
||||
"confidence": 0.36770453019575644
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
458.0,
|
||||
68.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
458.2991027832031,
|
||||
69.70124816894531
|
||||
],
|
||||
"reprojection_error_px": 1.2379255382753527,
|
||||
"confidence": 0.8685664374455692
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
731.5,
|
||||
117.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
731.38525390625,
|
||||
117.72209930419922
|
||||
],
|
||||
"reprojection_error_px": 0.485844027498816,
|
||||
"confidence": 0.9524913713047937
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
1128.75,
|
||||
284.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1128.236083984375,
|
||||
284.30267333984375
|
||||
],
|
||||
"reprojection_error_px": 0.5504974858473882,
|
||||
"confidence": 0.9144001450649527
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
262.25,
|
||||
123.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
263.9138488769531,
|
||||
125.33772277832031
|
||||
],
|
||||
"reprojection_error_px": 2.479035718842208,
|
||||
"confidence": 0.9376517069038668
|
||||
},
|
||||
{
|
||||
"marker_id": 48,
|
||||
"observed_center_px": [
|
||||
148.5,
|
||||
51.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
149.4849395751953,
|
||||
51.862728118896484
|
||||
],
|
||||
"reprojection_error_px": 1.3093531891436279,
|
||||
"confidence": 0.5517245546325658
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
432.0,
|
||||
147.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
432.3002014160156,
|
||||
149.29176330566406
|
||||
],
|
||||
"reprojection_error_px": 1.5707179826022124,
|
||||
"confidence": 0.9442241264421224
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
1019.75,
|
||||
308.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1019.1754150390625,
|
||||
307.9459228515625
|
||||
],
|
||||
"reprojection_error_px": 0.5771240900522866,
|
||||
"confidence": 0.9297130863840988
|
||||
},
|
||||
{
|
||||
"marker_id": 102,
|
||||
"observed_center_px": [
|
||||
239.25,
|
||||
213.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
240.9429473876953,
|
||||
215.61300659179688
|
||||
],
|
||||
"reprojection_error_px": 2.5173129361648683,
|
||||
"confidence": 0.9498751926981491
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
689.75,
|
||||
280.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
689.872314453125,
|
||||
280.85595703125
|
||||
],
|
||||
"reprojection_error_px": 0.37638575097840954,
|
||||
"confidence": 0.8766456914396368
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
634.75,
|
||||
296.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
634.6029052734375,
|
||||
297.34588623046875
|
||||
],
|
||||
"reprojection_error_px": 0.6137729696270053,
|
||||
"confidence": 0.8625841801430425
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
63.5,
|
||||
113.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
64.10832977294922,
|
||||
114.1216812133789
|
||||
],
|
||||
"reprojection_error_px": 0.8698003470479402,
|
||||
"confidence": 0.6962965934908386
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
256.25,
|
||||
273.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
257.945556640625,
|
||||
274.84429931640625
|
||||
],
|
||||
"reprojection_error_px": 2.327381067178508,
|
||||
"confidence": 0.8785732433921414
|
||||
},
|
||||
{
|
||||
"marker_id": 63,
|
||||
"observed_center_px": [
|
||||
36.75,
|
||||
191.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
37.489715576171875,
|
||||
191.14720153808594
|
||||
],
|
||||
"reprojection_error_px": 0.7468243819019208,
|
||||
"confidence": 0.23403229407345516
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
295.25,
|
||||
413.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
288.3956298828125,
|
||||
413.3526916503906
|
||||
],
|
||||
"reprojection_error_px": 6.8658752994838395,
|
||||
"confidence": 0.7372980849807326
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
1059.0,
|
||||
559.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.6019287109375,
|
||||
555.7957763671875
|
||||
],
|
||||
"reprojection_error_px": 15.127861931469159,
|
||||
"confidence": 0.6136138322804316
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
162.25,
|
||||
532.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
146.7255401611328,
|
||||
520.9512939453125
|
||||
],
|
||||
"reprojection_error_px": 19.348939630537274,
|
||||
"confidence": 0.6320655742155583
|
||||
},
|
||||
{
|
||||
"marker_id": 74,
|
||||
"observed_center_px": [
|
||||
1086.0,
|
||||
734.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1084.8804931640625,
|
||||
735.0576782226562
|
||||
],
|
||||
"reprojection_error_px": 1.1610174177882524,
|
||||
"confidence": 0.49861888008985367
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
1251.5,
|
||||
801.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1249.17626953125,
|
||||
800.1643676757812
|
||||
],
|
||||
"reprojection_error_px": 2.8131749250632194,
|
||||
"confidence": 0.06713952659324364
|
||||
},
|
||||
{
|
||||
"marker_id": 52,
|
||||
"observed_center_px": [
|
||||
1075.25,
|
||||
807.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.9066162109375,
|
||||
807.3248291015625
|
||||
],
|
||||
"reprojection_error_px": 1.354756379713829,
|
||||
"confidence": 0.44660041827617547
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
421.5,
|
||||
733.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
422.2747497558594,
|
||||
733.5101318359375
|
||||
],
|
||||
"reprojection_error_px": 0.9276161243969003,
|
||||
"confidence": 0.453656743367513
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
274.0,
|
||||
722.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
274.78045654296875,
|
||||
723.6485595703125
|
||||
],
|
||||
"reprojection_error_px": 1.19017717876916,
|
||||
"confidence": 0.42914121819077033
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
472.5,
|
||||
751.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
472.7325744628906,
|
||||
751.6512451171875
|
||||
],
|
||||
"reprojection_error_px": 0.4637763737575183,
|
||||
"confidence": 0.42705375163400877
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
541.0,
|
||||
783.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
541.320556640625,
|
||||
783.3278198242188
|
||||
],
|
||||
"reprojection_error_px": 0.32986737469810884,
|
||||
"confidence": 0.4239630899600427
|
||||
},
|
||||
{
|
||||
"marker_id": 101,
|
||||
"observed_center_px": [
|
||||
1031.25,
|
||||
900.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1029.7542724609375,
|
||||
900.5302124023438
|
||||
],
|
||||
"reprojection_error_px": 1.4960326401403623,
|
||||
"confidence": 0.3366794154513476
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
427.25,
|
||||
777.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.572998046875,
|
||||
777.9152221679688
|
||||
],
|
||||
"reprojection_error_px": 0.5260581593870906,
|
||||
"confidence": 0.3955545216798782
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
120.0,
|
||||
723.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
122.01116180419922,
|
||||
723.669677734375
|
||||
],
|
||||
"reprojection_error_px": 2.018306799327716,
|
||||
"confidence": 0.4621067158671878
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
894.75,
|
||||
890.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
893.3291625976562,
|
||||
890.5499267578125
|
||||
],
|
||||
"reprojection_error_px": 1.523547952267044,
|
||||
"confidence": 0.37908960001789416
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
613.0,
|
||||
860.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
612.6180419921875,
|
||||
860.841552734375
|
||||
],
|
||||
"reprojection_error_px": 0.39277706514463534,
|
||||
"confidence": 0.3907528879258137
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
497.75,
|
||||
837.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
497.95806884765625,
|
||||
838.1924438476562
|
||||
],
|
||||
"reprojection_error_px": 0.48892658313275084,
|
||||
"confidence": 0.3865272468846743
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
889.25,
|
||||
925.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
887.6505126953125,
|
||||
926.299560546875
|
||||
],
|
||||
"reprojection_error_px": 1.6912647434798707,
|
||||
"confidence": 0.15682847151496923
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
29.0,
|
||||
720.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
31.336706161499023,
|
||||
720.2244873046875
|
||||
],
|
||||
"reprojection_error_px": 2.3528924604549735,
|
||||
"confidence": 0.09146699566173024
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
401.25,
|
||||
866.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
401.60943603515625,
|
||||
867.041259765625
|
||||
],
|
||||
"reprojection_error_px": 0.46262999734212534,
|
||||
"confidence": 0.35896437880813437
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
107.5,
|
||||
792.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
109.24372100830078,
|
||||
792.2886352539062
|
||||
],
|
||||
"reprojection_error_px": 1.7674482353344945,
|
||||
"confidence": 0.3880719051456888
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
436.5,
|
||||
882.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
436.5618896484375,
|
||||
883.1439819335938
|
||||
],
|
||||
"reprojection_error_px": 0.8961216581333906,
|
||||
"confidence": 0.36026641726859315
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
351.75,
|
||||
880.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.1582336425781,
|
||||
880.5147094726562
|
||||
],
|
||||
"reprojection_error_px": 0.4865447685943754,
|
||||
"confidence": 0.3567245846660417
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
254.25,
|
||||
887.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
255.22390747070312,
|
||||
886.9551391601562
|
||||
],
|
||||
"reprojection_error_px": 0.9749401296709685,
|
||||
"confidence": 0.3494656541641164
|
||||
},
|
||||
{
|
||||
"marker_id": 88,
|
||||
"observed_center_px": [
|
||||
199.75,
|
||||
872.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
201.13644409179688,
|
||||
872.6148681640625
|
||||
],
|
||||
"reprojection_error_px": 1.3911944201992585,
|
||||
"confidence": 0.347903013426907
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_172504/cam1_debug.jpg
Normal file
BIN
data/homing/20260625_172504/cam1_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 257 KiB |
BIN
data/homing/20260625_172504/cam2.jpg
Normal file
BIN
data/homing/20260625_172504/cam2.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 220 KiB |
2521
data/homing/20260625_172504/cam2_aruco_detection.json
Normal file
2521
data/homing/20260625_172504/cam2_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
495
data/homing/20260625_172504/cam2_camera_pose.json
Normal file
495
data/homing/20260625_172504/cam2_camera_pose.json
Normal file
@@ -0,0 +1,495 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:15Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_172504/cam2_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam2",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1388.99072265625,
|
||||
0.0,
|
||||
933.082763671875
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1394.8729248046875,
|
||||
562.4996948242188
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.019531700760126114,
|
||||
-0.11213663965463638,
|
||||
0.0026758278254419565,
|
||||
0.0007694826927036047,
|
||||
0.05339815095067024
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 21,
|
||||
"used_marker_ids": [
|
||||
85,
|
||||
92,
|
||||
105,
|
||||
54,
|
||||
93,
|
||||
66,
|
||||
217,
|
||||
62,
|
||||
96,
|
||||
95,
|
||||
79,
|
||||
76,
|
||||
103,
|
||||
64,
|
||||
46,
|
||||
90,
|
||||
53,
|
||||
86,
|
||||
84,
|
||||
82,
|
||||
73
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.014228673477920699,
|
||||
0.0012360717022841035,
|
||||
0.0009070175499997285,
|
||||
0.0009070142696966918
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 1.7852212006525887,
|
||||
"residual_median_px": 1.0469264348760319,
|
||||
"residual_max_px": 6.647095648204168,
|
||||
"sigma2_normalized": 9.597873663078857e-07
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.7617526054382324,
|
||||
0.6427322030067444,
|
||||
0.08141452819108963
|
||||
],
|
||||
[
|
||||
0.48559483885765076,
|
||||
-0.4832412004470825,
|
||||
-0.7284748554229736
|
||||
],
|
||||
[
|
||||
-0.42887139320373535,
|
||||
0.5944520831108093,
|
||||
-0.6802176833152771
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.1742696762084961,
|
||||
-0.1401522308588028,
|
||||
1.1940648555755615
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.1767839356074785,
|
||||
0.8396398702141942,
|
||||
-0.2585585732707637
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.7129078507423401,
|
||||
-0.6655329465866089,
|
||||
0.7243147492408752
|
||||
],
|
||||
"position_mm": [
|
||||
712.9078369140625,
|
||||
-665.532958984375,
|
||||
724.3147583007812
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 138.84930419921875,
|
||||
"pitch": 25.395954132080078,
|
||||
"yaw": 32.51630783081055
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
2.4924502189037485e-06,
|
||||
7.284908255672299e-07,
|
||||
2.0024504778922095e-07,
|
||||
6.993863992084647e-08,
|
||||
-3.359070245275866e-07,
|
||||
5.600958340179941e-09
|
||||
],
|
||||
[
|
||||
7.284908255672354e-07,
|
||||
1.064107327675121e-06,
|
||||
-2.3219104124960886e-08,
|
||||
2.319589057300187e-07,
|
||||
-2.846525717659831e-07,
|
||||
1.5012786826699554e-07
|
||||
],
|
||||
[
|
||||
2.0024504778920997e-07,
|
||||
-2.3219104124972556e-08,
|
||||
4.278959925593973e-06,
|
||||
-1.4661732034947215e-07,
|
||||
-6.23228741847165e-07,
|
||||
-1.5703080494058352e-06
|
||||
],
|
||||
[
|
||||
6.993863992084778e-08,
|
||||
2.3195890573001912e-07,
|
||||
-1.4661732034946982e-07,
|
||||
1.0962701995068561e-07,
|
||||
-2.8204197612513988e-08,
|
||||
1.4125379662533936e-07
|
||||
],
|
||||
[
|
||||
-3.359070245275866e-07,
|
||||
-2.8465257176598095e-07,
|
||||
-6.232287418471685e-07,
|
||||
-2.8204197612513405e-08,
|
||||
2.305112527858795e-07,
|
||||
2.649685210729187e-07
|
||||
],
|
||||
[
|
||||
5.600958340183888e-09,
|
||||
1.5012786826699972e-07,
|
||||
-1.5703080494058346e-06,
|
||||
1.4125379662534013e-07,
|
||||
2.6496852107291726e-07,
|
||||
1.078572601871589e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.09045568752546955,
|
||||
0.05910379253809281,
|
||||
0.11852002706372318
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0003310997130030251,
|
||||
0.00048011587433231107,
|
||||
0.0010385435002307746
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0015926294915772563,
|
||||
0.001282414773137875,
|
||||
0.0014297273307869451
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
1.5926294915772563,
|
||||
1.282414773137875,
|
||||
1.4297273307869451
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.12744173458500993,
|
||||
"pitch": 0.08524351737408967,
|
||||
"yaw": 0.06595345807707714
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
943.25,
|
||||
1039.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
943.3576049804688,
|
||||
1037.9586181640625
|
||||
],
|
||||
"reprojection_error_px": 1.0469264348760319,
|
||||
"confidence": 0.25017342512163626
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
1262.25,
|
||||
1041.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1262.399658203125,
|
||||
1040.8380126953125
|
||||
],
|
||||
"reprojection_error_px": 0.9241852745384849,
|
||||
"confidence": 0.18638720024967376
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1019.5,
|
||||
1007.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1019.3496704101562,
|
||||
1006.0000610351562
|
||||
],
|
||||
"reprojection_error_px": 1.2589465443049024,
|
||||
"confidence": 0.7893291944675445
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
726.75,
|
||||
892.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
727.0918579101562,
|
||||
891.9924926757812
|
||||
],
|
||||
"reprojection_error_px": 0.4279916503422691,
|
||||
"confidence": 0.712278812924601
|
||||
},
|
||||
{
|
||||
"marker_id": 93,
|
||||
"observed_center_px": [
|
||||
1893.5,
|
||||
971.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1894.048095703125,
|
||||
971.9451904296875
|
||||
],
|
||||
"reprojection_error_px": 0.70611855836639,
|
||||
"confidence": 0.042321015168188404
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
548.25,
|
||||
804.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
549.4153442382812,
|
||||
803.2486572265625
|
||||
],
|
||||
"reprojection_error_px": 1.5364616961092168,
|
||||
"confidence": 0.5883494177188935
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
1527.25,
|
||||
916.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1529.0672607421875,
|
||||
922.8938598632812
|
||||
],
|
||||
"reprojection_error_px": 6.647095648204168,
|
||||
"confidence": 0.45290577054800996
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
962.0,
|
||||
802.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
961.2557373046875,
|
||||
802.291748046875
|
||||
],
|
||||
"reprojection_error_px": 0.7728491674409725,
|
||||
"confidence": 0.6057547967936824
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
912.25,
|
||||
783.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
911.0736694335938,
|
||||
783.0982055664062
|
||||
],
|
||||
"reprojection_error_px": 1.18608395635878,
|
||||
"confidence": 0.5545391608146377
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
630.5,
|
||||
709.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
631.0589599609375,
|
||||
709.0905151367188
|
||||
],
|
||||
"reprojection_error_px": 0.6929026563578145,
|
||||
"confidence": 0.4695004591117706
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
875.75,
|
||||
715.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
875.3380126953125,
|
||||
715.4541625976562
|
||||
],
|
||||
"reprojection_error_px": 0.6131861090513578,
|
||||
"confidence": 0.5142115324222218
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
1551.75,
|
||||
746.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1552.177490234375,
|
||||
744.9830322265625
|
||||
],
|
||||
"reprojection_error_px": 1.576051752365359,
|
||||
"confidence": 0.29593493306278906
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
648.5,
|
||||
590.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
648.8373413085938,
|
||||
590.6143798828125
|
||||
],
|
||||
"reprojection_error_px": 0.7009007054415375,
|
||||
"confidence": 0.39305998326235797
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
541.0,
|
||||
511.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
541.9498901367188,
|
||||
512.197021484375
|
||||
],
|
||||
"reprojection_error_px": 1.3413206043684687,
|
||||
"confidence": 0.3099175748319284
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
1382.0,
|
||||
629.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1381.5179443359375,
|
||||
628.320556640625
|
||||
],
|
||||
"reprojection_error_px": 0.8330791930264032,
|
||||
"confidence": 0.26236530151152454
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
1573.0,
|
||||
614.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1573.05712890625,
|
||||
612.7874145507812
|
||||
],
|
||||
"reprojection_error_px": 1.4637007577355878,
|
||||
"confidence": 0.21907023701319386
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
1341.0,
|
||||
580.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1340.56494140625,
|
||||
579.7637939453125
|
||||
],
|
||||
"reprojection_error_px": 0.49504472552160556,
|
||||
"confidence": 0.22146320976627604
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
1260.75,
|
||||
466.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1259.335205078125,
|
||||
466.6405944824219
|
||||
],
|
||||
"reprojection_error_px": 1.4217635103809503,
|
||||
"confidence": 0.19461123347835588
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
1285.5,
|
||||
508.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1284.366455078125,
|
||||
507.854248046875
|
||||
],
|
||||
"reprojection_error_px": 1.200643035340168,
|
||||
"confidence": 0.17943691614102802
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
1130.0,
|
||||
393.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1129.2742919921875,
|
||||
393.4766540527344
|
||||
],
|
||||
"reprojection_error_px": 0.7260834289920938,
|
||||
"confidence": 0.18215071600887703
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
1154.0,
|
||||
379.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1152.795654296875,
|
||||
379.095703125
|
||||
],
|
||||
"reprojection_error_px": 1.2081422353226527,
|
||||
"confidence": 0.17728829216377978
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_172504/cam2_debug.jpg
Normal file
BIN
data/homing/20260625_172504/cam2_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 353 KiB |
2487
data/homing/20260625_172504/robot_1781069752019.json
Normal file
2487
data/homing/20260625_172504/robot_1781069752019.json
Normal file
File diff suppressed because it is too large
Load Diff
59
data/homing/20260625_172504/robot_state.json
Normal file
59
data/homing/20260625_172504/robot_state.json
Normal file
@@ -0,0 +1,59 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:25:19Z",
|
||||
"method": "hybrid",
|
||||
"seeded": true,
|
||||
"movements": {
|
||||
"x": {
|
||||
"value": 162.42504894251783,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 4
|
||||
},
|
||||
"y": {
|
||||
"value": -1.169420311588133,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 4
|
||||
},
|
||||
"z": {
|
||||
"value": 98.46924561003269,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 1
|
||||
},
|
||||
"a": {
|
||||
"value": 89.51769789582495,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 4
|
||||
},
|
||||
"b": {
|
||||
"value": -47.89793601064412,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"c": {
|
||||
"value": -64.66160343747586,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"e": {
|
||||
"value": 15.8937800585978,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
}
|
||||
},
|
||||
"residual_rms": 22.202829667894964,
|
||||
"num_markers": 43
|
||||
}
|
||||
105
data/homing/20260625_172504/state_Arm1.json
Normal file
105
data/homing/20260625_172504/state_Arm1.json
Normal file
@@ -0,0 +1,105 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm1",
|
||||
"joint": "y",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
216.50906842923365,
|
||||
108.3968,
|
||||
46.3163
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 8.73396517409613,
|
||||
"circular_variance": 0.07161679045871494,
|
||||
"circular_std_deg": 22.088350114431325,
|
||||
"num_pairs_used": 6,
|
||||
"num_markers_matched": 4,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 2.7136316277566275,
|
||||
"baseline_model_mm": 120.70271952197267,
|
||||
"baseline_obs_mm": 235.58340824705388,
|
||||
"weight": 28435.558049674528
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 36.09512656392027,
|
||||
"baseline_model_mm": 63.35571402801804,
|
||||
"baseline_obs_mm": 159.67340571245236,
|
||||
"weight": 10116.222630197835
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 88.90480962311312,
|
||||
"baseline_model_mm": 34.32559540634365,
|
||||
"baseline_obs_mm": 110.45934908729735,
|
||||
"weight": 3791.582925618644
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -3.440668721283733,
|
||||
"baseline_model_mm": 89.99999999999997,
|
||||
"baseline_obs_mm": 88.29072557685865,
|
||||
"weight": 7946.165301917275
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -2.7040713039960176,
|
||||
"baseline_model_mm": 129.8075498574717,
|
||||
"baseline_obs_mm": 129.20056719980755,
|
||||
"weight": 16771.209068402644
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
229,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -0.7105184107327543,
|
||||
"baseline_model_mm": 49.49747468305837,
|
||||
"baseline_obs_mm": 49.40955380679017,
|
||||
"weight": 2445.6481386528067
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 106.50906842923364,
|
||||
"y": 8.73396517409613
|
||||
}
|
||||
}
|
||||
101
data/homing/20260625_172504/state_Arm2.json
Normal file
101
data/homing/20260625_172504/state_Arm2.json
Normal file
@@ -0,0 +1,101 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm2",
|
||||
"joint": "a",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
306.50906842923365,
|
||||
-138.70421127250364,
|
||||
84.27799422070137
|
||||
],
|
||||
"joint_axis_world": [
|
||||
0.0,
|
||||
0.13361549880725562,
|
||||
0.9910332479177922
|
||||
],
|
||||
"mean_angle_deg": 93.37084948672933,
|
||||
"circular_variance": 0.0007324693649812808,
|
||||
"circular_std_deg": 2.193370075603702,
|
||||
"num_pairs_used": 4,
|
||||
"num_markers_matched": 4,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
144
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 89.94688841244773,
|
||||
"baseline_model_mm": 26.68445427585132,
|
||||
"baseline_obs_mm": 24.643765187064083,
|
||||
"weight": 657.6054253190281
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
146
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": true,
|
||||
"reason": "bl_model=0.3 bl_obs=2.8 < 15.0"
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 93.80406057538599,
|
||||
"baseline_model_mm": 26.836171485515575,
|
||||
"baseline_obs_mm": 28.679452701596173,
|
||||
"weight": 769.6467108107678
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
146
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 92.82624404009415,
|
||||
"baseline_model_mm": 26.791733799812224,
|
||||
"baseline_obs_mm": 27.197454261300408,
|
||||
"weight": 728.6669546013292
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": true,
|
||||
"reason": "bl_model=0.4 bl_obs=4.3 < 15.0"
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
146,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 96.11666161271195,
|
||||
"baseline_model_mm": 26.946706663338293,
|
||||
"baseline_obs_mm": 31.282017166008377,
|
||||
"weight": 842.9473404099408
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 106.50906842923364,
|
||||
"y": 8.73396517409613,
|
||||
"z": 88.94460334069886,
|
||||
"a": 93.37084948672933
|
||||
}
|
||||
}
|
||||
54
data/homing/20260625_172504/state_Ellbow.json
Normal file
54
data/homing/20260625_172504/state_Ellbow.json
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Ellbow",
|
||||
"joint": "z",
|
||||
"method": "fallback_1_child_axis",
|
||||
"joint_origin_world_mm": [
|
||||
216.50906842923365,
|
||||
-138.70421127250364,
|
||||
84.27799422070137
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 88.94460334069886,
|
||||
"circular_variance": 5.079874864089007e-07,
|
||||
"circular_std_deg": 0.05775162282873076,
|
||||
"num_pairs_used": 2,
|
||||
"num_markers_matched": 1,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
146
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "fallback_1_child_axis",
|
||||
"skipped": false,
|
||||
"angle_deg": 89.03275897168004,
|
||||
"baseline_model_mm": 70.00000071428568,
|
||||
"baseline_obs_mm": 70.28215356251216,
|
||||
"weight": 4919.750799577387
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "fallback_1_child_axis",
|
||||
"skipped": false,
|
||||
"angle_deg": 88.90676969215383,
|
||||
"baseline_model_mm": 106.99999999999999,
|
||||
"baseline_obs_mm": 107.1349128404376,
|
||||
"weight": 11463.43567392682
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 106.50906842923364,
|
||||
"y": 8.73396517409613,
|
||||
"z": 88.94460334069886
|
||||
}
|
||||
}
|
||||
61
data/homing/20260625_175916/aruco_marker_poses.csv
Normal file
61
data/homing/20260625_175916/aruco_marker_poses.csv
Normal file
@@ -0,0 +1,61 @@
|
||||
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
|
||||
0,unknown,,3,505.79,-100.06,-8.17,0.00654,-0.01478,0.99987,,,,,,23.71
|
||||
46,Board,A0,2,537.9,185.52,-27.5,0.0061,0.00117,0.99998,537.44,185.2,-27.2,0.638,-0.305,23.83
|
||||
47,Board,A0,3,343.34,-286.18,-27.52,-0.00826,0.02432,0.99967,343.18,-286.05,-27.49,0.21,-0.034,24.1
|
||||
50,Board,A0,3,574.64,211.77,-27.34,0.00242,0.0109,0.99994,574.23,211.48,-27.15,0.539,-0.186,24.07
|
||||
51,Board,A0,3,166.89,-171.06,-27.49,-0.04142,0.00081,0.99914,167.18,-170.93,-27.76,0.417,0.272,24.2
|
||||
53,Board,A0,3,487.16,212.93,-27.58,0.06677,0.02171,0.99753,487.08,212.19,-27.28,0.798,-0.296,23.92
|
||||
54,Board,A0,3,341.31,-330.18,-27.55,-0.01014,0.01262,0.99987,341.05,-330.09,-27.5,0.275,-0.049,24.21
|
||||
55,Arm1,A0,3,282.71,-261.9,-26.63,-0.03413,0.00638,0.9994,,,,,,24.37
|
||||
56,Arm1,A0,2,500.02,169.42,-27.88,-0.01336,0.00216,0.99991,,,,,,23.7
|
||||
58,Board,A0,3,48.69,-216.76,-27.87,-0.0193,0.00188,0.99981,49.3,-216.52,-27.93,0.658,0.059,24.44
|
||||
60,Board,A0,2,435.69,286.11,-29.34,-0.04419,0.06927,0.99662,435.46,283.95,-27.36,2.937,-1.981,24.83
|
||||
62,Board,A0,3,404.22,-174.75,-27.14,0.00893,-0.00881,0.99992,404.07,-174.84,-27.4,0.313,0.259,23.96
|
||||
64,Board,A0,2,-22.33,-187.15,-26.56,0.02977,0.00062,0.99956,-21.95,-186.37,-28.04,1.719,1.484,23.66
|
||||
66,Board,A0,2,207.97,-362.78,-28.65,-0.08,0.0928,0.99247,208.41,-362.24,-27.7,1.18,-0.951,25.14
|
||||
67,Board,A0,2,524.61,268.02,-28.51,-0.0163,-0.00123,0.99987,524.1,266.85,-27.22,1.818,-1.29,23.6
|
||||
68,Board,A0,3,575.05,170.15,-27.14,0.01722,0.03205,0.99934,574.4,170.26,-27.15,0.663,0.006,24.1
|
||||
69,Board,A0,2,6.91,-280.99,-27.23,-0.00089,0.03112,0.99952,6.58,-279.46,-28,1.744,0.77,24.07
|
||||
70,Board,A0,2,603.56,300.57,-28.34,-0.06715,0.04327,0.9968,603.03,299.84,-27.11,1.527,-1.233,23.99
|
||||
73,Board,A0,2,222.01,337.1,-29.9,-0.0437,0.11256,0.99268,223.01,334.05,-27.67,3.914,-2.235,25.6
|
||||
75,Board,A0,2,-27.07,199.16,-28.78,-0.06027,0.14192,0.98804,-24.94,196.46,-28.04,3.523,-0.736,26.71
|
||||
76,Board,A0,2,686.17,164.68,-26.47,-0.08323,0.00998,0.99648,685.86,166,-26.98,1.45,0.514,23.85
|
||||
77,Arm1,A0,2,17.53,194.86,-28.25,-0.02333,0.14876,0.9886,,,,,,26.07
|
||||
79,Board,A0,2,311.53,-157.96,-26.95,0.02919,-0.01163,0.99951,311.73,-158.5,-27.54,0.827,0.59,24.03
|
||||
82,Board,A0,2,219.4,300.84,-30.04,0.00365,0.102,0.99478,220.31,298.19,-27.68,3.663,-2.363,25.25
|
||||
85,Board,A0,3,504.05,-313.36,-27.37,-0.00147,-0.03538,0.99937,503.43,-312.87,-27.25,0.799,-0.124,24.1
|
||||
88,Board,A0,2,767.2,313.41,-25.61,0.05089,-0.00066,0.9987,767.09,314.94,-26.86,1.978,1.252,23.68
|
||||
90,Board,A0,2,644.9,315.86,-27.76,-0.00476,-0.03483,0.99938,644.39,315.79,-27.04,0.882,-0.718,23.64
|
||||
91,Board,A0,2,725.06,326.79,-26.9,-0.04318,-0.00677,0.99904,724.61,327.11,-26.92,0.553,0.023,23.76
|
||||
92,Board,A0,2,644.97,-186.59,-26.03,0.03072,-0.01393,0.99943,644.42,-185.49,-27.04,1.592,1.007,24.18
|
||||
94,Board,A0,2,875.22,168.95,-22.69,0.07264,0.05485,0.99585,876.38,172.13,-26.7,5.244,4.006,24.44
|
||||
95,Board,A0,3,184.79,-273.16,-27.83,-0.00258,0.02727,0.99962,185.04,-272.99,-27.73,0.315,-0.097,24.35
|
||||
96,Board,A0,3,369.18,-185.71,-27.51,0.02262,0.01808,0.99958,369.1,-186.1,-27.46,0.4,-0.049,24.16
|
||||
97,Board,A0,2,303.36,-359.54,-26.35,-0.00502,0.02288,0.99973,303.02,-359.03,-27.55,1.344,1.197,24.46
|
||||
98,Board,A0,2,577.15,315.74,-29.24,-0.02274,0.04433,0.99876,576.48,314.67,-27.15,2.445,-2.094,24.33
|
||||
100,Board,A0,2,803.8,169.01,-24.93,0.01883,-0.01657,0.99969,803.92,171.12,-26.81,2.829,1.882,23.24
|
||||
103,Board,A0,3,104.64,-186.37,-27.12,-0.00058,0.0069,0.99998,105.03,-186.33,-27.85,0.825,0.726,24.45
|
||||
104,Board,A0,2,827.17,235.41,-24.77,0.08284,0.04199,0.99568,827.64,237.83,-26.77,3.174,1.996,24.77
|
||||
105,Board,A0,3,524.5,-266.88,-27.45,-0.00083,0.01085,0.99994,523.86,-266.44,-27.22,0.811,-0.234,24.19
|
||||
118,unknown,,3,323.07,-174.22,47.26,0.01395,-0.99423,0.10635,,,,,,24.41
|
||||
122,Ellbow,,3,360.07,-173.45,46.18,0.01114,-0.99429,0.10615,,,,,,24.51
|
||||
143,Arm2,,2,339.81,-138.06,229.48,-0.73183,-0.67663,0.08126,,,,,,24.18
|
||||
144,Arm2,,3,362.95,-156.86,158.05,-0.03961,-0.99309,0.11048,,,,,,24.37
|
||||
147,FingerA,,3,382.82,-143.67,226.6,0.42827,-0.9012,0.06646,,,,,,24.01
|
||||
148,Arm2,,3,366.84,-142.43,264.66,-0.04437,-0.99493,0.09027,,,,,,24.39
|
||||
178,FingerB,,2,287.31,-120.45,315.29,-0.66387,-0.73035,-0.1608,,,,,,22.88
|
||||
179,FingerB,,2,329.61,-140.32,313.07,-0.6954,-0.15177,0.70241,,,,,,26.39
|
||||
198,Arm1,,2,268.82,-53.79,84.52,0.01757,0.02587,0.99951,,,,,,24.56
|
||||
200,unknown,,2,199.61,-28.62,110.03,-0.06794,-0.01026,0.99764,,,,,,23.25
|
||||
204,unknown,,2,198.89,116.09,120.31,0.05577,0.03761,0.99774,,,,,,24.15
|
||||
208,Board,rail,3,626.32,-98.56,-6.78,0.00383,-0.01334,0.9999,631.01,-98.43,-7.71,4.781,0.93,23.72
|
||||
210,Board,rail,2,129.76,-7.26,-6.27,0.028,0.01002,0.99956,122.63,-13.98,-0.72,11.263,-5.553,23.35
|
||||
214,unknown,,3,532.15,-7.88,-7.33,-0.02081,0.05531,0.99825,,,,,,24.05
|
||||
217,Board,rail,2,730.24,-8.43,-5.0,-0.01833,-0.01881,0.99966,732.39,-23.88,7.39,19.917,-12.387,23.61
|
||||
229,Arm1,,3,271.15,-142.19,79.27,0.01827,-0.03009,0.99938,,,,,,24.04
|
||||
243,Arm1,,3,270.52,-175.76,42.77,0.0038,-0.99933,-0.03654,,,,,,24.16
|
||||
|
||||
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
|
||||
cam0,335.07,-885.3,469.64,-0.06533,0.89189,-0.44751
|
||||
cam1,297.15,-462.72,780.65,0.09465,0.43305,-0.89639
|
||||
cam2,714.94,-666.29,727.22,-0.42921,0.59338,-0.68094
|
||||
|
2542
data/homing/20260625_175916/aruco_marker_poses.json
Normal file
2542
data/homing/20260625_175916/aruco_marker_poses.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
data/homing/20260625_175916/cam0.jpg
Normal file
BIN
data/homing/20260625_175916/cam0.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 103 KiB |
2628
data/homing/20260625_175916/cam0_aruco_detection.json
Normal file
2628
data/homing/20260625_175916/cam0_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
481
data/homing/20260625_175916/cam0_camera_pose.json
Normal file
481
data/homing/20260625_175916/cam0_camera_pose.json
Normal file
@@ -0,0 +1,481 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:21Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_175916/cam0_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam0",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1424.7584228515625,
|
||||
0.0,
|
||||
635.95947265625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1421.5770263671875,
|
||||
482.1744384765625
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.05634751915931702,
|
||||
0.33765655755996704,
|
||||
0.002130246954038739,
|
||||
-0.004022662527859211,
|
||||
-1.182201862335205
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 20,
|
||||
"used_marker_ids": [
|
||||
97,
|
||||
66,
|
||||
85,
|
||||
54,
|
||||
105,
|
||||
69,
|
||||
47,
|
||||
95,
|
||||
58,
|
||||
64,
|
||||
103,
|
||||
96,
|
||||
62,
|
||||
51,
|
||||
79,
|
||||
208,
|
||||
210,
|
||||
68,
|
||||
50,
|
||||
53
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011436738437523918,
|
||||
0.0017627388219078695,
|
||||
0.0017082741597643208,
|
||||
0.0017082738298200922
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 3.4895581122703203,
|
||||
"residual_median_px": 1.3632084656881505,
|
||||
"residual_max_px": 12.179917236270976,
|
||||
"sigma2_normalized": 3.433175856049273e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.99742591381073,
|
||||
0.04507605358958244,
|
||||
-0.055764634162187576
|
||||
],
|
||||
[
|
||||
-0.029563840478658676,
|
||||
-0.4500020146369934,
|
||||
-0.8925380110740662
|
||||
],
|
||||
[
|
||||
-0.06532628834247589,
|
||||
0.891889214515686,
|
||||
-0.44751107692718506
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.26810967922210693,
|
||||
0.0306912399828434,
|
||||
1.021647572517395
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.035801112416453,
|
||||
0.010908616696792009,
|
||||
-0.08515448496251522
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.33506736159324646,
|
||||
-0.8853000402450562,
|
||||
0.46964067220687866
|
||||
],
|
||||
"position_mm": [
|
||||
335.0673522949219,
|
||||
-885.300048828125,
|
||||
469.64068603515625
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 116.64550018310547,
|
||||
"pitch": 3.745587110519409,
|
||||
"yaw": -1.6977574825286865
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
1.0402983243682025e-05,
|
||||
9.607061849723336e-07,
|
||||
3.175419172829312e-06,
|
||||
-1.413862159845341e-07,
|
||||
-2.12955432250041e-06,
|
||||
2.312142235999549e-07
|
||||
],
|
||||
[
|
||||
9.607061849723307e-07,
|
||||
3.7788686477185887e-06,
|
||||
-1.6924039364567714e-07,
|
||||
5.562539816390373e-07,
|
||||
-8.841983839385075e-07,
|
||||
6.874403655011122e-07
|
||||
],
|
||||
[
|
||||
3.175419172829249e-06,
|
||||
-1.692403936456681e-07,
|
||||
1.3583347408234157e-05,
|
||||
-9.175016077988046e-07,
|
||||
-2.2808655354386924e-06,
|
||||
-3.2773776598692575e-06
|
||||
],
|
||||
[
|
||||
-1.4138621598453382e-07,
|
||||
5.562539816390341e-07,
|
||||
-9.175016077987983e-07,
|
||||
2.6164306620048117e-07,
|
||||
4.0225581275842607e-08,
|
||||
3.496020393352873e-07
|
||||
],
|
||||
[
|
||||
-2.1295543225004017e-06,
|
||||
-8.841983839385094e-07,
|
||||
-2.280865535438699e-06,
|
||||
4.02255812758387e-08,
|
||||
9.903676474491735e-07,
|
||||
6.66954432381659e-07
|
||||
],
|
||||
[
|
||||
2.3121422359997266e-07,
|
||||
6.874403655011114e-07,
|
||||
-3.277377659869258e-06,
|
||||
3.4960203933526876e-07,
|
||||
6.669544323816537e-07,
|
||||
3.1141328985088255e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.18479983617802564,
|
||||
0.11137905544845887,
|
||||
0.2111669934581791
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0005115105729117251,
|
||||
0.0009951721697521356,
|
||||
0.0017646905956877612
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0026703652741227927,
|
||||
0.0018374002535456223,
|
||||
0.0027539784841622054
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.670365274122793,
|
||||
1.8374002535456224,
|
||||
2.7539784841622055
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.1722531066068174,
|
||||
"pitch": 0.1564002711188888,
|
||||
"yaw": 0.12201801252145385
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
676.0,
|
||||
910.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
675.7621459960938,
|
||||
911.7245483398438
|
||||
],
|
||||
"reprojection_error_px": 1.247434633072338,
|
||||
"confidence": 0.42385670146087856
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
479.5,
|
||||
922.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
480.50054931640625,
|
||||
919.6788330078125
|
||||
],
|
||||
"reprojection_error_px": 2.7589850735869397,
|
||||
"confidence": 0.27078087716396265
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
1079.0,
|
||||
843.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1076.984619140625,
|
||||
842.904296875
|
||||
],
|
||||
"reprojection_error_px": 2.1856289218368885,
|
||||
"confidence": 0.6759853525830334
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
753.5,
|
||||
868.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
752.8966064453125,
|
||||
868.8074340820312
|
||||
],
|
||||
"reprojection_error_px": 0.606120825922678,
|
||||
"confidence": 0.6610788202218779
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1098.25,
|
||||
784.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1095.7879638671875,
|
||||
782.9950561523438
|
||||
],
|
||||
"reprojection_error_px": 2.6592356150248286,
|
||||
"confidence": 0.5825558453258135
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
130.5,
|
||||
818.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
131.20993041992188,
|
||||
816.9083862304688
|
||||
],
|
||||
"reprojection_error_px": 1.302160444400257,
|
||||
"confidence": 0.5841161997457144
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
755.0,
|
||||
810.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
754.5029907226562,
|
||||
810.5115966796875
|
||||
],
|
||||
"reprojection_error_px": 0.4971445511574386,
|
||||
"confidence": 0.5487375886524822
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
461.0,
|
||||
800.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
461.86224365234375,
|
||||
800.0243530273438
|
||||
],
|
||||
"reprojection_error_px": 0.8912803555986298,
|
||||
"confidence": 0.5215660558894334
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
243.25,
|
||||
743.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
244.70860290527344,
|
||||
742.6357421875
|
||||
],
|
||||
"reprojection_error_px": 1.5033982137941357,
|
||||
"confidence": 0.468912299986827
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
139.5,
|
||||
714.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
141.43846130371094,
|
||||
715.3218383789062
|
||||
],
|
||||
"reprojection_error_px": 2.346249970897008,
|
||||
"confidence": 0.4334442420959473
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
350.5,
|
||||
708.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.01885986328125,
|
||||
708.9456787109375
|
||||
],
|
||||
"reprojection_error_px": 1.78920191946218,
|
||||
"confidence": 0.3938267188343575
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
793.0,
|
||||
697.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
793.4209594726562,
|
||||
698.4274291992188
|
||||
],
|
||||
"reprojection_error_px": 0.7975695565737103,
|
||||
"confidence": 0.36075364145684324
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
851.25,
|
||||
685.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
851.3250732421875,
|
||||
686.2469482421875
|
||||
],
|
||||
"reprojection_error_px": 0.7507114413671468,
|
||||
"confidence": 0.34135471017020086
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
457.75,
|
||||
690.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
459.0975646972656,
|
||||
691.2110595703125
|
||||
],
|
||||
"reprojection_error_px": 1.4242564869760441,
|
||||
"confidence": 0.3676955439548635
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
695.75,
|
||||
672.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
696.7139892578125,
|
||||
673.8245849609375
|
||||
],
|
||||
"reprojection_error_px": 2.0635856097717857,
|
||||
"confidence": 0.3187317073170732
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
1201.75,
|
||||
583.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1208.861572265625,
|
||||
583.9735107421875
|
||||
],
|
||||
"reprojection_error_px": 7.177895461370356,
|
||||
"confidence": 0.28568227626765064
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
439.0,
|
||||
532.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.0049133300781,
|
||||
530.6361694335938
|
||||
],
|
||||
"reprojection_error_px": 12.179917236270976,
|
||||
"confidence": 0.16251118951659882
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
1027.25,
|
||||
434.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1026.4912109375,
|
||||
434.3568420410156
|
||||
],
|
||||
"reprojection_error_px": 0.7662741435661348,
|
||||
"confidence": 0.09654486228169104
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
1016.5,
|
||||
413.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1016.3207397460938,
|
||||
413.5331726074219
|
||||
],
|
||||
"reprojection_error_px": 0.18230375891269784,
|
||||
"confidence": 0.08377973241313476
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
909.0,
|
||||
416.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
909.6261596679688,
|
||||
416.78497314453125
|
||||
],
|
||||
"reprojection_error_px": 0.6879575734700244,
|
||||
"confidence": 0.07916194626380657
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_175916/cam0_debug.jpg
Normal file
BIN
data/homing/20260625_175916/cam0_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 223 KiB |
BIN
data/homing/20260625_175916/cam1.jpg
Normal file
BIN
data/homing/20260625_175916/cam1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 110 KiB |
3558
data/homing/20260625_175916/cam1_aruco_detection.json
Normal file
3558
data/homing/20260625_175916/cam1_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
761
data/homing/20260625_175916/cam1_camera_pose.json
Normal file
761
data/homing/20260625_175916/cam1_camera_pose.json
Normal file
@@ -0,0 +1,761 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:25Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_175916/cam1_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam1",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1367.5723876953125,
|
||||
0.0,
|
||||
672.1165771484375
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1372.3011474609375,
|
||||
445.8396911621094
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.01016925647854805,
|
||||
0.7656787633895874,
|
||||
-0.0031530377455055714,
|
||||
-0.00288817984983325,
|
||||
-2.490830183029175
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 40,
|
||||
"used_marker_ids": [
|
||||
54,
|
||||
95,
|
||||
58,
|
||||
85,
|
||||
47,
|
||||
103,
|
||||
59,
|
||||
105,
|
||||
48,
|
||||
51,
|
||||
102,
|
||||
96,
|
||||
62,
|
||||
71,
|
||||
92,
|
||||
208,
|
||||
63,
|
||||
210,
|
||||
217,
|
||||
74,
|
||||
75,
|
||||
52,
|
||||
76,
|
||||
68,
|
||||
46,
|
||||
53,
|
||||
50,
|
||||
101,
|
||||
82,
|
||||
100,
|
||||
73,
|
||||
60,
|
||||
67,
|
||||
70,
|
||||
94,
|
||||
90,
|
||||
104,
|
||||
98,
|
||||
91,
|
||||
88
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.011863062568042395,
|
||||
0.0021882720514259653,
|
||||
0.0021293863444762583,
|
||||
0.0021293859894700134
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 4.205255463207004,
|
||||
"residual_median_px": 1.1858116613812175,
|
||||
"residual_max_px": 19.118438507028245,
|
||||
"sigma2_normalized": 4.9019293968833265e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.995226263999939,
|
||||
0.019625136628746986,
|
||||
-0.09560120105743408
|
||||
],
|
||||
[
|
||||
-0.023808155208826065,
|
||||
0.9011572599411011,
|
||||
0.4328380525112152
|
||||
],
|
||||
[
|
||||
0.09464621543884277,
|
||||
0.4330478608608246,
|
||||
-0.8963881134986877
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
0.37944144010543823,
|
||||
0.08616430312395096,
|
||||
0.8720222115516663
|
||||
],
|
||||
"rvec_rad": [
|
||||
0.0032728478486759383,
|
||||
-2.9675124187665194,
|
||||
-0.6774800690352322
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.2971478998661041,
|
||||
-0.4627215266227722,
|
||||
0.7806501984596252
|
||||
],
|
||||
"position_mm": [
|
||||
297.14788818359375,
|
||||
-462.7215270996094,
|
||||
780.6502075195312
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 154.2146453857422,
|
||||
"pitch": -5.430957317352295,
|
||||
"yaw": -178.6295928955078
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
2.216368778099703e-06,
|
||||
-1.0420377127470311e-07,
|
||||
-5.846035681663414e-07,
|
||||
9.638753004284797e-09,
|
||||
6.568277931365646e-07,
|
||||
1.8540529905122184e-07
|
||||
],
|
||||
[
|
||||
-1.0420377127470325e-07,
|
||||
7.716359917594955e-06,
|
||||
-8.780258686751835e-07,
|
||||
-8.798584488389978e-07,
|
||||
8.337615617564991e-07,
|
||||
-3.831747509912712e-06
|
||||
],
|
||||
[
|
||||
-5.846035681663423e-07,
|
||||
-8.780258686751325e-07,
|
||||
1.8740126796208265e-05,
|
||||
-6.332974421687337e-07,
|
||||
-7.826466185078999e-07,
|
||||
3.040527671220899e-07
|
||||
],
|
||||
[
|
||||
9.638753004284528e-09,
|
||||
-8.798584488389991e-07,
|
||||
-6.332974421687222e-07,
|
||||
2.409326607990254e-07,
|
||||
-7.666433629844329e-08,
|
||||
3.751828231192777e-07
|
||||
],
|
||||
[
|
||||
6.568277931365642e-07,
|
||||
8.337615617564963e-07,
|
||||
-7.826466185079048e-07,
|
||||
-7.66643362984426e-08,
|
||||
4.122731304646649e-07,
|
||||
-3.5672545335086356e-07
|
||||
],
|
||||
[
|
||||
1.8540529905122184e-07,
|
||||
-3.831747509912709e-06,
|
||||
3.040527671221047e-07,
|
||||
3.7518282311927725e-07,
|
||||
-3.567254533508643e-07,
|
||||
2.7716081204320043e-06
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.08529894195541628,
|
||||
0.15915814568050488,
|
||||
0.24803267374807242
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0004908489185065252,
|
||||
0.0006420849869485074,
|
||||
0.0016648147405738587
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.0027539612430056413,
|
||||
0.002145451671454086,
|
||||
0.0019549734255131183
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2.7539612430056413,
|
||||
2.145451671454086,
|
||||
1.9549734255131184
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.19816528973825537,
|
||||
"pitch": 0.158142424860459,
|
||||
"yaw": 0.04543296464811841
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
735.25,
|
||||
38.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
735.1257934570312,
|
||||
38.84978103637695
|
||||
],
|
||||
"reprojection_error_px": 0.6125067811164705,
|
||||
"confidence": 0.29867757253933086
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
1005.25,
|
||||
139.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1004.7445678710938,
|
||||
139.50558471679688
|
||||
],
|
||||
"reprojection_error_px": 0.714896875632619,
|
||||
"confidence": 0.9333333333333333
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
1234.75,
|
||||
235.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1234.0479736328125,
|
||||
235.1490020751953
|
||||
],
|
||||
"reprojection_error_px": 0.7092542569779415,
|
||||
"confidence": 0.36770453019575644
|
||||
},
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
458.0,
|
||||
68.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
458.30291748046875,
|
||||
69.59838104248047
|
||||
],
|
||||
"reprojection_error_px": 1.6268315699341729,
|
||||
"confidence": 0.875342401743355
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
731.75,
|
||||
117.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
731.4143676757812,
|
||||
117.63695526123047
|
||||
],
|
||||
"reprojection_error_px": 0.7199729591239207,
|
||||
"confidence": 0.9759139241315488
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
1128.75,
|
||||
284.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1128.21533203125,
|
||||
284.2250061035156
|
||||
],
|
||||
"reprojection_error_px": 0.6012416152520633,
|
||||
"confidence": 0.9144001450649527
|
||||
},
|
||||
{
|
||||
"marker_id": 59,
|
||||
"observed_center_px": [
|
||||
262.0,
|
||||
123.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
263.8606872558594,
|
||||
125.23478698730469
|
||||
],
|
||||
"reprojection_error_px": 2.5439424041120047,
|
||||
"confidence": 0.9428090418229809
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
432.25,
|
||||
147.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
432.2873840332031,
|
||||
149.19923400878906
|
||||
],
|
||||
"reprojection_error_px": 1.449716102610904,
|
||||
"confidence": 0.9442241264421224
|
||||
},
|
||||
{
|
||||
"marker_id": 48,
|
||||
"observed_center_px": [
|
||||
148.5,
|
||||
50.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
149.40765380859375,
|
||||
51.741458892822266
|
||||
],
|
||||
"reprojection_error_px": 1.3441823434382303,
|
||||
"confidence": 0.5647167996853729
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
1019.5,
|
||||
307.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1019.1678466796875,
|
||||
307.8690490722656
|
||||
],
|
||||
"reprojection_error_px": 0.49651087191997795,
|
||||
"confidence": 0.8824030145295596
|
||||
},
|
||||
{
|
||||
"marker_id": 102,
|
||||
"observed_center_px": [
|
||||
239.5,
|
||||
213.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
240.8688507080078,
|
||||
215.52737426757812
|
||||
],
|
||||
"reprojection_error_px": 2.24339286525173,
|
||||
"confidence": 0.9331395642876747
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
690.0,
|
||||
280.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
689.8768310546875,
|
||||
280.7818908691406
|
||||
],
|
||||
"reprojection_error_px": 0.12723056481812098,
|
||||
"confidence": 0.9012948266053811
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
634.75,
|
||||
296.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
634.6005249023438,
|
||||
297.2737731933594
|
||||
],
|
||||
"reprojection_error_px": 0.5446844617769289,
|
||||
"confidence": 0.8643569912878776
|
||||
},
|
||||
{
|
||||
"marker_id": 71,
|
||||
"observed_center_px": [
|
||||
63.25,
|
||||
113.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
63.99217224121094,
|
||||
114.01245880126953
|
||||
],
|
||||
"reprojection_error_px": 0.7872129686354927,
|
||||
"confidence": 0.6618010073234457
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
256.25,
|
||||
273.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
257.8680114746094,
|
||||
274.7709045410156
|
||||
],
|
||||
"reprojection_error_px": 2.220610671605798,
|
||||
"confidence": 0.8785732433921414
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
295.25,
|
||||
413.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
288.319091796875,
|
||||
413.3032531738281
|
||||
],
|
||||
"reprojection_error_px": 6.945291300358831,
|
||||
"confidence": 0.7372980849807326
|
||||
},
|
||||
{
|
||||
"marker_id": 63,
|
||||
"observed_center_px": [
|
||||
36.0,
|
||||
190.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
37.349727630615234,
|
||||
191.05543518066406
|
||||
],
|
||||
"reprojection_error_px": 1.3838552404184128,
|
||||
"confidence": 0.23122213258874602
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
1059.0,
|
||||
560.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.5731201171875,
|
||||
555.7078857421875
|
||||
],
|
||||
"reprojection_error_px": 15.192039848292012,
|
||||
"confidence": 0.6479471317434429
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
162.0,
|
||||
532.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
146.59190368652344,
|
||||
520.931640625
|
||||
],
|
||||
"reprojection_error_px": 19.118438507028245,
|
||||
"confidence": 0.6089399477645175
|
||||
},
|
||||
{
|
||||
"marker_id": 74,
|
||||
"observed_center_px": [
|
||||
1085.75,
|
||||
734.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1084.8079833984375,
|
||||
734.9703979492188
|
||||
],
|
||||
"reprojection_error_px": 0.9674557011249624,
|
||||
"confidence": 0.4941465175073702
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
1251.5,
|
||||
801.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1249.0706787109375,
|
||||
800.055419921875
|
||||
],
|
||||
"reprojection_error_px": 2.8263781642957686,
|
||||
"confidence": 0.07129070123540064
|
||||
},
|
||||
{
|
||||
"marker_id": 52,
|
||||
"observed_center_px": [
|
||||
1075.0,
|
||||
807.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1073.82666015625,
|
||||
807.2386474609375
|
||||
],
|
||||
"reprojection_error_px": 1.1973633531819778,
|
||||
"confidence": 0.4747611026689374
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
273.75,
|
||||
722.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
274.6397705078125,
|
||||
723.6699829101562
|
||||
],
|
||||
"reprojection_error_px": 1.4698814804706541,
|
||||
"confidence": 0.4464638052540792
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
421.5,
|
||||
733.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
422.1742858886719,
|
||||
733.5136108398438
|
||||
],
|
||||
"reprojection_error_px": 0.847618755377099,
|
||||
"confidence": 0.453656743367513
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
472.25,
|
||||
751.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
472.6407165527344,
|
||||
751.65087890625
|
||||
],
|
||||
"reprojection_error_px": 0.41883632713964764,
|
||||
"confidence": 0.4433609532391809
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
541.25,
|
||||
783.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
541.2369384765625,
|
||||
783.3223876953125
|
||||
],
|
||||
"reprojection_error_px": 0.07355665725931064,
|
||||
"confidence": 0.41918904165916515
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
427.0,
|
||||
777.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
427.4671630859375,
|
||||
777.926025390625
|
||||
],
|
||||
"reprojection_error_px": 0.49922568744740276,
|
||||
"confidence": 0.4069334148740479
|
||||
},
|
||||
{
|
||||
"marker_id": 101,
|
||||
"observed_center_px": [
|
||||
1031.0,
|
||||
900.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1029.668212890625,
|
||||
900.4525146484375
|
||||
],
|
||||
"reprojection_error_px": 1.3470965397955643,
|
||||
"confidence": 0.34672467375183735
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
894.5,
|
||||
890.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
893.2540283203125,
|
||||
890.4962158203125
|
||||
],
|
||||
"reprojection_error_px": 1.2700660048814185,
|
||||
"confidence": 0.39232527116476396
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
120.0,
|
||||
723.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
121.81710815429688,
|
||||
723.7096557617188
|
||||
],
|
||||
"reprojection_error_px": 1.8291630826238716,
|
||||
"confidence": 0.4207088246724057
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
889.0,
|
||||
926.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
887.5708618164062,
|
||||
926.2489624023438
|
||||
],
|
||||
"reprojection_error_px": 1.4291385602573807,
|
||||
"confidence": 0.1469791980355697
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
613.0,
|
||||
860.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
612.533935546875,
|
||||
860.8355102539062
|
||||
],
|
||||
"reprojection_error_px": 0.5742675377756796,
|
||||
"confidence": 0.36564926628565203
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
497.75,
|
||||
838.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
497.8586120605469,
|
||||
838.2026977539062
|
||||
],
|
||||
"reprojection_error_px": 0.22996295165716743,
|
||||
"confidence": 0.363884422773956
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
401.25,
|
||||
866.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
401.48388671875,
|
||||
867.0733032226562
|
||||
],
|
||||
"reprojection_error_px": 0.39903379679866513,
|
||||
"confidence": 0.35896437880813437
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
29.0,
|
||||
720.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
31.11460304260254,
|
||||
720.2709350585938
|
||||
],
|
||||
"reprojection_error_px": 2.1147066710213593,
|
||||
"confidence": 0.09031730482356817
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
351.5,
|
||||
880.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
352.0174560546875,
|
||||
880.55810546875
|
||||
],
|
||||
"reprojection_error_px": 0.6022372857988869,
|
||||
"confidence": 0.36026641726859315
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
107.5,
|
||||
792.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
109.0373764038086,
|
||||
792.3465576171875
|
||||
],
|
||||
"reprojection_error_px": 1.540405654502856,
|
||||
"confidence": 0.40599116793083023
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
436.5,
|
||||
882.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
436.4425048828125,
|
||||
883.1728515625
|
||||
],
|
||||
"reprojection_error_px": 1.174259969580457,
|
||||
"confidence": 0.35391110957760685
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
254.25,
|
||||
887.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
255.05325317382812,
|
||||
887.015869140625
|
||||
],
|
||||
"reprojection_error_px": 0.8034099146071898,
|
||||
"confidence": 0.3494656541641164
|
||||
},
|
||||
{
|
||||
"marker_id": 88,
|
||||
"observed_center_px": [
|
||||
199.5,
|
||||
873.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
200.95053100585938,
|
||||
872.6802978515625
|
||||
],
|
||||
"reprojection_error_px": 1.4853449642002237,
|
||||
"confidence": 0.33862598740745903
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_175916/cam1_debug.jpg
Normal file
BIN
data/homing/20260625_175916/cam1_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 260 KiB |
BIN
data/homing/20260625_175916/cam2.jpg
Normal file
BIN
data/homing/20260625_175916/cam2.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 282 KiB |
3092
data/homing/20260625_175916/cam2_aruco_detection.json
Normal file
3092
data/homing/20260625_175916/cam2_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
761
data/homing/20260625_175916/cam2_camera_pose.json
Normal file
761
data/homing/20260625_175916/cam2_camera_pose.json
Normal file
@@ -0,0 +1,761 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:28Z",
|
||||
"source": {
|
||||
"detection_json": "/app/data/homing/20260625_175916/cam2_aruco_detection.json",
|
||||
"robot_json": "/app/scripts/robot_1781069752019.json"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam2",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1388.99072265625,
|
||||
0.0,
|
||||
933.082763671875
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1394.8729248046875,
|
||||
562.4996948242188
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.019531700760126114,
|
||||
-0.11213663965463638,
|
||||
0.0026758278254419565,
|
||||
0.0007694826927036047,
|
||||
0.05339815095067024
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 40,
|
||||
"used_marker_ids": [
|
||||
85,
|
||||
92,
|
||||
105,
|
||||
97,
|
||||
54,
|
||||
208,
|
||||
217,
|
||||
47,
|
||||
93,
|
||||
66,
|
||||
62,
|
||||
96,
|
||||
94,
|
||||
95,
|
||||
79,
|
||||
100,
|
||||
104,
|
||||
76,
|
||||
69,
|
||||
103,
|
||||
51,
|
||||
58,
|
||||
68,
|
||||
50,
|
||||
88,
|
||||
46,
|
||||
91,
|
||||
64,
|
||||
90,
|
||||
53,
|
||||
72,
|
||||
98,
|
||||
67,
|
||||
70,
|
||||
60,
|
||||
84,
|
||||
86,
|
||||
82,
|
||||
75,
|
||||
73
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"rms": [
|
||||
0.00929587415047541,
|
||||
0.0010748985788974661,
|
||||
0.001013950655392918,
|
||||
0.001013950511849939
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 1.9984633198313304,
|
||||
"residual_median_px": 1.1048397349885588,
|
||||
"residual_max_px": 8.012141964697209,
|
||||
"sigma2_normalized": 1.1114547464625753e-06
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.7610290050506592,
|
||||
0.6436193585395813,
|
||||
0.08117286115884781
|
||||
],
|
||||
[
|
||||
0.4864314794540405,
|
||||
-0.4833734929561615,
|
||||
-0.7278286218643188
|
||||
],
|
||||
[
|
||||
-0.42920777201652527,
|
||||
0.5933837294578552,
|
||||
-0.6809379458427429
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-0.17428335547447205,
|
||||
-0.14054450392723083,
|
||||
1.1974174976348877
|
||||
],
|
||||
"rvec_rad": [
|
||||
2.177352630181243,
|
||||
0.841105236753405,
|
||||
-0.2590449733390141
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
0.7149407863616943,
|
||||
-0.6662914156913757,
|
||||
0.7272217273712158
|
||||
],
|
||||
"position_mm": [
|
||||
714.9407958984375,
|
||||
-666.2914428710938,
|
||||
727.2217407226562
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 138.93040466308594,
|
||||
"pitch": 25.41729164123535,
|
||||
"yaw": 32.58573913574219
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
1.7325268790245518e-06,
|
||||
5.981655510564799e-07,
|
||||
1.9008081078750225e-07,
|
||||
6.084320732905797e-08,
|
||||
-2.3354702118446124e-07,
|
||||
-3.602729870649897e-08
|
||||
],
|
||||
[
|
||||
5.981655510564792e-07,
|
||||
7.185403999584639e-07,
|
||||
-1.733161988537022e-07,
|
||||
1.3590794895526218e-07,
|
||||
-1.804162779559688e-07,
|
||||
1.5814835072347804e-07
|
||||
],
|
||||
[
|
||||
1.9008081078749777e-07,
|
||||
-1.733161988537059e-07,
|
||||
2.5200605751354745e-06,
|
||||
-7.330065793590657e-08,
|
||||
-3.490611313350714e-07,
|
||||
-1.0443589786543664e-06
|
||||
],
|
||||
[
|
||||
6.084320732905791e-08,
|
||||
1.3590794895526253e-07,
|
||||
-7.330065793590603e-08,
|
||||
6.430560370646186e-08,
|
||||
-2.0737599537247602e-08,
|
||||
9.272757844523572e-08
|
||||
],
|
||||
[
|
||||
-2.3354702118446045e-07,
|
||||
-1.8041627795596826e-07,
|
||||
-3.490611313350726e-07,
|
||||
-2.073759953724742e-08,
|
||||
1.4131779018249e-07,
|
||||
1.552658806009075e-07
|
||||
],
|
||||
[
|
||||
-3.6027298706497375e-08,
|
||||
1.5814835072348006e-07,
|
||||
-1.0443589786543662e-06,
|
||||
9.272757844523604e-08,
|
||||
1.552658806009069e-07,
|
||||
7.158822480370525e-07
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.07541584873239728,
|
||||
0.048567777257391795,
|
||||
0.09095532342017693
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.0002535854958519155,
|
||||
0.0003759225853583288,
|
||||
0.0008460982496359702
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
0.001238859522904557,
|
||||
0.0010725601282025278,
|
||||
0.0013428043149285806
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
1.2388595229045571,
|
||||
1.0725601282025277,
|
||||
1.3428043149285807
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.09670418188338105,
|
||||
"pitch": 0.07031307438362219,
|
||||
"yaw": 0.04214847006011442
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 85,
|
||||
"observed_center_px": [
|
||||
943.0,
|
||||
1037.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
942.2024536132812,
|
||||
1035.979736328125
|
||||
],
|
||||
"reprojection_error_px": 1.7167649434302272,
|
||||
"confidence": 0.28339330788046485
|
||||
},
|
||||
{
|
||||
"marker_id": 92,
|
||||
"observed_center_px": [
|
||||
1260.5,
|
||||
1040.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1260.0,
|
||||
1039.166259765625
|
||||
],
|
||||
"reprojection_error_px": 1.1935212170729015,
|
||||
"confidence": 0.186820630468236
|
||||
},
|
||||
{
|
||||
"marker_id": 105,
|
||||
"observed_center_px": [
|
||||
1018.75,
|
||||
1005.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1017.9473266601562,
|
||||
1004.2492065429688
|
||||
],
|
||||
"reprojection_error_px": 1.48619270710366,
|
||||
"confidence": 0.868630415221559
|
||||
},
|
||||
{
|
||||
"marker_id": 97,
|
||||
"observed_center_px": [
|
||||
650.75,
|
||||
884.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
651.09619140625,
|
||||
883.6798095703125
|
||||
],
|
||||
"reprojection_error_px": 0.47156166195399984,
|
||||
"confidence": 0.7472812484733762
|
||||
},
|
||||
{
|
||||
"marker_id": 54,
|
||||
"observed_center_px": [
|
||||
727.5,
|
||||
891.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
727.0736083984375,
|
||||
890.49853515625
|
||||
],
|
||||
"reprojection_error_px": 1.0884583736414755,
|
||||
"confidence": 0.7927270034536174
|
||||
},
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
1312.75,
|
||||
909.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1318.954833984375,
|
||||
914.5689697265625
|
||||
],
|
||||
"reprojection_error_px": 8.012141964697209,
|
||||
"confidence": 0.5865958415608975
|
||||
},
|
||||
{
|
||||
"marker_id": 217,
|
||||
"observed_center_px": [
|
||||
1525.0,
|
||||
915.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1526.02783203125,
|
||||
921.9722290039062
|
||||
],
|
||||
"reprojection_error_px": 6.553334034174421,
|
||||
"confidence": 0.48103075735306616
|
||||
},
|
||||
{
|
||||
"marker_id": 47,
|
||||
"observed_center_px": [
|
||||
779.5,
|
||||
850.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
779.2447509765625,
|
||||
849.6724243164062
|
||||
],
|
||||
"reprojection_error_px": 0.8660447887040601,
|
||||
"confidence": 0.7324762772043267
|
||||
},
|
||||
{
|
||||
"marker_id": 93,
|
||||
"observed_center_px": [
|
||||
1890.0,
|
||||
969.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1890.317138671875,
|
||||
971.253173828125
|
||||
],
|
||||
"reprojection_error_px": 1.5362644612040617,
|
||||
"confidence": 0.0615171510658591
|
||||
},
|
||||
{
|
||||
"marker_id": 66,
|
||||
"observed_center_px": [
|
||||
549.25,
|
||||
803.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
550.2374877929688,
|
||||
801.9854125976562
|
||||
],
|
||||
"reprojection_error_px": 1.6044667149644478,
|
||||
"confidence": 0.526209880769093
|
||||
},
|
||||
{
|
||||
"marker_id": 62,
|
||||
"observed_center_px": [
|
||||
961.5,
|
||||
801.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
960.479248046875,
|
||||
801.3424072265625
|
||||
],
|
||||
"reprojection_error_px": 1.0328455993265524,
|
||||
"confidence": 0.5777637144454008
|
||||
},
|
||||
{
|
||||
"marker_id": 96,
|
||||
"observed_center_px": [
|
||||
911.5,
|
||||
782.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
910.5089721679688,
|
||||
782.1806640625
|
||||
],
|
||||
"reprojection_error_px": 1.0412068021481435,
|
||||
"confidence": 0.5986996695409863
|
||||
},
|
||||
{
|
||||
"marker_id": 94,
|
||||
"observed_center_px": [
|
||||
1813.75,
|
||||
889.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1813.9437255859375,
|
||||
890.62109375
|
||||
],
|
||||
"reprojection_error_px": 1.1377085738166388,
|
||||
"confidence": 0.321218994735393
|
||||
},
|
||||
{
|
||||
"marker_id": 95,
|
||||
"observed_center_px": [
|
||||
631.25,
|
||||
708.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
631.6299438476562,
|
||||
708.2338256835938
|
||||
],
|
||||
"reprojection_error_px": 0.640931550393094,
|
||||
"confidence": 0.5102620497459525
|
||||
},
|
||||
{
|
||||
"marker_id": 79,
|
||||
"observed_center_px": [
|
||||
875.5,
|
||||
714.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
875.01416015625,
|
||||
714.7401733398438
|
||||
],
|
||||
"reprojection_error_px": 0.5419627173032531,
|
||||
"confidence": 0.5178115536596077
|
||||
},
|
||||
{
|
||||
"marker_id": 100,
|
||||
"observed_center_px": [
|
||||
1710.25,
|
||||
832.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1710.6463623046875,
|
||||
831.8109130859375
|
||||
],
|
||||
"reprojection_error_px": 0.7949489617009169,
|
||||
"confidence": 0.31927346015206026
|
||||
},
|
||||
{
|
||||
"marker_id": 104,
|
||||
"observed_center_px": [
|
||||
1769.75,
|
||||
795.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1770.6925048828125,
|
||||
794.8803100585938
|
||||
],
|
||||
"reprojection_error_px": 1.1279765412478604,
|
||||
"confidence": 0.2820489477598529
|
||||
},
|
||||
{
|
||||
"marker_id": 76,
|
||||
"observed_center_px": [
|
||||
1550.0,
|
||||
745.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1549.9093017578125,
|
||||
744.649169921875
|
||||
],
|
||||
"reprojection_error_px": 0.8556506255348009,
|
||||
"confidence": 0.2957408773689774
|
||||
},
|
||||
{
|
||||
"marker_id": 69,
|
||||
"observed_center_px": [
|
||||
465.5,
|
||||
587.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
467.173583984375,
|
||||
587.0509643554688
|
||||
],
|
||||
"reprojection_error_px": 1.7327770666811397,
|
||||
"confidence": 0.3850070054256877
|
||||
},
|
||||
{
|
||||
"marker_id": 103,
|
||||
"observed_center_px": [
|
||||
649.0,
|
||||
589.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
649.4606323242188,
|
||||
590.1286010742188
|
||||
],
|
||||
"reprojection_error_px": 0.7793083142275168,
|
||||
"confidence": 0.4188423377379467
|
||||
},
|
||||
{
|
||||
"marker_id": 51,
|
||||
"observed_center_px": [
|
||||
720.75,
|
||||
621.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
720.8779907226562,
|
||||
620.9248046875
|
||||
],
|
||||
"reprojection_error_px": 0.14844514174617424,
|
||||
"confidence": 0.43643037974141263
|
||||
},
|
||||
{
|
||||
"marker_id": 58,
|
||||
"observed_center_px": [
|
||||
569.25,
|
||||
573.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
570.5189208984375,
|
||||
573.7750244140625
|
||||
],
|
||||
"reprojection_error_px": 1.2691676279320252,
|
||||
"confidence": 0.37500699513057323
|
||||
},
|
||||
{
|
||||
"marker_id": 68,
|
||||
"observed_center_px": [
|
||||
1415.5,
|
||||
663.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1414.463134765625,
|
||||
662.5946044921875
|
||||
],
|
||||
"reprojection_error_px": 1.2266347402207098,
|
||||
"confidence": 0.25940485673792224
|
||||
},
|
||||
{
|
||||
"marker_id": 50,
|
||||
"observed_center_px": [
|
||||
1438.0,
|
||||
635.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1437.16650390625,
|
||||
634.8544921875
|
||||
],
|
||||
"reprojection_error_px": 0.922573665375857,
|
||||
"confidence": 0.270957436835909
|
||||
},
|
||||
{
|
||||
"marker_id": 88,
|
||||
"observed_center_px": [
|
||||
1720.5,
|
||||
694.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1720.8408203125,
|
||||
693.2443237304688
|
||||
],
|
||||
"reprojection_error_px": 0.8289783530016113,
|
||||
"confidence": 0.21331579725472802
|
||||
},
|
||||
{
|
||||
"marker_id": 46,
|
||||
"observed_center_px": [
|
||||
1380.75,
|
||||
628.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1379.9970703125,
|
||||
628.1585693359375
|
||||
],
|
||||
"reprojection_error_px": 0.8267272903932755,
|
||||
"confidence": 0.23649772201893263
|
||||
},
|
||||
{
|
||||
"marker_id": 91,
|
||||
"observed_center_px": [
|
||||
1672.75,
|
||||
657.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1672.596923828125,
|
||||
656.64208984375
|
||||
],
|
||||
"reprojection_error_px": 0.8714597813971704,
|
||||
"confidence": 0.22421739747061642
|
||||
},
|
||||
{
|
||||
"marker_id": 64,
|
||||
"observed_center_px": [
|
||||
541.75,
|
||||
511.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
543.0106811523438,
|
||||
511.8523864746094
|
||||
],
|
||||
"reprojection_error_px": 1.5218014555032349,
|
||||
"confidence": 0.3194003123985894
|
||||
},
|
||||
{
|
||||
"marker_id": 90,
|
||||
"observed_center_px": [
|
||||
1571.25,
|
||||
614.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1571.232177734375,
|
||||
612.768798828125
|
||||
],
|
||||
"reprojection_error_px": 1.2313301583159495,
|
||||
"confidence": 0.21534147510121324
|
||||
},
|
||||
{
|
||||
"marker_id": 53,
|
||||
"observed_center_px": [
|
||||
1340.25,
|
||||
579.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1339.28076171875,
|
||||
579.6795654296875
|
||||
],
|
||||
"reprojection_error_px": 0.9717941523468644,
|
||||
"confidence": 0.2131836038878541
|
||||
},
|
||||
{
|
||||
"marker_id": 72,
|
||||
"observed_center_px": [
|
||||
1282.0,
|
||||
562.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1280.0421142578125,
|
||||
563.0445556640625
|
||||
],
|
||||
"reprojection_error_px": 1.9799190939764175,
|
||||
"confidence": 0.2329113678114362
|
||||
},
|
||||
{
|
||||
"marker_id": 98,
|
||||
"observed_center_px": [
|
||||
1493.25,
|
||||
573.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1492.691650390625,
|
||||
572.2074584960938
|
||||
],
|
||||
"reprojection_error_px": 1.6404842509340254,
|
||||
"confidence": 0.19836555127576866
|
||||
},
|
||||
{
|
||||
"marker_id": 67,
|
||||
"observed_center_px": [
|
||||
1411.0,
|
||||
570.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1410.1534423828125,
|
||||
569.2794799804688
|
||||
],
|
||||
"reprojection_error_px": 1.111669419280521,
|
||||
"confidence": 0.22146320976627604
|
||||
},
|
||||
{
|
||||
"marker_id": 70,
|
||||
"observed_center_px": [
|
||||
1516.0,
|
||||
598.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1515.696044921875,
|
||||
597.093505859375
|
||||
],
|
||||
"reprojection_error_px": 1.1957706246675956,
|
||||
"confidence": 0.17917987028547397
|
||||
},
|
||||
{
|
||||
"marker_id": 60,
|
||||
"observed_center_px": [
|
||||
1326.5,
|
||||
509.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1325.19873046875,
|
||||
509.093017578125
|
||||
],
|
||||
"reprojection_error_px": 1.3634284303456619,
|
||||
"confidence": 0.19672907121069258
|
||||
},
|
||||
{
|
||||
"marker_id": 84,
|
||||
"observed_center_px": [
|
||||
1284.5,
|
||||
508.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1283.4097900390625,
|
||||
507.8693542480469
|
||||
],
|
||||
"reprojection_error_px": 1.0980100506965966,
|
||||
"confidence": 0.1992299923059727
|
||||
},
|
||||
{
|
||||
"marker_id": 86,
|
||||
"observed_center_px": [
|
||||
1259.0,
|
||||
466.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
1258.5447998046875,
|
||||
466.7055358886719
|
||||
],
|
||||
"reprojection_error_px": 0.4994519189518402,
|
||||
"confidence": 0.20697942435059868
|
||||
},
|
||||
{
|
||||
"marker_id": 82,
|
||||
"observed_center_px": [
|
||||
1129.75,
|
||||
393.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1128.9288330078125,
|
||||
393.5796813964844
|
||||
],
|
||||
"reprojection_error_px": 0.8386439386067119,
|
||||
"confidence": 0.16919547786983627
|
||||
},
|
||||
{
|
||||
"marker_id": 75,
|
||||
"observed_center_px": [
|
||||
862.5,
|
||||
325.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
861.7324829101562,
|
||||
326.8608703613281
|
||||
],
|
||||
"reprojection_error_px": 1.7843726640496438,
|
||||
"confidence": 0.21628605150283992
|
||||
},
|
||||
{
|
||||
"marker_id": 73,
|
||||
"observed_center_px": [
|
||||
1153.5,
|
||||
379.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
1152.4403076171875,
|
||||
379.2215881347656
|
||||
],
|
||||
"reprojection_error_px": 1.08261223328565,
|
||||
"confidence": 0.1485104612681437
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
data/homing/20260625_175916/cam2_debug.jpg
Normal file
BIN
data/homing/20260625_175916/cam2_debug.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 458 KiB |
2487
data/homing/20260625_175916/robot_1781069752019.json
Normal file
2487
data/homing/20260625_175916/robot_1781069752019.json
Normal file
File diff suppressed because it is too large
Load Diff
59
data/homing/20260625_175916/robot_state.json
Normal file
59
data/homing/20260625_175916/robot_state.json
Normal file
@@ -0,0 +1,59 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-06-25T17:59:33Z",
|
||||
"method": "hybrid",
|
||||
"seeded": true,
|
||||
"movements": {
|
||||
"x": {
|
||||
"value": 110.01986467359067,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 6
|
||||
},
|
||||
"y": {
|
||||
"value": 1.0330890750947754,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 6
|
||||
},
|
||||
"z": {
|
||||
"value": 96.2744445958484,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 1
|
||||
},
|
||||
"a": {
|
||||
"value": 86.75801106951552,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "high",
|
||||
"n_markers": 3
|
||||
},
|
||||
"b": {
|
||||
"value": 62.05866240456044,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"c": {
|
||||
"value": -80.56572105590129,
|
||||
"unit": "deg",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
},
|
||||
"e": {
|
||||
"value": 35.00722900692152,
|
||||
"unit": "mm",
|
||||
"observable": true,
|
||||
"confidence": "medium",
|
||||
"n_markers": 3
|
||||
}
|
||||
},
|
||||
"residual_rms": 33.572073291771,
|
||||
"num_markers": 55
|
||||
}
|
||||
222
data/homing/20260625_175916/state_Arm1.json
Normal file
222
data/homing/20260625_175916/state_Arm1.json
Normal file
@@ -0,0 +1,222 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm1",
|
||||
"joint": "y",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
154.64058080413625,
|
||||
108.3154,
|
||||
53.4964
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 7.073578785211089,
|
||||
"circular_variance": 0.007972526914742928,
|
||||
"circular_std_deg": 7.249434372855015,
|
||||
"num_pairs_used": 15,
|
||||
"num_markers_matched": 6,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
56
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 0.20885786426191633,
|
||||
"baseline_model_mm": 430.4301189508002,
|
||||
"baseline_obs_mm": 431.321568455382,
|
||||
"weight": 185653.79401629578
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
77
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 0.1534985213298885,
|
||||
"baseline_model_mm": 455.14017577005876,
|
||||
"baseline_obs_mm": 456.76224811573417,
|
||||
"weight": 207890.84989252244
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 2.7294530137510984,
|
||||
"baseline_model_mm": 120.70271952197267,
|
||||
"baseline_obs_mm": 235.9270100140718,
|
||||
"weight": 28477.031717386148
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 36.070256539732,
|
||||
"baseline_model_mm": 63.35571402801802,
|
||||
"baseline_obs_mm": 159.8295466346313,
|
||||
"weight": 10126.115049811471
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
55,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 89.62500483491822,
|
||||
"baseline_model_mm": 34.32559540634365,
|
||||
"baseline_obs_mm": 110.61945936866468,
|
||||
"weight": 3797.0788063572545
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
77
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -0.8278666326016993,
|
||||
"baseline_model_mm": 24.720487454740837,
|
||||
"baseline_obs_mm": 25.442346313142185,
|
||||
"weight": 628.9472028532032
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 16.060958275347453,
|
||||
"baseline_model_mm": 332.5358726513577,
|
||||
"baseline_obs_mm": 249.91702454137288,
|
||||
"weight": 83106.37584629621
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 10.575835506835253,
|
||||
"baseline_model_mm": 421.3102260804976,
|
||||
"baseline_obs_mm": 329.51739043275734,
|
||||
"weight": 138829.0462606806
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
56,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 8.204562901697962,
|
||||
"baseline_model_mm": 452.5694494770941,
|
||||
"baseline_obs_mm": 352.3360154416916,
|
||||
"weight": 159456.51653939928
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
77,
|
||||
198
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 14.349880680055369,
|
||||
"baseline_model_mm": 356.9731122927888,
|
||||
"baseline_obs_mm": 273.03257450221435,
|
||||
"weight": 97465.28787736819
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
77,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 9.66556769141446,
|
||||
"baseline_model_mm": 445.86971516352173,
|
||||
"baseline_obs_mm": 353.78452354827397,
|
||||
"weight": 157741.80474373116
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
77,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 7.573295647776105,
|
||||
"baseline_model_mm": 477.27969043318825,
|
||||
"baseline_obs_mm": 377.3635068156209,
|
||||
"weight": 180107.93771374188
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
229
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -3.397605318552869,
|
||||
"baseline_model_mm": 90.00000000000003,
|
||||
"baseline_obs_mm": 88.54992394070814,
|
||||
"weight": 7969.493154663735
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
198,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -3.2531512518968957,
|
||||
"baseline_model_mm": 129.8075498574717,
|
||||
"baseline_obs_mm": 128.9115321451203,
|
||||
"weight": 16733.690136130772
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
229,
|
||||
243
|
||||
],
|
||||
"link": "Arm1",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": -2.3933465309361535,
|
||||
"baseline_model_mm": 49.49747468305833,
|
||||
"baseline_obs_mm": 49.589805881590635,
|
||||
"weight": 2454.5701611618097
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 44.640580804136256,
|
||||
"y": 7.073578785211089
|
||||
}
|
||||
}
|
||||
65
data/homing/20260625_175916/state_Arm2.json
Normal file
65
data/homing/20260625_175916/state_Arm2.json
Normal file
@@ -0,0 +1,65 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Arm2",
|
||||
"joint": "a",
|
||||
"method": "primary",
|
||||
"joint_origin_world_mm": [
|
||||
244.64058080413622,
|
||||
-139.78180740318305,
|
||||
84.28236561327859
|
||||
],
|
||||
"joint_axis_world": [
|
||||
0.0,
|
||||
0.13416044351804374,
|
||||
0.9909596234938344
|
||||
],
|
||||
"mean_angle_deg": 92.49229573636204,
|
||||
"circular_variance": 0.0005036790852294137,
|
||||
"circular_std_deg": 1.8187344232244496,
|
||||
"num_pairs_used": 2,
|
||||
"num_markers_matched": 3,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
144
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 90.53857082520275,
|
||||
"baseline_model_mm": 26.684454275851333,
|
||||
"baseline_obs_mm": 24.841154956793282,
|
||||
"weight": 662.8726636038881
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
143,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": false,
|
||||
"angle_deg": 94.18507945848495,
|
||||
"baseline_model_mm": 26.836171485515596,
|
||||
"baseline_obs_mm": 28.506933940749555,
|
||||
"weight": 765.0169677602199
|
||||
},
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "primary",
|
||||
"skipped": true,
|
||||
"reason": "bl_model=0.4 bl_obs=3.9 < 15.0"
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 44.640580804136256,
|
||||
"y": 7.073578785211089,
|
||||
"z": 90.63649643342701,
|
||||
"a": 92.49229573636204
|
||||
}
|
||||
}
|
||||
41
data/homing/20260625_175916/state_Ellbow.json
Normal file
41
data/homing/20260625_175916/state_Ellbow.json
Normal file
@@ -0,0 +1,41 @@
|
||||
{
|
||||
"status": "ok",
|
||||
"link": "Ellbow",
|
||||
"joint": "z",
|
||||
"method": "fallback_1_child_axis",
|
||||
"joint_origin_world_mm": [
|
||||
154.64058080413625,
|
||||
-139.78180740318305,
|
||||
84.28236561327859
|
||||
],
|
||||
"joint_axis_world": [
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"mean_angle_deg": 90.63649643342701,
|
||||
"circular_variance": 0.0,
|
||||
"circular_std_deg": 0.0,
|
||||
"num_pairs_used": 1,
|
||||
"num_markers_matched": 1,
|
||||
"per_pair": [
|
||||
{
|
||||
"marker_ids": [
|
||||
144,
|
||||
148
|
||||
],
|
||||
"link": "Arm2",
|
||||
"tier": "fallback_1_child_axis",
|
||||
"skipped": false,
|
||||
"angle_deg": 90.63649643342701,
|
||||
"baseline_model_mm": 107.0,
|
||||
"baseline_obs_mm": 107.57847026231472,
|
||||
"weight": 11510.896318067675
|
||||
}
|
||||
],
|
||||
"accumulated_state": {
|
||||
"x": 44.640580804136256,
|
||||
"y": 7.073578785211089,
|
||||
"z": 90.63649643342701
|
||||
}
|
||||
}
|
||||
@@ -85,9 +85,11 @@ X-Position aus Marker-Positionen schätzen
|
||||
│ → state_Arm2.json
|
||||
▼
|
||||
4b_revolute_angle.py --link Hand --from-state state_Arm2.json
|
||||
│ → state_Hand.json ← accumulated_state enthält x,y,z,a,b,c,e
|
||||
│ → state_Hand.json ← accumulated_state: x,y,z,a,b (4b-Primärkette)
|
||||
│ Fallback 5_pose_estimation.py liefert alle 7 (…,c,e). Nur bekannte
|
||||
│ Achsen gehen ins G92; wirklich fehlende werden weggelassen.
|
||||
▼
|
||||
POST ROBOT_URL/api/state
|
||||
G92 über Driver-WebSocket (DRIVER_WS_URL) — setzt Motorposition ohne Bewegung
|
||||
```
|
||||
|
||||
**Schritte 1–3b** sind dieselbe Board-Pipeline wie in der Kalibrierung.
|
||||
@@ -107,7 +109,7 @@ X-Slider-Position über `--x-mm`.
|
||||
| X-Schätzung | `server/homingOrchestrator.js` → `estimateXFromMarkers()` | Pro Arm-Marker `beobachtetes_x − Modell_x(slider=0)`, gemittelt — rechnet den kinematischen Gelenk-Offset (z.B. Arm1.origin.x=110) heraus. Nur x-zuverlässige Ketten (x-Rotation: Arm1/Ellbow). Fallback: roher Mittelwert |
|
||||
| Homing-Orchestrator | `server/homingOrchestrator.js` → `runHoming()` | Kompletter Ablauf als SSE-Stream |
|
||||
| Backend-Route | `POST /api/homing/run` | SSE-Stream, startet `runHoming()` |
|
||||
| State senden | `POST /api/homing/send-state` | Weiterleitung an `ROBOT_URL/api/state` |
|
||||
| State senden | `POST /api/homing/send-state` | Baut `G92` (fehlende c/e → 0) und sendet es als Plain-Text-G-Code über den Driver-WebSocket (`DRIVER_WS_URL`, `server/driverClient.js`). Der Driver verarbeitet G92 intern als M92 = Motorposition setzen ohne Bewegung. Kein HTTP `/api/state` (gibt es am Driver nicht) |
|
||||
| Run-Daten | `GET /api/homing/run-data?run=ts` | Debug-Bilder (base64) + finalState |
|
||||
| Frontend | `public/index.html` + `public/client.js` | Homing-Buttons, Fortschrittsbalken, Tree View; schreibt Teil-Pose als `G92`-GCode ins Eingabefeld |
|
||||
| Board-Viewer (Homing) | `public/boardViewer.html?mode=homing` | Skelett + Arm-Marker per FK (Three.js): Marker-Quadrat spin-korrekt rotiert + Orientierungszeiger zu Ecke 0 (Modell-Seite); gemessene Marker als Kugeln + Fehlerlinien; progressiver Update je erkanntem Gelenk |
|
||||
@@ -159,4 +161,4 @@ die aktuelle Konfiguration.
|
||||
- [x] **X-Schätzung verfeinern** (2026-06-14): `estimateXFromMarkers()` rechnet den kinematischen Gelenk-Offset heraus statt rohem Mittelwert — behebt den ~110 mm Versatz der Modell-Marker
|
||||
- [x] **Unit-Test für X-Schätzung** (2026-06-14): reine Geometrie nach `server/homingXEstimate.cjs` ausgelöst, `test/homingXEstimate.test.js` (9 Tests, inkl. Regression gegen den Offset-Bug)
|
||||
- [ ] **y-Restfehler** (~2°): erkannt 30° → ausgegeben 28°; vermutlich X-Rest-Rauschen + 4b-Fit-Residuum, noch zu untersuchen
|
||||
- [ ] **robot.json via Driver-API** (optional): wenn Driver `GET ROBOT_URL/api/robot/config` bereitstellt
|
||||
- [x] **robot.json via Driver-API** (2026-06-17): `server/robotConfig.js` — `fetchRobot()`/`pushRobot()`/`robotCachePath`; automatischer Fallback auf lokale Datei wenn `ROBOT_URL` nicht gesetzt
|
||||
|
||||
365
doc/Homing_5_Pose_MultiPoint_Weighted.md
Normal file
365
doc/Homing_5_Pose_MultiPoint_Weighted.md
Normal file
@@ -0,0 +1,365 @@
|
||||
# Homing 5 – Verbesserungspfade: Mehrpunkt-Residuen & Gewichtung
|
||||
|
||||
> Ergänzt [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche
|
||||
> Verbesserungen, die alle an derselben Stelle ansetzen: *was* ein Marker als
|
||||
> Messung beiträgt und *wie stark* sie zählt.
|
||||
>
|
||||
> **Status (2026-06-25):** **Qualitäts-Gewichtung** (Doc-Punkt 3 / Schritte 1+2)
|
||||
> umgesetzt. **Mehrpunkt-Eckresiduen** (Doc-Punkt 1 / Schritte 3+4) als
|
||||
> Opt-in-Modus `corner_points` implementiert, aber nach gescheitertem
|
||||
> Live-Test wieder **DEAKTIVIERT** — robot.json steht auf `corner_pose`
|
||||
> (bewährter Zustand). Der Code bleibt inaktiv im Repo. Ursache + Vorbedingungen
|
||||
> für eine erneute Aktivierung: Notiz unten. **Einzelkamera-Einbindung**
|
||||
> (Doc-Punkt 2 / Schritte 5+6) weiterhin **offen**.
|
||||
|
||||
---
|
||||
|
||||
## Ausgangslage (aktueller Code)
|
||||
|
||||
Pro Marker fließen heute genau **6 Residuen** in `5_pose_estimation.py`s
|
||||
`residual_vector()` ein: 3× Position (`pos_mm`) + 3× Normale (skaliert mit
|
||||
`normal_weight`, Default 100). Beide kommen aus `aruco_marker_poses.json`,
|
||||
geschrieben von `3b_corner_marker_poses.py`:
|
||||
|
||||
```
|
||||
3b: trianguliert 4 Ecken (DLT, je Ecke eigene Multi-View-Triangulation)
|
||||
→ position_m/mm = Schwerpunkt der 4 Ecken
|
||||
→ normal = SVD-Ebenen-Fit durch die 4 Ecken
|
||||
→ corners_m = die 4 Ecken selbst (steht im JSON, wird aber von
|
||||
5_pose_estimation.py nie gelesen)
|
||||
```
|
||||
|
||||
Alle drei folgenden Punkte hängen an genau dieser Kette.
|
||||
|
||||
---
|
||||
|
||||
## 1. Vier Eckpunkte statt Center+Normal
|
||||
|
||||
**Frage des Nutzers:** „Mit vier Eckpunkten hätten wir weit mehr Statistik,
|
||||
und nicht die zusammengefassten Werte, oder?" — ja.
|
||||
|
||||
**Befund:** `corners_m` steht bereits in `aruco_marker_poses.json`
|
||||
(`3b_corner_marker_poses.py:166`), wird aber von
|
||||
`load_observations()` (`5_pose_estimation.py:110`) nicht gelesen. Center und
|
||||
Normale sind beide bereits verlustbehaftete Zusammenfassungen der 4 Ecken
|
||||
(Mittelwert bzw. SVD-Ebenen-Fit) — der eigentliche Fit in `residual_vector()`
|
||||
sieht nur noch diese zusammengefasste Version, nicht die Rohdaten.
|
||||
|
||||
**Idee:** Pro Marker 4×3=12 Eck-Residuen statt 3+3 Center/Normal-Residuen.
|
||||
Vorteile:
|
||||
- Mehr unabhängige Messpunkte → realistischere Statistik/Unsicherheit.
|
||||
- Eine einzelne schlecht erkannte Ecke (Verdeckung, Blur) verzerrt nicht mehr
|
||||
automatisch Center *und* Normale des ganzen Markers — der robuste
|
||||
Huber-Verlust könnte direkt auf Eck-Ebene greifen statt auf Marker-Ebene.
|
||||
- Lage *und* Orientierung kommen aus denselben 4 Punkten — keine separate,
|
||||
zusätzlich verlustbehaftete Normalen-Schätzung nötig.
|
||||
|
||||
**Nötig dafür:**
|
||||
- `robot_fk.py`: neue Methode (analog `marker_world()`), die die 4 lokalen
|
||||
Eckpunkte eines Markers (aus `marker["size"]` + ArUco-Eckreihenfolge/Spin-
|
||||
Konvention, schon bekannt aus `corner_plane_normal()` und
|
||||
`doc/Kalibrierung_Marker.md`) ins Weltsystem transformiert.
|
||||
- `5_pose_estimation.py`: `load_observations()` um `corners_m` erweitern,
|
||||
`residual_vector()` um einen dritten Modus neben den bestehenden
|
||||
`"corner_pose"`/`"center_point"`-Werten von `marker_observation`.
|
||||
|
||||
**Risiko:** mittel. Die Spin/Eckreihenfolge-Konvention muss exakt stimmen,
|
||||
sonst systematischer Bias statt Verbesserung. Gut gegen Simulations-GT
|
||||
(appRobotRendering) prüfbar, bevor an reale Daten.
|
||||
|
||||
---
|
||||
|
||||
## 2. Einzelkamera-Marker einbeziehen statt verwerfen
|
||||
|
||||
**Frage des Nutzers:** Komplett verworfene 1-Kamera-Marker seien vermutlich
|
||||
schlechter als sie schwach gewichtet einzubeziehen — insbesondere relevant für
|
||||
zukünftige Finger-Marker, wo oft zwei Kameras je einen *anderen* Marker sehen.
|
||||
|
||||
**Befund — zwei Stellen verwerfen heute hart:**
|
||||
- `3b_corner_marker_poses.py:142`: `if len(cam_ids) < args.minCams: continue`
|
||||
(Default `--minCams 2`) — ein 1-Kamera-Marker bekommt **gar keinen Eintrag**
|
||||
in `aruco_marker_poses.json`. Das passiert schon **vor** `5_pose_estimation.py`.
|
||||
- `5_pose_estimation.py`s `load_observations(..., min_cams=2)` filtert
|
||||
zusätzlich — meist wirkungslos, weil 3b den Marker oft schon gar nicht erst
|
||||
schreibt.
|
||||
|
||||
### Reprojektions-Residuum statt Single-View-PnP (bevorzugter Weg)
|
||||
|
||||
**Ursprünglich angenommener Weg — Single-View-PnP — explizit nicht bevorzugt.**
|
||||
`triangulate_multiview()` (3b) ist Multi-View-DLT — mit 1 Kamera unterbestimmt,
|
||||
ein einzelnes Bild kann keinen 3D-Punkt triangulieren. Der naheliegende Ersatz
|
||||
wäre Single-View-PnP (bekannte Kantenlänge `size` als Skalen-Anker) — das hat
|
||||
aber die bekannte **Flip-Ambiguität** (zwei spiegelbildliche Lösungen aus
|
||||
einer Ansicht, vgl. appRobotRendering `scene_reconstruct.py`). Nur als
|
||||
**Fallback** vorgesehen ("besser als nichts"), explizit nicht der Hauptweg.
|
||||
|
||||
**Bevorzugt: Reprojektion in den bestehenden globalen Fit, keine eigene
|
||||
Marker-Pose.** Kamera A ist gegen das Board bereits voll posenbestimmt
|
||||
(Stufe 2: `K`,`D`,`R`,`t` bekannt). Statt für einen 1-Kamera-Marker einen
|
||||
eigenständigen (zwangsläufig mehrdeutigen) 3D-Punkt zu rekonstruieren, geht er
|
||||
direkt als **Bild-Residuum** in denselben globalen Zustand `q` ein:
|
||||
|
||||
```
|
||||
für den Gelenkzustand q (einzige Unbekannte — keine separate Marker-Pose):
|
||||
P_k(q) = T_link(q) · p_k_lokal (4 Eckpunkte via FK)
|
||||
û_k = project(K_A, D_A, R_A, t_A; P_k(q)) (Reprojektion in Kamera A)
|
||||
residual_k = û_k − u_k_beobachtet (Pixel-Differenz je Ecke)
|
||||
```
|
||||
|
||||
Keine Flip-Ambiguität, weil keine unabhängige Marker-Pose gesucht wird — nur
|
||||
`q` (7 Variablen, durch alles andere im Modell schon mitbestimmt). Ein falscher
|
||||
`q` projiziert einfach sichtbar daneben, statt eine zweite gültige Lösung zu
|
||||
erzeugen.
|
||||
|
||||
**„Kette von Triangulations-Optionen" (Nutzeridee):** Geteilte Variablen
|
||||
(z. B. `e` bei FingerA/FingerB) werden in `analyze_chain()` schon heute in
|
||||
**einen** Block zusammengefasst — sind beide Finger-Marker einzeln
|
||||
triangulierbar (je ≥2 Kameras), bestimmen sie `e` schon jetzt gemeinsam. Neu
|
||||
wäre: auch wenn FingerA nur von Kamera 1 und FingerB nur von Kamera 2 gesehen
|
||||
wird (keiner einzeln triangulierbar), könnten zwei Reprojektions-Residuen
|
||||
(beide Funktionen desselben `q`) den geteilten Freiheitsgrad **gemeinsam**
|
||||
einschränken — ohne dass einer der beiden Marker für sich eine vollständige
|
||||
Pose bräuchte. Passt zur vom Nutzer ausdrücklich erlaubten groben Genauigkeit
|
||||
(±10°/±10 mm reicht für die Finger völlig).
|
||||
|
||||
**Bezug zu Finger-Positionen:** `Hand`/`Palm`/`FingerA`/`FingerB` haben aktuell
|
||||
**0 Marker** in `robot.json`. Sobald dort Marker ergänzt werden, dürften sie
|
||||
wegen Größe/Beweglichkeit/Verdeckung oft nur von 1 Kamera gesehen werden — mit
|
||||
der harten ≥2-Kamera-Regel blieben diese Gelenke dann trotz vorhandener Marker
|
||||
oft `confidence:"none"`. Die Teilbaum-Logik in `observability()` hilft nur,
|
||||
wenn irgendein Marker *im Teilbaum* ≥2 Kameras hat — nicht, wenn alle
|
||||
Finger-Marker isoliert nur je 1 Kamera sehen.
|
||||
|
||||
**Nötig dafür (deutlich größer als „Schwellwert ändern"):**
|
||||
- 3b (oder ein neuer Zwischenschritt): 1-Kamera-Beobachtungen nicht verwerfen,
|
||||
sondern mit Kamera-Referenz + rohen 2D-Eckpunkten aufnehmen.
|
||||
- `5_pose_estimation.py`: `load_observations()`/`estimate_pose()` müssten
|
||||
zusätzlich Kamerakalibrierung (`{cam}_camera_pose.json`, NPZ-Intrinsik)
|
||||
einlesen können — heute reicht `aruco_marker_poses.json` allein.
|
||||
- `residual_vector()`: zweiter Residuumstyp (Pixel statt mm) **gemeinsam** mit
|
||||
den bestehenden mm-Residuen optimiert, mit eigenem Gewicht (analog
|
||||
`normal_weight`, aber für „Pixel vs. Millimeter").
|
||||
|
||||
**Risiko:** primär die **Gewichtung zwischen den beiden Residuumstypen** —
|
||||
falsch skaliert, dominiert einer den Fit und verschlechtert sogar die heute
|
||||
schon gut funktionierenden ≥2-Kamera-Marker. Architektonisch größer als
|
||||
Punkt 1/3, aber ohne die Flip-Problematik des ursprünglich angenommenen Wegs.
|
||||
|
||||
---
|
||||
|
||||
## 3. Marker-Qualitäts-Gewichtung (Größe/Schärfe/Distanz/Kontrast)
|
||||
|
||||
**Vom Nutzer bestätigt:** bewusst (provisorisch) entfernt — brachte in
|
||||
Simulationen wenig, Option soll aber offen bleiben.
|
||||
|
||||
**Befund — die Bausteine existieren bereits, sind aber nicht verbunden:**
|
||||
- `1_detect_aruco_observations.py` berechnet pro Detektion bereits:
|
||||
`area_px`, `sharpness` (Laplace-Varianz, `compute_sharpness()`), `contrast`/
|
||||
`dynamic_range` (`compute_contrast()`), `distance_to_border_px`, kombiniert
|
||||
zu einem `confidence`-Score (`compute_confidence()`) — geschrieben ins
|
||||
`quality`-Feld jeder Detektion in `{cam}_aruco_detection.json`.
|
||||
- `robot.json` hat dafür ein fertiges Schema: `observation_weighting`
|
||||
(`distance_weight`, `marker_size_weight`, `view_angle_weight`) und
|
||||
`multiview_calculation` (`size_factor`, `sharpness_factor`, `border_factor`,
|
||||
`homography_factor`, `spin_factor`, `weight_floor`, ...).
|
||||
- **Aber:** Dieses Schema liest nur `3_multiview_bundle_adjustment_v4.py` —
|
||||
ein Skript, das im Homing-Pfad **nicht** läuft (Homing nutzt
|
||||
`3b_corner_marker_poses.py`).
|
||||
- `3b_corner_marker_poses.py`s `load_cameras()` liest aus der Detection-JSON
|
||||
nur `camera_matrix`, `distortion_coefficients`, `image_points_px` — das
|
||||
`quality`-Feld wird **nie gelesen**. Es geht also schon hier verloren, bevor
|
||||
es überhaupt zu `aruco_marker_poses.json` käme.
|
||||
|
||||
**Idee (falls reaktiviert):** Die Qualitätsmaße existieren schon ganz am
|
||||
Anfang der Pipeline — nur durchreichen nötig: 3b liest `quality`/`confidence`
|
||||
aus der Detection-JSON und schreibt einen `weight`-Wert pro Marker in
|
||||
`aruco_marker_poses.json`; `5_pose_estimation.py`s `residual_vector()` skaliert
|
||||
das jeweilige Markerresiduum damit (analog zu `normal_weight`, aber pro Marker
|
||||
statt global für alle).
|
||||
|
||||
**Mögliche Erklärung, warum es in Simulation wenig brachte:** Der
|
||||
appRobotRendering-Renderfehler-Boden (`markerOffsetMaxMm`, `sensorNoise`, …
|
||||
aus `renderingInfo`) ist recht gleichförmig über alle Marker — wenig echte
|
||||
Qualitätsunterschiede zum Gewichten. Bei echten Kameras (Beleuchtung,
|
||||
Entfernung, Bewegungsunschärfe) könnte die Streuung größer und der Effekt
|
||||
dadurch sichtbarer sein — das ist der Grund, die Option offen zu halten.
|
||||
|
||||
**Nötig dafür:** klein und lokal begrenzt — nur 2 Stellen (3b: `quality`
|
||||
durchreichen; `5_pose_estimation.py`: Gewicht im Residuum nutzen).
|
||||
`1_detect_aruco_observations.py` und das `robot.json`-Schema müssen nicht
|
||||
angefasst werden, die liegen schon bereit.
|
||||
|
||||
**Risiko:** niedrig (reines Durchreichen + ein Faktor) — der Aufwand liegt im
|
||||
erneuten Validieren (Simulation + reale Daten), nicht im Code selbst.
|
||||
|
||||
---
|
||||
|
||||
## Zusammenhang der drei Punkte
|
||||
|
||||
Alle drei ändern letztlich dasselbe: was als „eine Messung" zählt und wie
|
||||
stark sie zählt. Sie sind unabhängig umsetzbar, aber am Ende würde man sie
|
||||
zusammenführen wollen:
|
||||
|
||||
```
|
||||
heute: residual = [Δposition, Δnormal × normal_weight] (6 Werte/Marker, nur ≥2-Kamera-Marker)
|
||||
1: residual = [Δcorner_0 .. Δcorner_3] × marker_weight (12 Werte/Marker, weiterhin ≥2-Kamera)
|
||||
2: + Reprojektions-Residuen für 1-Kamera-Marker (neuer Typ, eigene Gewichtung)
|
||||
3: marker_weight zusätzlich nach Quality-Score aus Stufe 1
|
||||
```
|
||||
|
||||
**Geschätzte Reihenfolge nach Aufwand/Risiko** (keine Festlegung, nur
|
||||
Einschätzung): 3 (niedrig, reines Durchreichen) → 1 (mittel, FK-Erweiterung,
|
||||
gut simulationstestbar) → 2 (architektonisch am größten — neuer Residuumstyp
|
||||
+ Kamerakalibrierung als zusätzlicher Input —, aber ohne Flip-Risiko, seit
|
||||
Reprojektion statt PnP der bevorzugte Weg ist).
|
||||
|
||||
---
|
||||
|
||||
## Umsetzungsplan (ToDo)
|
||||
|
||||
Reihenfolge folgt der Risiko-Einschätzung oben: erst risikoarmes Durchreichen,
|
||||
dann die beiden strukturellen Erweiterungen. Jeder Schritt ist einzeln
|
||||
umsetzbar und (wo möglich) einzeln testbar, bevor der nächste beginnt.
|
||||
|
||||
> **Status-Legende:** ✅ erledigt · ⬜ **offen**
|
||||
>
|
||||
> **Punkt-Zuordnung** (Doc-Abschnitt ↔ Chat-Nummerierung): Doc-Punkt 1 „Vier
|
||||
> Eckpunkte" = Schritte 3+4 · Doc-Punkt 2 „Einzelkamera" = Schritte 5+6 ·
|
||||
> Doc-Punkt 3 „Qualitäts-Gewichtung" = Schritte 1+2 (erledigt).
|
||||
|
||||
| # | Status | Schritt | Testbar an | Bricht Bestehendes? |
|
||||
|---|---|---|---|---|
|
||||
| 1 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es |
|
||||
| 2 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden |
|
||||
| 3 | ✅ erledigt (2026-06-25) | **(Doc-Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode |
|
||||
| 4 | 🔴 Code da, aber DEAKTIVIERT (live gescheitert 2026-06-25 → robot.json zurück auf `corner_pose`) | **(Doc-Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden |
|
||||
| 5 | ⬜ **offen** (Vorarbeit/Guards ✅) | **(Doc-Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird |
|
||||
| 6 | ⬜ **offen** | **(Doc-Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) |
|
||||
| 7 | ⬜ **offen** | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten |
|
||||
|
||||
### Recherche zu Schritt 5: wer liest `aruco_marker_poses.json`? (2026-06-16)
|
||||
|
||||
Alle Dateien mit Referenz auf `aruco_marker_poses` im Projekt durchsucht und
|
||||
geprüft, ob sie `position_mm` (o. ä.) ungeschützt voraussetzen oder schon einen
|
||||
Guard haben:
|
||||
|
||||
| Datei | Nutzung | Wenn `position_mm` fehlt | Guard schon da? |
|
||||
|---|---|---|---|
|
||||
| `public/yAxisCompute.js:109-111` | Y-Rotationsachse Base↔Arm1 (Kalibrierung [4], Kreisfit über 3 Posen) | Guard eingefügt (Z. 109–114): `Array.isArray()`-Check auf alle drei Posen, fehlende landen im `skipped`-Log statt Crash | ✅ Ja (2026-06-16) |
|
||||
| `public/boardViewer.html` | X-Achsen-Richtung (Kalibrierung [3]) **und** Y-Achsen-Viewer **und** allgemeiner Pos-A/B/C-Vergleich | `hasXYZ()`-Helper (Z. 220–226) + Pre-Filterung der `_*FremdMarkers`-Arrays beim Laden (Z. 1069/1107/1140); direkte Zugriffe in `buildCompareLines()` (Z. 892, 915–916) sicher, weil nur pre-gefilterte Marker in den Arrays stehen | ✅ Ja (2026-06-16) |
|
||||
| `public/homing.js:96` | Homing-Marker-Tabelle | Keiner — `m.position_mm ?? [null,null,null]` | ✅ Ja |
|
||||
| `server/editRobot.js` → `assignByZRange()` | Marker-Z-Bereich-Zuordnung (Kalibrierung „Board"-Tab) | Keiner — `Array.isArray(emPos)`-Check, sonst `continue` | ✅ Ja |
|
||||
| `server/editRobot.js` → `alignSetToMeasured()` | Set-Ausrichtung (Kabsch-Fit) | Keiner — Marker ohne `position_mm` werden beim Aufbau der Messwert-Map einfach ausgelassen | ✅ Ja |
|
||||
| `server/editRobot.js` → `assignMarkerId()` | Einzelnen Marker manuell per ID zuordnen | Guard eingefügt (vor Z. 379): `Array.isArray(em.position_mm)`-Check — fehlende Position gibt klare Fehlermeldung statt Crash | ✅ Ja (2026-06-16) |
|
||||
| `scripts/4_yAxis_rotation_reconstruction.py` | Python-Variante der Y-Achsen-Rekonstruktion (offline, parallel zu `yAxisCompute.js`) | Guard eingefügt (Z. 165–174): expliziter `None`-Check statt `.get(..., [0,0,0])` — fehlende Messung landet mit klarem Grund im `skipped`-Log | ✅ Ja (2026-06-16) |
|
||||
| `scripts/9_evaluateMarker.py` | Offline-Benchmark gegen Simulations-GT, **nicht** im Live-Homing-Pfad | **Crash** — `o["position_m"]` ohne `.get()` | ❌ Nein, aber kein Produktionscode |
|
||||
| `public/client.js` | Nur CSV-Anzeige/Zahlenformat | Keine Berechnung, nur Darstellung | n/a |
|
||||
|
||||
**Bestätigt deine Vermutung, aber breiter als gedacht:** Es betrifft tatsächlich
|
||||
nur die **X-Achsen-** und **Y-Rotationsachsen-Kalibrierung** (Schritt [3]/[4]
|
||||
in `Kalibrierung.md`) plus den Offline-Benchmark — aber **nicht nur ein
|
||||
Filter an einer Stelle**, sondern `yAxisCompute.js` **und** mehrere Stellen in
|
||||
`boardViewer.html` (das Viewer-File bedient beide Kalibrierschritte). Die
|
||||
Homing-Seite selbst (`editRobot.js`, `homing.js`) ist bereits robust.
|
||||
|
||||
**Guards umgesetzt (2026-06-16) — alle relevanten Stellen:**
|
||||
- `yAxisCompute.js` (Z. 109–114): `Array.isArray()`-Check, fehlende landen im `skipped`-Log.
|
||||
- `boardViewer.html`: `hasXYZ()`-Helper (Z. 220–226) + Pre-Filterung der `_*FremdMarkers`-Arrays; Viewer in allen Situationen getestet und stabil.
|
||||
- `4_yAxis_rotation_reconstruction.py` (Z. 165–174): expliziter `None`-Check ersetzt irreführendes `.get(..., [0,0,0])`; fehlende Messung landet mit klarem Grund im `skipped`-Log.
|
||||
- `editRobot.js` → `assignMarkerId()` (vor Z. 379): `Array.isArray(em.position_mm)`-Check gibt klare Fehlermeldung zurück statt Crash.
|
||||
|
||||
Alle anderen Konsumenten (`homing.js`, `editRobot.js` → `assignByZRange`/`alignSetToMeasured`, `scripts/9_evaluateMarker.py`) waren schon robust oder sind Offline-Benchmark-Code ohne Produktionsrelevanz.
|
||||
|
||||
### Umsetzung Schritt 3+4 (Doc-Punkt 1) — Befunde (2026-06-25)
|
||||
|
||||
- **Schritt 3** (`robot_fk.py`): `marker_corners_local/_world` + `all_markers_world`
|
||||
liefern jetzt `corners_world` (4×3, in `corners_m`-Reihenfolge). Orientierung =
|
||||
Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale (exakt wie
|
||||
boardViewer.html). Verifiziert in `test/test_robot_fk_corners.py`:
|
||||
Selbst-Konsistenz (Center/Kanten/planar/Normalen-Rückgewinnung) **und** gegen
|
||||
echte triangulierte Roboter-Ecken am Seed-Pose (~1 mm RMS, Identitäts-Paarung
|
||||
schlägt jede Umordnung). Eckreihenfolge = `(+h,+h),(+h,-h),(-h,-h),(-h,+h)`
|
||||
(= boardViewer-Zeiger (1,1,0)).
|
||||
- **Wichtiger Konventions-Befund:** Die `spin`-Werte sind nur für die
|
||||
**Roboter-Marker** kalibriert/visuell verifiziert, **nicht** für die Board/
|
||||
Rail-Marker (Set A0/rail, alle auf dem Root-Link `Board`). Deren Eckreihenfolge
|
||||
liegt ~90° daneben. Lösung: `corner_points` nutzt 12 Eck-Residuen nur für
|
||||
Roboter-Links (`corner_point_links`, Default = alle Nicht-Root-Links) und 1
|
||||
Center-Residuum für die Board/Rail-Marker. Da `Board` Root ist (Residuum
|
||||
konstant bzgl. der Gelenke), kostet das nichts an Information.
|
||||
- **Datenbefund (nicht Code):** 6 Marker des Board-Sets **A0 sind auf `Arm1`
|
||||
fehlzugeordnet** (`[55,56,57,77,78,99]`), ~230 mm neben dem Modell. Sie
|
||||
destabilisieren `corner_points` (12 statt 3 Residuen → falsches Minimum) und
|
||||
ziehen auch `corner_pose` leicht. Auf bereinigten Markern konvergiert
|
||||
`corner_points` ≈ `corner_pose`. → Marker-Zuordnung korrigieren (separate
|
||||
Kalibrier-Aufgabe).
|
||||
- **DEAKTIVIERT (2026-06-25) — Aktivierung am echten Roboter gescheitert,
|
||||
zurückgedreht.** robot.json steht wieder auf `marker_observation:
|
||||
"corner_pose"` (der bewährte Zustand). Der `corner_points`-Code bleibt im
|
||||
Repo, ist aber **inaktiv** (opt-in).
|
||||
|
||||
**Was probiert wurde:** `corner_points` mit `corner_point_links:
|
||||
["Hand","Palm","FingerA","FingerB"]` scharfgeschaltet (nur Hand/Finger über
|
||||
Ecken, Arme/Board unverändert). Am echten Lauf (data/homing/20260625_175916)
|
||||
kippte die Hand (`b` −52° → +62°) und `x` wanderte (160 → 110 mm).
|
||||
|
||||
**Belegte Ursache (Gegenprobe an denselben Daten):**
|
||||
- `corner_pose` → gutes Ergebnis (`b`≈−52, `x`≈160); `corner_points` → das
|
||||
schlechte. Also eindeutig der Modus.
|
||||
- Die **Eck-Konvention ist NICHT der Fehler**: 2 der 3 Finger-Marker passen am
|
||||
guten Pose exakt (FingerB #178/#179, ~2–3 mm, korrekte Eck-Paarung fwd+r0).
|
||||
- **Eigentliche Ursache: ein einzelner schlechter Marker.** FingerA **#147**
|
||||
liegt **132 mm neben dem Modell** (Position/Zuordnung in robot.json noch
|
||||
provisorisch, vgl. Commits „Finger1 Marker"/„zweiter Finger – verdreht").
|
||||
Im Eck-Modus liefert ein Marker **12 statt 3 Residuen** → ein einzelner
|
||||
Ausreißer hat ~4× Zugkraft und reißt `global_ba` ins falsche Minimum.
|
||||
`corner_pose` (3 Residuen) dämpft ihn per Huber weg.
|
||||
|
||||
**Damit `corner_points` je produktiv taugt, fehlen ZWEI Dinge:**
|
||||
1. **Daten:** FingerA #147 sauber einmessen / Zuordnung prüfen.
|
||||
2. **Code-Robustheit:** Ausreißer-Schutz im Eck-Modus (grob daneben liegende
|
||||
Marker pro Marker auf Center zurückfallen lassen oder verwerfen), sonst
|
||||
kippt jede reale Aufnahme mit *einem* schlechten Marker. Erst danach –
|
||||
gegen GT (Simulation oder Finger-Capture bekannter Pose), **nicht** live –
|
||||
erneut testen. CLI zum Vergleichen: `--marker-observation corner_points`.
|
||||
- **Offen (Schritt 4 Tuning):** `huber_delta_mm` ist auf 6 Residuen/Marker
|
||||
kalibriert; mit 12 verschiebt sich die RMS-Größenordnung. Sauberes A/B + Tuning
|
||||
der Hand/Finger-Ecken gegen appRobotRendering-Simulations-GT steht aus (hier
|
||||
fehlten Finger-Marker in den Captures). CLI: `--marker-observation corner_pose`
|
||||
schaltet zum Vergleich zurück.
|
||||
|
||||
## Offene Punkte
|
||||
|
||||
- [ ] Keiner der drei Punkte/Schritte ist priorisiert/entschieden — reine Optionen.
|
||||
- [x] Schritt 5: Konsumenten-Recherche erledigt (Tabelle oben) — vor Schritt 5
|
||||
müssen mindestens `yAxisCompute.js` (1 Stelle) und `boardViewer.html`
|
||||
(≥9 Stellen) einen Guard bekommen (fehlende Marker überspringen statt
|
||||
crashen), sonst brechen X-/Y-Achsen-Kalibrierung beim nächsten Lauf mit
|
||||
1-Kamera-Markern.
|
||||
- [x] Guards für Schritt 5 umgesetzt (2026-06-16): alle vier offenen Stellen
|
||||
(`yAxisCompute.js`, `boardViewer.html`, `4_yAxis_rotation_reconstruction.py`,
|
||||
`editRobot.js → assignMarkerId`) schützen nun gegen fehlende `position_mm`.
|
||||
Schritt 5 selbst (1-Kamera-Marker in 3b aufnehmen) ist noch offen.
|
||||
- [x] Schritt 1 (Punkt 3) umgesetzt (2026-06-17): `3b_corner_marker_poses.py`
|
||||
liest `confidence` aus der Detection-JSON pro Kamera und schreibt
|
||||
`weight` (Mittelwert über alle beteiligten Kameras) als neues Feld in
|
||||
`aruco_marker_poses.json`. Alles andere identisch zu vorher — rein additiv.
|
||||
- [x] Schritt 2 (Punkt 3) umgesetzt (2026-06-17): `5_pose_estimation.py`
|
||||
liest `weight` aus `aruco_marker_poses.json` in `load_observations()` und
|
||||
wendet es in `residual_vector()` an, gesteuert durch
|
||||
`pose_estimation.use_marker_weight` (Default `false`). Kein Verhalten bei
|
||||
Default; aktivierbar sobald Simulationsvalidierung erfolgt.
|
||||
- [ ] Für Schritt 3/4: Eckreihenfolge/Spin-Konvention zuerst exakt verifizieren,
|
||||
bevor Residuen darauf aufbauen.
|
||||
- [ ] Für Schritt 2: Simulationsvalidierung (A/B-Vergleich mit/ohne
|
||||
`use_marker_weight`) vor Umstellung des Defaults auf `true`.
|
||||
|
||||
## Verweise
|
||||
|
||||
- [`Homing_5_Pose.md`](Homing_5_Pose.md) — Hauptdokument zu `5_pose_estimation.py`
|
||||
- `scripts/1_detect_aruco_observations.py` — Qualitätsmaße je Detektion
|
||||
- `scripts/3b_corner_marker_poses.py` — Triangulation, Center/Normal-Aggregation,
|
||||
≥2-Kamera-Filter
|
||||
- `scripts/3_multiview_bundle_adjustment_v4.py` — einziger Ort, der das
|
||||
bestehende Gewichtungsschema (`observation_weighting`/`multiview_calculation`)
|
||||
aktuell liest (nicht im Homing-Pfad)
|
||||
191
doc/Homing_8_appRobotDriver.md
Normal file
191
doc/Homing_8_appRobotDriver.md
Normal file
@@ -0,0 +1,191 @@
|
||||
# Homing 8 – Übergabe an appRobotDriver (G92-Konvention)
|
||||
|
||||
> Technische Doku zur Konvertierung des Homing-Ergebnisses
|
||||
> (FK-State aus [`4b_revolute_angle.py`](../scripts/4b_revolute_angle.py) /
|
||||
> [`5_pose_estimation.py`](../scripts/5_pose_estimation.py))
|
||||
> in die G92-Konvention von **appRobotDriver**.
|
||||
>
|
||||
> Umgesetzt in [`server/server.js`](../server/server.js) → Funktion `fkStateToDriverG92()`,
|
||||
> aufgerufen im `POST /api/homing/send-state`-Handler.
|
||||
>
|
||||
> Vollständige Driver-Konvention: [`appRobotDriver/doc/Info_G92.md`](../../appRobotDriver/doc/Info_G92.md).
|
||||
|
||||
---
|
||||
|
||||
## Das Problem: zwei verschiedene Winkel-Konventionen
|
||||
|
||||
Die Homing-Pipeline (4b / 5_pose_estimation) liefert Gelenkwinkel im
|
||||
**FK-Koordinatensystem von `robot.json`** (forward kinematics, Blender-Hierarchie).
|
||||
Der appRobotDriver erwartet in `G92` eine **eigene Konvention**, die sich an
|
||||
physischen Konfigurationen und Motor-Nullpunkten orientiert — nicht an der FK-Nulllage.
|
||||
|
||||
Für X (Schiene), Y (Oberarm), A (Unterarm-Dreher) stimmen die Konventionen überein.
|
||||
Für **B, C und Z** gibt es definierte Verschiebungen, die vor dem Senden umgerechnet
|
||||
werden müssen.
|
||||
|
||||
---
|
||||
|
||||
## Befehlsformat G92
|
||||
|
||||
```
|
||||
G92 X<mm> Y<°> Z<°> A<°> B<°> C<°> E<mm>
|
||||
```
|
||||
|
||||
Der Befehl setzt am Driver die Motorposition **ohne Bewegung** (intern M92 = Homing).
|
||||
Fehlende Achsen werden weggelassen; der Driver lässt sie unverändert.
|
||||
|
||||
---
|
||||
|
||||
## Umrechnungstabelle FK → Driver
|
||||
|
||||
| Achse | FK-State (Homing) | Driver G92 erwartet | Umrechnung |
|
||||
|-------|--------------------------------------------|--------------------------------------------|--------------------------|
|
||||
| X | xMotor in mm | xMotor in mm | — (identisch) |
|
||||
| Y | Oberarm-Winkel α, absolut (0=waagerecht) | α absolut (0=waagerecht, 90=hoch) | — (identisch) |
|
||||
| Z | Ellbogen-Knick **relativ** zu Arm1 | Unterarm-Winkel β **absolut** (wie Y) | `Z = Y + z_relativ` |
|
||||
| A | Unterarm-Dreher (Arm2-Rotation um -y) | Unterarm-Dreher (Roll um Unterarm-Achse) | — (identisch) |
|
||||
| B | FK `b=0` = gerade Hand (kein Knick) | `B=180°` = gerade Hand, `B=0°` = zurück | `B = 180 − b` |
|
||||
| C | FK `c=0` = neutraler Palm-Roll | `C=90°` = neutral (ψ=0°) | `C = c + 90` |
|
||||
| E | Finger-Öffnung in mm (rein geometrisch) | Finger-Öffnung in mm (Sehnen-Kopplung im Driver) | — (identisch) |
|
||||
|
||||
---
|
||||
|
||||
## Detailerklärung je Achse
|
||||
|
||||
### Z — Ellbogen: relativ vs. absolut
|
||||
|
||||
`4b_revolute_angle.py` misst den Ellbogen-Winkel **relativ zu Arm1** (FK-Variable z =
|
||||
Rotation des Ellbow-Gelenks gegenüber der Arm1-Nulllage). Der Driver interpretiert Z
|
||||
dagegen als **absoluten** Unterarm-Winkel zur Horizontalen — genauso wie Y den
|
||||
Oberarm beschreibt.
|
||||
|
||||
```
|
||||
Z_Driver = Y_FK + z_relativ_FK
|
||||
```
|
||||
|
||||
Intern rechnet der Driver für die FluidNC-Weiterleitung wieder zurück:
|
||||
`FluidNC-z = (β − α) × D`, d.h. relativer Motor-Winkel.
|
||||
|
||||
> **Typische Falle:** z_relativ direkt als Z senden. Bei kleinen Y-Winkeln (Y ≈ 0) ist
|
||||
> der Fehler kaum merklich; bei Y = 86° beträgt er 86°.
|
||||
|
||||
---
|
||||
|
||||
### B — Hand-Knick: 180°-Dreher
|
||||
|
||||
Im `robot.json`:
|
||||
```json
|
||||
"Hand": {
|
||||
"jointToParent": { "axis": [1, 0, 0], "rotation": [0, 0, 0], "variable": "b" }
|
||||
}
|
||||
```
|
||||
|
||||
Die FK-Nulllage (`b = 0`) bedeutet: Hand verlängert Arm2 geradeaus — kein Knick.
|
||||
Der Driver definiert dagegen:
|
||||
|
||||
| B (G92) | physischer Knick Unterarm↔Hand |
|
||||
|---------|-------------------------------|
|
||||
| 0° | 180° (Hand voll zurückgeklappt) |
|
||||
| 90° | 90° (Hand ⊥ Unterarm) |
|
||||
| 180° | 0° (Hand gerade) |
|
||||
|
||||
Ohne Umrechnung wird `b ≈ 0` (gerade Hand) als `B = 0°` gesendet → Driver klappt
|
||||
die Hand voll zurück → „rückwärts haltende Hand-Stellung".
|
||||
|
||||
```
|
||||
B_Driver = 180 − b_FK
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### C — Palm-Roll: 90°-Offset
|
||||
|
||||
Im `robot.json`:
|
||||
```json
|
||||
"Palm": {
|
||||
"jointToParent": { "axis": [0, -1, 0], "rotation": [0, 0, 0], "variable": "c" }
|
||||
}
|
||||
```
|
||||
|
||||
Die FK-Nulllage (`c = 0`) entspricht keiner Rotation des Palm-Gelenks = neutraler Roll.
|
||||
Der Driver definiert `C = 90°` als neutral (ψ = 0°):
|
||||
|
||||
| C (G92) | Hand-Roll ψ |
|
||||
|---------|-------------|
|
||||
| 0° | −90° |
|
||||
| 90° | 0° (neutral) |
|
||||
| 180° | +90° |
|
||||
|
||||
```
|
||||
C_Driver = c_FK + 90
|
||||
```
|
||||
|
||||
> **Vorzeichen prüfen:** Falls nach dem Fix die Palm-Rotation spiegelverkehrt erscheint,
|
||||
> lautet die Formel `C = 90 − c_FK` (Vorzeichen des physischen Motors umgekehrt).
|
||||
> Hängt von der Einbaurichtung des Palm-Servos ab.
|
||||
|
||||
---
|
||||
|
||||
### E — Greifer: Sehnen-Kopplung
|
||||
|
||||
E wird als reine Finger-Öffnung in mm übergeben (FK und Driver identisch — keine
|
||||
Winkelumrechnung). Der Driver berechnet den tatsächlichen Motor-Wert intern:
|
||||
|
||||
```
|
||||
eMotor = E − b − c (b, c in Radiant!)
|
||||
```
|
||||
|
||||
Die Sehnen-Kopplung kompensiert, dass Handgelenk-Knick (B) und Palm-Roll (C)
|
||||
an der Greifer-Sehne ziehen. Sind B und C korrekt gesetzt, stimmt die Kompensation
|
||||
automatisch — E selbst braucht nicht angepasst zu werden.
|
||||
|
||||
---
|
||||
|
||||
## Implementierung
|
||||
|
||||
**`server/server.js`** — Funktion `fkStateToDriverG92()`:
|
||||
|
||||
```js
|
||||
function fkStateToDriverG92(s) {
|
||||
const d = { ...s };
|
||||
if (d.b != null) d.b = 180 - d.b; // Hand-Knick: 180°-Dreher
|
||||
if (d.c != null) d.c = d.c + 90; // Palm-Roll: +90° Offset
|
||||
if (d.z != null && d.y != null) d.z = d.y + d.z; // Ellbogen: relativ → absolut
|
||||
return d;
|
||||
}
|
||||
```
|
||||
|
||||
Aufruf im `POST /api/homing/send-state`-Handler:
|
||||
|
||||
```js
|
||||
const gcode = buildG92(fkStateToDriverG92(state));
|
||||
```
|
||||
|
||||
Null-Werte (unbeobachtbare Achsen) bleiben null → werden von `buildG92` weggelassen →
|
||||
Driver lässt die entsprechenden Achsen unverändert.
|
||||
|
||||
---
|
||||
|
||||
## Kontext: Woher kommen die FK-Werte?
|
||||
|
||||
```
|
||||
4b_revolute_angle.py × N → accumulated_state {x, y, z, a, b}
|
||||
(Hand/b schlägt fehl, wenn keine Marker am Hand-Link)
|
||||
↓ (falls Hand/b fehlt)
|
||||
5_pose_estimation.py → movements {x,y,z,a,b,c,e} als FK-Winkel (Bundle-Adjustment)
|
||||
↓
|
||||
homingOrchestrator.js → finalState (flaches {x,y,z,a,b,c,e})
|
||||
↓
|
||||
POST /api/homing/send-state
|
||||
↓
|
||||
fkStateToDriverG92() → Konvertierung FK → Driver
|
||||
↓
|
||||
buildG92() → "G92 X… Y… Z… A… B… C… E…"
|
||||
↓
|
||||
sendGcode() → appRobotDriver (WebSocket)
|
||||
```
|
||||
|
||||
Im aktuellen `robot_1781069752019.json` hat der Hand-Link **keine eigenen Marker**;
|
||||
der b-Wert kommt daher immer aus `5_pose_estimation.py` (Bundle-Adjustment über
|
||||
die FingerA/FingerB-Marker, die durch die kinematische Kette Hand→Palm→Finger
|
||||
indirekt b einschränken).
|
||||
@@ -1,14 +1,25 @@
|
||||
# robot.json – Zugriff via appRobotDriver
|
||||
|
||||
> Stand: 2026-06-15
|
||||
> Beschreibt die geplante Umstellung: robot.json kommt vom appRobotDriver, nicht
|
||||
> mehr aus einer lokalen Datei.
|
||||
> **Status: umgesetzt** (2026-06-17) — `server/robotConfig.js` ist aktiv.
|
||||
> Dieses Dokument beschreibt Entwurf und Implementierung. Der Implementierungsplan
|
||||
> (Schritte 1–3) ist vollständig abgearbeitet.
|
||||
|
||||
---
|
||||
|
||||
## Ist-Zustand
|
||||
## Verhalten je Env-Variable
|
||||
|
||||
`appRobotHoming` liest und schreibt die Roboter-Konfiguration direkt aus einer
|
||||
| Variable | nicht gesetzt | gesetzt |
|
||||
|----------|--------------|---------|
|
||||
| `ROBOT_URL` | Kein Driver-Kontakt; alle Lese-/Schreibvorgänge direkt auf die lokale Datei | `fetchRobot()` liest von `GET {ROBOT_URL}/api/robot/config`; `pushRobot()` schreibt nach `POST {ROBOT_URL}/api/robot/config` |
|
||||
| `ROBOT_JSON` | Standardpfad `scripts/robot_1781069752019.json` | Angegebener Pfad wird als lokale Cache-Datei verwendet |
|
||||
|
||||
Beide Variablen nicht gesetzt = **reiner Lokal-Modus**, identisch zum Verhalten vor dem Umbau.
|
||||
|
||||
---
|
||||
|
||||
## Ehemaliger Ist-Zustand (vor 2026-06-17)
|
||||
|
||||
`appRobotHoming` las und schrieb die Roboter-Konfiguration direkt aus einer
|
||||
lokalen Datei:
|
||||
|
||||
```
|
||||
@@ -16,8 +27,8 @@ ROBOT_JSON = process.env.ROBOT_JSON
|
||||
|| 'scripts/robot_1781069752019.json'
|
||||
```
|
||||
|
||||
Die Python-Skripte erhalten den Dateipfad als CLI-Argument (`-robot`, `--robot`).
|
||||
Alle Kalibrierungs-Endpoints schreiben ebenfalls in diese Datei.
|
||||
Die Python-Skripte erhielten den Dateipfad als CLI-Argument (`-robot`, `--robot`).
|
||||
Alle Kalibrierungs-Endpoints schrieben ebenfalls in diese Datei.
|
||||
|
||||
**Problem:** Der appRobotDriver besitzt die maßgebliche Konfiguration — nicht das
|
||||
Homing-System. Nach einem Neustart könnten Konfiguration und Driver auseinanderlaufen.
|
||||
@@ -247,9 +258,13 @@ try {
|
||||
|
||||
---
|
||||
|
||||
## Status: Umgesetzt (2026-06-17)
|
||||
|
||||
`server/robotConfig.js` erstellt. `server/editRobot.js` und `server/server.js` angepasst.
|
||||
|
||||
## Offene Fragen
|
||||
|
||||
- [ ] Genaue Endpoints des appRobotDriver für GET / POST robot.json bestätigen
|
||||
- [ ] Genaue Endpoints des appRobotDriver für GET / POST robot.json bestätigen (aktuell: `/api/robot/config`)
|
||||
- [ ] Soll der Driver eine Versions-/Konflikterkennung haben (z.B. ETag / `updatedAt`)?
|
||||
- [ ] Soll `pushRobot()` bei Driver-Fehler still auf lokal-only zurückfallen, oder hard fail?
|
||||
- [ ] `pushRobot()` bei Driver-Fehler: aktuell hard fail → Kalibrierungs-Endpoint antwortet 502
|
||||
- [ ] Authentifizierung zwischen appRobotHoming und appRobotDriver nötig?
|
||||
|
||||
467
doc/homingAPI.md
Normal file
467
doc/homingAPI.md
Normal file
@@ -0,0 +1,467 @@
|
||||
# Homing Offline-API
|
||||
|
||||
> **Status: implementiert** (2026-06-18).
|
||||
> `POST /api/homing/run-offline` ist aktiv in `server/server.js`.
|
||||
> Abhängigkeit: `multer` (npm-Package, bereits installiert).
|
||||
>
|
||||
> Gedacht für die **Simulations-Pipeline** (`appRobotRendering`), die synthetische oder
|
||||
> aufgezeichnete Bilder liefert und die aktuelle Pose-Erkennung von appRobotHoming nutzen
|
||||
> möchte — ohne den Umweg über echte Kameras und den WebCam-Service.
|
||||
|
||||
---
|
||||
|
||||
## Motivation
|
||||
|
||||
Die Live-Homing-Pipeline (`POST /api/homing/run`) setzt voraus, dass `WEBCAM_URL` auf
|
||||
einen laufenden WebCam-Service zeigt, der Bilder auf Abruf liefert. Das ist für zwei
|
||||
Szenarien unpraktisch:
|
||||
|
||||
1. **Simulations-Validierung** — `appRobotRendering` rendert synthetische Bilder zu
|
||||
bekannten Gelenkwinkeln und will prüfen, wie gut die aktuelle Pose-Erkennung die
|
||||
Winkel zurückrechnet. Die Pipeline in `appRobotRendering` liegt lokal und braucht
|
||||
keine Live-Kamera.
|
||||
|
||||
2. **Offline-Replay** — früher aufgenommene Bildsätze sollen mit dem *aktuellen*
|
||||
Stand der Algorithmen neu ausgewertet werden (z. B. nach Verbesserungen an
|
||||
`4b_revolute_angle.py` oder `5_pose_estimation.py`).
|
||||
|
||||
In beiden Fällen sind Bilder und Kalibrierungsdaten bereits vorhanden. Die API soll
|
||||
diese entgegennehmen, die Pipeline identisch zum Live-Modus durchlaufen und die
|
||||
Ergebnisdateien zurückgeben.
|
||||
|
||||
---
|
||||
|
||||
## Abgrenzung zum Live-Modus
|
||||
|
||||
| Aspekt | Live (`/api/homing/run`) | Offline (diese API) |
|
||||
|---|---|---|
|
||||
| Bilder | WebCam-Service liefert auf Abruf | Caller liefert im Request |
|
||||
| NPZ | Server sucht neueste Session in `data/calibration/` | Caller liefert im Request |
|
||||
| `robot.json` | Server nutzt `robotConfig.js` (Driver oder lokale Cache-Datei) | Caller liefert im Request |
|
||||
| Pipeline (1→2→3b→4b→5) | identisch | identisch |
|
||||
| Antwort | SSE-Stream während Lauf | Synchrones JSON mit allen Ergebnisdateien |
|
||||
| Zweck | Produktions-Homing | Simulation / Replay |
|
||||
|
||||
Die Pipeline-Skripte (`1_detect_aruco_observations.py` bis `5_pose_estimation.py`)
|
||||
werden **unverändert** aufgerufen — die Offline-API ist nur eine andere Eingangsschicht.
|
||||
Neue Algorithmen erscheinen automatisch in beiden Pfaden.
|
||||
|
||||
---
|
||||
|
||||
## API-Beschreibung
|
||||
|
||||
### `POST /api/homing/run-offline`
|
||||
|
||||
**Content-Type:** `multipart/form-data`
|
||||
|
||||
#### Felder
|
||||
|
||||
| Feld | Typ | Pflicht | Beschreibung |
|
||||
|---|---|---|---|
|
||||
| `images` | Datei(en), `image/jpeg` | ja | Ein oder mehrere JPEG-Bilder. Dateiname **muss** `{cameraId}.jpg` sein (z. B. `cam0.jpg`, `cam1.jpg`). |
|
||||
| `calibrations` | Datei(en), `application/octet-stream` | ja | Je eine `.npz`-Datei pro Kamera. Dateiname **muss** `{cameraId}_calibration.npz` sein (z. B. `cam0_calibration.npz`). |
|
||||
| `robot` | Datei, `application/json` | ja | `robot.json` für diesen Lauf. Wird nur für diesen Aufruf verwendet, nicht dauerhaft gespeichert. |
|
||||
| `refSet` | Text | nein | Optionaler Referenz-Set-Name für Script 2 (`--refSet`), z. B. `A0`. |
|
||||
|
||||
Das Pairing `{cameraId}.jpg` ↔ `{cameraId}_calibration.npz` erfolgt rein über den
|
||||
Dateinamen-Prefix vor dem ersten `_` bzw. vor `.jpg`. Kamera-IDs ohne passende NPZ
|
||||
werden wie im Live-Modus übersprungen (Log-Eintrag, kein Fehler).
|
||||
|
||||
#### Antwort (Erfolg): `200 OK`
|
||||
|
||||
```json
|
||||
{
|
||||
"ok": true,
|
||||
"runDir": "20260616_183042",
|
||||
"state": {
|
||||
"x": 312.4,
|
||||
"y": 44.8,
|
||||
"z": -12.1,
|
||||
"a": 7.3,
|
||||
"b": 0.0,
|
||||
"c": null,
|
||||
"e": null
|
||||
},
|
||||
"files": {
|
||||
"aruco_marker_poses.json": { /* Inhalt */ },
|
||||
"robot_state.json": { /* Inhalt */ },
|
||||
"state_Arm1.json": { /* Inhalt */ },
|
||||
"state_Ellbow.json": { /* Inhalt */ },
|
||||
"state_Arm2.json": { /* Inhalt */ },
|
||||
"state_Hand.json": { /* Inhalt */ },
|
||||
"cam0_aruco_detection.json": { /* Inhalt */ },
|
||||
"cam0_camera_pose.json": { /* Inhalt */ },
|
||||
"cam1_aruco_detection.json": { /* Inhalt */ },
|
||||
"cam1_camera_pose.json": { /* Inhalt */ }
|
||||
},
|
||||
"log": [
|
||||
"▶ Homing-Run: 20260616_183042",
|
||||
"▶ cam0: 14 Marker erkannt",
|
||||
"…"
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
`state` entspricht dem `accumulated_state` aus der 4b-Kette (wenn erfolgreich) oder
|
||||
den `movements`-Werten aus `robot_state.json` (wenn 4b abbricht und 5_pose_estimation
|
||||
einspringt). `null`-Werte bedeuten: Gelenk nicht beobachtbar (wie im Live-Modus).
|
||||
|
||||
`files` enthält alle JSON-Ausgabedateien des Laufs als geparste Objekte — kein Base64,
|
||||
da es sich ausschliesslich um JSON handelt.
|
||||
|
||||
#### Antwort (Fehler)
|
||||
|
||||
| Code | Bedeutung |
|
||||
|---|---|
|
||||
| `400` | Pflichtfelder fehlen, Dateinamen-Convention verletzt, robot.json ungültig |
|
||||
| `422` | Pipeline abgebrochen: Script 3b konnte `aruco_marker_poses.json` nicht erzeugen (zu wenige Kameras o. ä.) |
|
||||
| `500` | Unerwarteter Server-Fehler (Python nicht gefunden, Datei-I/O-Fehler, …) |
|
||||
|
||||
Fehlerresponse immer `{ "error": "…", "log": ["…"] }`.
|
||||
|
||||
---
|
||||
|
||||
## Aufruf-Beispiele
|
||||
|
||||
### curl
|
||||
|
||||
```bash
|
||||
curl -X POST https://thinkcentre.local:2093/api/homing/run-offline \
|
||||
-F "images=@cam0.jpg" \
|
||||
-F "images=@cam1.jpg" \
|
||||
-F "images=@cam2.jpg" \
|
||||
-F "calibrations=@cam0_calibration.npz" \
|
||||
-F "calibrations=@cam1_calibration.npz" \
|
||||
-F "calibrations=@cam2_calibration.npz" \
|
||||
-F "robot=@robot_1781069752019.json;type=application/json" \
|
||||
-F "refSet=A0"
|
||||
```
|
||||
|
||||
Ohne `refSet` (alle Board-Marker als Referenz):
|
||||
```bash
|
||||
curl -X POST https://thinkcentre.local:2093/api/homing/run-offline \
|
||||
-F "images=@cam0.jpg" -F "images=@cam1.jpg" -F "images=@cam2.jpg" \
|
||||
-F "calibrations=@cam0_calibration.npz" -F "calibrations=@cam1_calibration.npz" \
|
||||
-F "calibrations=@cam2_calibration.npz" \
|
||||
-F "robot=@robot_1781069752019.json;type=application/json"
|
||||
```
|
||||
|
||||
### JavaScript / fetch (Browser oder Node)
|
||||
|
||||
```javascript
|
||||
const formData = new FormData();
|
||||
|
||||
// Bilder – Dateiname MUSS {cameraId}.jpg sein
|
||||
formData.append('images', cam0Blob, 'cam0.jpg');
|
||||
formData.append('images', cam1Blob, 'cam1.jpg');
|
||||
formData.append('images', cam2Blob, 'cam2.jpg');
|
||||
|
||||
// Kalibrierungen – Dateiname MUSS {cameraId}_calibration.npz sein
|
||||
formData.append('calibrations', cam0NpzBlob, 'cam0_calibration.npz');
|
||||
formData.append('calibrations', cam1NpzBlob, 'cam1_calibration.npz');
|
||||
formData.append('calibrations', cam2NpzBlob, 'cam2_calibration.npz');
|
||||
|
||||
// robot.json
|
||||
formData.append('robot', new Blob([JSON.stringify(robotJson)], { type: 'application/json' }), 'robot.json');
|
||||
|
||||
// optional
|
||||
formData.append('refSet', 'A0');
|
||||
|
||||
const res = await fetch('/api/homing/run-offline', { method: 'POST', body: formData });
|
||||
const data = await res.json();
|
||||
// data.state → { x, y, z, a, b, c, e }
|
||||
// data.files → { "aruco_marker_poses.json": {...}, "state_Arm1.json": {...}, … }
|
||||
// data.log → ["▶ Kameras: cam0, cam1, cam2", …]
|
||||
// data.runDir → "20260618_143022"
|
||||
```
|
||||
|
||||
### Python (requests)
|
||||
|
||||
```python
|
||||
import requests
|
||||
|
||||
url = 'https://thinkcentre.local:2093/api/homing/run-offline'
|
||||
|
||||
files = [
|
||||
('images', ('cam0.jpg', open('cam0.jpg', 'rb'), 'image/jpeg')),
|
||||
('images', ('cam1.jpg', open('cam1.jpg', 'rb'), 'image/jpeg')),
|
||||
('images', ('cam2.jpg', open('cam2.jpg', 'rb'), 'image/jpeg')),
|
||||
('calibrations', ('cam0_calibration.npz', open('cam0_calibration.npz', 'rb'), 'application/octet-stream')),
|
||||
('calibrations', ('cam1_calibration.npz', open('cam1_calibration.npz', 'rb'), 'application/octet-stream')),
|
||||
('calibrations', ('cam2_calibration.npz', open('cam2_calibration.npz', 'rb'), 'application/octet-stream')),
|
||||
('robot', ('robot.json', open('robot.json', 'rb'), 'application/json')),
|
||||
]
|
||||
data = {'refSet': 'A0'} # optional
|
||||
|
||||
resp = requests.post(url, files=files, data=data, verify=False, timeout=120)
|
||||
result = resp.json()
|
||||
print(result['state']) # {'x': 312.4, 'y': 44.8, 'z': -12.1, ...}
|
||||
```
|
||||
|
||||
### Wichtige Regeln für Dateinamen
|
||||
|
||||
| Feld | Pflichtformat | Beispiel |
|
||||
|------|--------------|---------|
|
||||
| `images` | `{cameraId}.jpg` | `cam0.jpg`, `cam1.jpg` |
|
||||
| `calibrations` | `{cameraId}_calibration.npz` | `cam0_calibration.npz` |
|
||||
| `robot` | beliebig (wird intern als `robot_run.json` gespeichert) | `robot.json` |
|
||||
|
||||
Das Pairing Bild ↔ NPZ erfolgt rein über den `cameraId`-Prefix.
|
||||
Eine Kamera ohne passende NPZ wird übersprungen (kein Fehler, aber Log-Eintrag).
|
||||
|
||||
### HTTP-Statuscodes
|
||||
|
||||
| Code | Bedeutung |
|
||||
|------|-----------|
|
||||
| `200` | Erfolg: `{ ok: true, runDir, state, files, log }` |
|
||||
| `400` | Pflichtfelder fehlen oder `robot.json` ist kein valides JSON |
|
||||
| `422` | Pipeline abgebrochen: `aruco_marker_poses.json` nicht erzeugt (< 2 Kamera-Posen) |
|
||||
| `500` | Homing fehlgeschlagen (4b-Kette + Fallback gescheitert) |
|
||||
|
||||
### Timeout-Hinweis
|
||||
|
||||
Die Pipeline kann 20–60 s dauern. Client-Timeout auf **≥ 120 s** setzen
|
||||
(curl: `--max-time 120`, requests: `timeout=120`, nginx/Caddy: `proxy_read_timeout 120s`).
|
||||
|
||||
---
|
||||
|
||||
## Datenfluss (detailliert)
|
||||
|
||||
```
|
||||
multipart/form-data
|
||||
images: cam0.jpg, cam1.jpg
|
||||
calibrations: cam0_calibration.npz, cam1_calibration.npz
|
||||
robot: robot_xxx.json
|
||||
│
|
||||
▼
|
||||
Server: Temp-Verzeichnis anlegen data/homing-offline/{timestamp}/
|
||||
→ Bilder speichern: cam0.jpg, cam1.jpg
|
||||
→ NPZ speichern: cam0_calibration.npz, cam1_calibration.npz
|
||||
→ robot.json speichern: robot_run.json (nur für diesen Lauf)
|
||||
│
|
||||
▼ für jede Kamera:
|
||||
1_detect_aruco_observations.py
|
||||
-i cam0.jpg -npz cam0_calibration.npz -robot robot_run.json
|
||||
-cameraId cam0 -outDir {runDir}
|
||||
→ cam0_aruco_detection.json
|
||||
|
||||
2_estimate_camera_from_observations.py
|
||||
-i cam0_aruco_detection.json -robot robot_run.json
|
||||
-outDir {runDir} [--refSet A0]
|
||||
→ cam0_camera_pose.json
|
||||
│
|
||||
▼ nach allen Kameras:
|
||||
3b_corner_marker_poses.py
|
||||
--evalDir {runDir} --robot robot_run.json
|
||||
→ aruco_marker_poses.json
|
||||
│
|
||||
▼ X-Position schätzen (homingXEstimate)
|
||||
x_mm ← estimateXFromMarkers(aruco_marker_poses.json, robot_run.json)
|
||||
│
|
||||
▼ 4b-Kette sequenziell Arm1→Ellbow→Arm2→Hand:
|
||||
4b_revolute_angle.py
|
||||
--robot robot_run.json --aruco aruco_marker_poses.json
|
||||
--link Arm1 --x-mm {x_mm} --output state_Arm1.json
|
||||
4b_revolute_angle.py --link Ellbow --from-state state_Arm1.json …
|
||||
…
|
||||
→ state_Arm1.json, state_Ellbow.json, state_Arm2.json, state_Hand.json
|
||||
│
|
||||
▼ (falls 4b-Kette vollständig)
|
||||
accumulated_state → state in der Antwort
|
||||
│
|
||||
▼ (Verfeinerung / Fallback bei abgebrochener 4b-Kette)
|
||||
5_pose_estimation.py
|
||||
aruco_marker_poses.json -robot robot_run.json
|
||||
--from-state state_Hand.json -out robot_state.json
|
||||
→ robot_state.json
|
||||
│
|
||||
▼
|
||||
Antwort: { ok, state, files: { alle JSON-Dateien }, log }
|
||||
```
|
||||
|
||||
Der Ablauf ist **identisch** zu `runHoming()` in `homingOrchestrator.js`, mit dem
|
||||
einzigen Unterschied, dass `runBoardPipeline()` die Bilder nicht vom WebCam-Service
|
||||
holt, sondern aus dem vorab befüllten `{runDir}`.
|
||||
|
||||
---
|
||||
|
||||
## Umsetzungsplan (abgeschlossen 2026-06-18)
|
||||
|
||||
### Schritt 1 — `runBoardPipelineOffline(runDir, send, opts)` (Kern)
|
||||
|
||||
**Was:** Neue Funktion in `server/server.js` oder `server/homingOrchestrator.js`, analog
|
||||
zu `runBoardPipeline()`. Unterschied: statt `WEBCAM_URL` und `findLatestNpzForCamera()`
|
||||
leitet sie die bereits-im-Verzeichnis-liegenden Dateien direkt weiter.
|
||||
|
||||
**Konkret:** Der einzige geänderte Teil in `runBoardPipeline()` ist die
|
||||
Kamera-Schleife — statt Snapshot + NPZ-Suche wird einfach geprüft, ob
|
||||
`{camId}.jpg` und `{camId}_calibration.npz` im `runDir` existieren. Danach
|
||||
rufts Script 1 und 2 identisch auf.
|
||||
|
||||
**Risiko:** keines — die Script-Aufrufe selbst bleiben unverändert. Nur der Pfad zur
|
||||
NPZ ändert sich von `data/calibration/{session}/` zu `{runDir}/`.
|
||||
|
||||
**Testbar:** Einzel-Test mit einer Kamera, geprüft, ob `cam0_aruco_detection.json`
|
||||
korrekt erzeugt wird.
|
||||
|
||||
---
|
||||
|
||||
### Schritt 2 — Multipart-Upload-Handling
|
||||
|
||||
**Was:** Parsing von `multipart/form-data` für den neuen Endpoint. Express verarbeitet
|
||||
`application/json` und `urlencoded` nativ, aber nicht `multipart`. Benötigt entweder:
|
||||
|
||||
- **Option A** — `multer` (npm-Package), etabliert, 0 Boilerplate
|
||||
- **Option B** — manuell mit dem Node `busboy`-Parser (bereits in Node 18+, kein
|
||||
Extra-Package)
|
||||
|
||||
Empfehlung: `multer` — es ist bereits `multer` in vielen Express-Projekten Standard,
|
||||
klar dokumentiert, und `diskStorage` legt Dateien direkt in `{runDir}` ab. Vermeidet
|
||||
Buffer-Accumulation für grosse NPZ-Dateien.
|
||||
|
||||
**Risiko:** Dateinamen-Sanitising — Multer übergibt den Originaldateinamen. Vor dem
|
||||
Speichern: `path.basename()` und nur Zeichen `[a-zA-Z0-9_.-]` zulassen, sonst 400.
|
||||
|
||||
**Testbar:** curl-Upload-Test mit zwei kleinen Dummy-Dateien, geprüft, ob sie im
|
||||
richtigen Verzeichnis landen.
|
||||
|
||||
---
|
||||
|
||||
### Schritt 3 — `runHomingOffline()` Orchestrator
|
||||
|
||||
**Was:** Analoge Funktion zu `runHoming()` in `homingOrchestrator.js`, jedoch:
|
||||
- kein SSE-Stream, sondern Log-Akkumulation in ein Array
|
||||
- `runBoardPipelineOffline()` statt `runBoardPipeline()`
|
||||
- `robotJsonPath` zeigt auf die hochgeladene, temporäre `robot_run.json`
|
||||
- Rückgabewert: `{ state, files, log }` statt SSE-Events
|
||||
|
||||
Die 4b-Kette und der 5_pose-Aufruf bleiben **unverändert** — gleiche Args, gleiche
|
||||
Exit-Code-Logik, gleiche `accumulated_state`-Extraktion.
|
||||
|
||||
**Risiko:** 5_pose_estimation.py braucht `scipy`. In der lokalen Entwicklungsumgebung
|
||||
muss `scipy` im Python-Umfeld vorhanden sein (in `docker-compose.yaml` ist es bereits
|
||||
eingetragen, lokal muss `pip install scipy` geprüft werden).
|
||||
|
||||
---
|
||||
|
||||
### Schritt 4 — Endpoint `POST /api/homing/run-offline`
|
||||
|
||||
**Was:** Express-Route in `server.js`:
|
||||
1. Multer-Middleware: Dateien in Temp-Verzeichnis
|
||||
2. Validierung: mindestens 1 Kamera mit passendem `.jpg` + `.npz`-Pair; `robot`-Feld vorhanden
|
||||
3. `runHomingOffline()` aufrufen
|
||||
4. Alle JSON-Dateien aus `{runDir}` einlesen und in `files`-Objekt verpacken
|
||||
5. Temp-Verzeichnis aufräumen (oder unter `data/homing-offline/` für Replay behalten?)
|
||||
|
||||
**Aufräumen vs. Behalten:** Empfehlung — Verzeichnis behalten, wie bei Live-Homing-Runs.
|
||||
So ist Replay/Debug möglich. Ein periodischer Aufräum-Cron ist eine separate Aufgabe.
|
||||
|
||||
**Risiko:** Timeouts — bei vielen Kameras oder langsamer Maschine kann die Pipeline
|
||||
20–60 Sekunden dauern. Express hat kein Default-Timeout, aber ein vorgeschalteter
|
||||
Reverse-Proxy (nginx, Caddy) schon. In der Doku festhalten: Client soll Timeout ≥ 120 s
|
||||
setzen. Alternativ: SSE-Variante (gleiche Daten, aber inkrementell gestreamt).
|
||||
|
||||
---
|
||||
|
||||
### Schritt 5 — Aufräumen temporärer `robot.json`
|
||||
|
||||
**Was:** Die hochgeladene `robot.json` wird nur für diesen Lauf im `{runDir}` als
|
||||
`robot_run.json` gespeichert. Sie liegt **nicht** an der Stelle von `robotCachePath` (aus `robotConfig.js`)
|
||||
und wird daher nie vom Live-Modus versehentlich gelesen. Kein Konflikt.
|
||||
|
||||
**Risiko:** keines — rein additive Datei in einem neuen Verzeichnis.
|
||||
|
||||
---
|
||||
|
||||
### Testplan
|
||||
|
||||
| Test | Was wird geprüft | Werkzeug |
|
||||
|---|---|---|
|
||||
| **Smoke-Test Upload** | Endpoint antwortet 200, `runDir` im Response vorhanden | curl mit zwei Dummy-JPEGs + NPZs + robot.json |
|
||||
| **Kamera-Pairing** | `.jpg` ohne passende NPZ wird übersprungen (kein 500) | curl mit fehlendem NPZ |
|
||||
| **Dateinamen-Sanitising** | `../../../evil.npz` → 400 | curl mit bösem Dateinamen |
|
||||
| **Simulations-Roundtrip** | `appRobotRendering` rendert 10 Posen mit bekannten GT-Winkeln, API gibt `state` zurück, Abweichung < Toleranz | automatisiert aus `appRobotRendering`-Pipeline |
|
||||
| **Identität Live vs. Offline** | Denselben Bildsatz einmal per Live-Run und einmal per Offline-API auswerten → `state`-Differenz ≈ 0 | manuell mit aufgezeichnetem Homing-Run |
|
||||
| **Fallback-Pfad** | Script 3b schlägt fehl (< 2 Kameras) → 422 mit log | curl mit nur einem Bild |
|
||||
| **Timeout-Robustheit** | 4b bricht ab → 5_pose_estimation greift ein, 200 wird trotzdem zurückgegeben | simulierter Abbruch durch fehlende Marker |
|
||||
|
||||
---
|
||||
|
||||
## Bekannte Risiken und Probleme
|
||||
|
||||
### Dateinamen-Convention ist implizit
|
||||
|
||||
Die Kamera-ID wird aus dem Dateinamen abgeleitet — kein explizites Metadaten-Feld.
|
||||
Das funktioniert solange die Namenskonvention (`{cameraId}.jpg` / `{cameraId}_calibration.npz`)
|
||||
eingehalten wird. Abweichungen (z. B. `frame_cam0_001.jpg`) führen zu einem ungematchten
|
||||
Bild, das still übersprungen wird — kein Fehler, aber unerwartetes Verhalten.
|
||||
|
||||
**Mitigation:** Explizite Validierung und Fehlermeldung wenn kein Match gefunden wird.
|
||||
Alternativ: Metadaten-Feld `cameraMappings: {"cam0": {"image": "frame_cam0_001.jpg", "npz": "cam0_calibration.npz"}}`.
|
||||
Für den Simulations-Use-Case ist die einfache Konvention ausreichend.
|
||||
|
||||
### robot.json-Versionskonflikt
|
||||
|
||||
Simulation und Live-Homing teilen sich `robot.json` konzeptionell, aber die Datei
|
||||
entwickelt sich weiter (Kalibrierung, neue Marker, geänderte Positionen). Eine veraltete
|
||||
`robot.json` aus `appRobotRendering` kann zu systematisch falschen Posen führen, die
|
||||
schwer von Algorithmus-Fehlern zu unterscheiden sind.
|
||||
|
||||
**Mitigation:** Im Simulations-Roundtrip-Test die `robot.json`-Version (z. B. Timestamp
|
||||
im Dateinamen) protokollieren und mit dem API-Response abgleichen.
|
||||
|
||||
### Gleichzeitige Anfragen
|
||||
|
||||
Mehrere Offline-Runs gleichzeitig schreiben in separate `{timestamp}`-Verzeichnisse —
|
||||
kein Konflikt. Aber: Python-Subprozesse multiplizieren sich. Bei parallelen Requests aus
|
||||
der Simulations-Pipeline könnte die CPU-Last (scipy + ArUco) den Server blockieren.
|
||||
|
||||
**Mitigation (optional):** Request-Queue mit max. 1–2 parallelen Runs. Für den
|
||||
Simulations-Use-Case wird sequenzieller Aufruf empfohlen.
|
||||
|
||||
### scipy-Abhängigkeit auf Deployment-Maschine
|
||||
|
||||
`5_pose_estimation.py` braucht `scipy`. In `docker-compose.yaml` ist es vorhanden.
|
||||
Lokal (Entwicklung ohne Docker) muss `pip install scipy` sichergestellt sein, sonst
|
||||
schlägt Schritt 5 stumm fehl (Exit 1, aber Schritt 4 hat bereits ein Ergebnis).
|
||||
|
||||
### Festplattenverbrauch
|
||||
|
||||
Jeder Offline-Run erzeugt ein Verzeichnis mit JPEGs + NPZs + ca. 10 JSON-Dateien.
|
||||
Bei intensiver Simulations-Nutzung (100 Posen/Tag) kann das summieren.
|
||||
|
||||
**Mitigation:** TTL-basiertes Aufräumen als gelegentliche Wartungsaufgabe (kein Teil
|
||||
dieser Implementierung).
|
||||
|
||||
---
|
||||
|
||||
## Offene Entscheidungen (getroffen)
|
||||
|
||||
- [x] `multer` (v2.2.0) — installiert, `diskStorage` schreibt direkt in `{runDir}`
|
||||
- [x] Offline-Runs in `data/homing-offline/` (eigenes Verzeichnis, nicht im boardViewer)
|
||||
- [x] Synchrone Antwort — Pipeline läuft durch, dann JSON; Client-Timeout ≥ 120 s empfohlen
|
||||
- [x] `5_pose_estimation.py` nur als Fallback (identisch zum Live-Modus)
|
||||
|
||||
---
|
||||
|
||||
## Abhängigkeiten und Voraussetzungen
|
||||
|
||||
Vor der Implementierung müssen vorhanden sein:
|
||||
|
||||
- `multer` installiert (oder busboy-basierte Alternative entschieden)
|
||||
- `scipy` im Python-Environment verfügbar (lokal + Docker)
|
||||
- `scripts/5_pose_estimation.py` + `scripts/robot_fk.py` im Repo (beide vorhanden, ✅)
|
||||
- `homingOrchestrator.js` für Orientierung (vorhanden, ✅)
|
||||
- Test-Bilder + Test-NPZs für automatisierten Smoke-Test (aus `test/homing/` oder
|
||||
`test/y-axis-finder-examples/`; die NPZs müssen dazu noch als Testfixtures bereitgestellt werden)
|
||||
|
||||
---
|
||||
|
||||
## Verweise
|
||||
|
||||
- [`Homing.md`](Homing.md) — Gesamtüberblick Homing-Ablauf
|
||||
- [`Homing_0_Camera.md`](Homing_0_Camera.md) — Schritte 1–3b (Board-Pipeline)
|
||||
- [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md) — 4b-Kette (Gelenkwinkel-Schätzung)
|
||||
- [`Homing_5_Pose.md`](Homing_5_Pose.md) — 5_pose_estimation.py (Bundle-Adjustment)
|
||||
- `server/server.js` — bestehende Endpoints (`/api/homing/run`, `runBoardPipeline()`)
|
||||
- `server/homingOrchestrator.js` — `runHoming()`, Vorlage für `runHomingOffline()`
|
||||
@@ -11,6 +11,15 @@ services:
|
||||
- PYTHON_BIN=python3
|
||||
- WEBCAM_URL=http://host.docker.internal:8444
|
||||
- BODYTRACKER_URL=http://host.docker.internal:8446
|
||||
# Driver-WebSocket (Plain-Text-G-Code, self-signed). Homing sendet G92
|
||||
# hierhin (= Motorposition setzen ohne Bewegung).
|
||||
# WICHTIG: Der Input-WS lauscht container-intern auf 2095 (startRobot.js:
|
||||
# PORT||2095) und ist NICHT per host-port veröffentlicht. Die Driver-Ports
|
||||
# 2081 (Node --inspect) und 2098 (Info/Status) sind NICHT dieser WS.
|
||||
# Variante A (robust): beide Container im selben Netz (approbots), per Name:
|
||||
- DRIVER_WS_URL=wss://appRobot_Driver:2095
|
||||
# Variante B (über Host): im Driver `- "2095:2095"` veröffentlichen, dann
|
||||
# DRIVER_WS_URL=wss://host.docker.internal:2095
|
||||
extra_hosts:
|
||||
# Macht host.docker.internal auf Linux verfügbar (Standard auf macOS/Windows)
|
||||
- "host.docker.internal:host-gateway"
|
||||
|
||||
98
package-lock.json
generated
98
package-lock.json
generated
@@ -9,7 +9,8 @@
|
||||
"version": "0.1.0",
|
||||
"dependencies": {
|
||||
"dotenv": "^16.4.5",
|
||||
"express": "^4.19.2"
|
||||
"express": "^4.19.2",
|
||||
"multer": "^2.2.0"
|
||||
},
|
||||
"devDependencies": {
|
||||
"jest": "^29.7.0",
|
||||
@@ -1256,6 +1257,12 @@
|
||||
"node": ">= 8"
|
||||
}
|
||||
},
|
||||
"node_modules/append-field": {
|
||||
"version": "1.0.0",
|
||||
"resolved": "https://registry.npmjs.org/append-field/-/append-field-1.0.0.tgz",
|
||||
"integrity": "sha512-klpgFSWLW1ZEs8svjfb7g4qWY0YS5imI82dTg+QahUvJ8YqAY0P10Uk8tTyh9ZGuYEZEMaeJYCF5BFuX552hsw==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/argparse": {
|
||||
"version": "1.0.10",
|
||||
"resolved": "https://registry.npmjs.org/argparse/-/argparse-1.0.10.tgz",
|
||||
@@ -1524,9 +1531,19 @@
|
||||
"version": "1.1.2",
|
||||
"resolved": "https://registry.npmjs.org/buffer-from/-/buffer-from-1.1.2.tgz",
|
||||
"integrity": "sha512-E+XQCRwSbaaiChtv6k6Dwgc+bx+Bs6vuKJHHl5kox/BaKbhiXzqQOwK4cO22yElGp2OCmjwVhT3HmxgyPGnJfQ==",
|
||||
"dev": true,
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/busboy": {
|
||||
"version": "1.6.0",
|
||||
"resolved": "https://registry.npmjs.org/busboy/-/busboy-1.6.0.tgz",
|
||||
"integrity": "sha512-8SFQbg/0hQ9xy3UNTB0YEnsNBbWfhf7RtnzpL7TkBiTBRfrQ9Fxcnz7VJsleJpyp6rVLvXiuORqjlHi5q+PYuA==",
|
||||
"dependencies": {
|
||||
"streamsearch": "^1.1.0"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">=10.16.0"
|
||||
}
|
||||
},
|
||||
"node_modules/bytes": {
|
||||
"version": "3.1.2",
|
||||
"resolved": "https://registry.npmjs.org/bytes/-/bytes-3.1.2.tgz",
|
||||
@@ -1754,6 +1771,21 @@
|
||||
"dev": true,
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/concat-stream": {
|
||||
"version": "2.0.0",
|
||||
"resolved": "https://registry.npmjs.org/concat-stream/-/concat-stream-2.0.0.tgz",
|
||||
"integrity": "sha512-MWufYdFw53ccGjCA+Ol7XJYpAlW6/prSMzuPOTRnJGcGzuhLn4Scrz7qf6o8bROZ514ltazcIFJZevcfbo0x7A==",
|
||||
"engines": [
|
||||
"node >= 6.0"
|
||||
],
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"buffer-from": "^1.0.0",
|
||||
"inherits": "^2.0.3",
|
||||
"readable-stream": "^3.0.2",
|
||||
"typedarray": "^0.0.6"
|
||||
}
|
||||
},
|
||||
"node_modules/content-disposition": {
|
||||
"version": "0.5.4",
|
||||
"resolved": "https://registry.npmjs.org/content-disposition/-/content-disposition-0.5.4.tgz",
|
||||
@@ -3978,6 +4010,25 @@
|
||||
"integrity": "sha512-Tpp60P6IUJDTuOq/5Z8cdskzJujfwqfOTkrwIwj7IRISpnkJnT6SyJ4PCPnGMoFjC9ddhal5KVIYtAt97ix05A==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/multer": {
|
||||
"version": "2.2.0",
|
||||
"resolved": "https://registry.npmjs.org/multer/-/multer-2.2.0.tgz",
|
||||
"integrity": "sha512-6rdyFg2kLrMh9Jee7/BMPuV9lEAd7lLW2YUpF9/YxR7njyoUwwQ0ZPh3TaIY50Sw6vlyD2HW3wGOkTS4P79xrQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"append-field": "^1.0.0",
|
||||
"busboy": "^1.6.0",
|
||||
"concat-stream": "^2.0.0",
|
||||
"type-is": "^1.6.18"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">= 10.16.0"
|
||||
},
|
||||
"funding": {
|
||||
"type": "opencollective",
|
||||
"url": "https://opencollective.com/express"
|
||||
}
|
||||
},
|
||||
"node_modules/natural-compare": {
|
||||
"version": "1.4.0",
|
||||
"resolved": "https://registry.npmjs.org/natural-compare/-/natural-compare-1.4.0.tgz",
|
||||
@@ -4554,6 +4605,20 @@
|
||||
"dev": true,
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/readable-stream": {
|
||||
"version": "3.6.2",
|
||||
"resolved": "https://registry.npmjs.org/readable-stream/-/readable-stream-3.6.2.tgz",
|
||||
"integrity": "sha512-9u/sniCrY3D5WdsERHzHE4G2YCXqoG5FTHUiCC4SIbr6XcLZBY05ya9EKjYek9O5xOAwjGq+1JdGBAS7Q9ScoA==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"inherits": "^2.0.3",
|
||||
"string_decoder": "^1.1.1",
|
||||
"util-deprecate": "^1.0.1"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">= 6"
|
||||
}
|
||||
},
|
||||
"node_modules/readdirp": {
|
||||
"version": "3.6.0",
|
||||
"resolved": "https://registry.npmjs.org/readdirp/-/readdirp-3.6.0.tgz",
|
||||
@@ -4934,6 +4999,23 @@
|
||||
"node": ">= 0.8"
|
||||
}
|
||||
},
|
||||
"node_modules/streamsearch": {
|
||||
"version": "1.1.0",
|
||||
"resolved": "https://registry.npmjs.org/streamsearch/-/streamsearch-1.1.0.tgz",
|
||||
"integrity": "sha512-Mcc5wHehp9aXz1ax6bZUyY5afg9u2rv5cqQI3mRrYkGC8rW2hM02jWuwjtL++LS5qinSyhj2QfLyNsuc+VsExg==",
|
||||
"engines": {
|
||||
"node": ">=10.0.0"
|
||||
}
|
||||
},
|
||||
"node_modules/string_decoder": {
|
||||
"version": "1.3.0",
|
||||
"resolved": "https://registry.npmjs.org/string_decoder/-/string_decoder-1.3.0.tgz",
|
||||
"integrity": "sha512-hkRX8U1WjJFd8LsDJ2yQ/wWWxaopEsABU1XfkM8A+j0+85JAGppt16cr1Whg6KIbb4okU6Mql6BOj+uup/wKeA==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"safe-buffer": "~5.2.0"
|
||||
}
|
||||
},
|
||||
"node_modules/string-length": {
|
||||
"version": "4.0.2",
|
||||
"resolved": "https://registry.npmjs.org/string-length/-/string-length-4.0.2.tgz",
|
||||
@@ -5161,6 +5243,12 @@
|
||||
"node": ">= 0.6"
|
||||
}
|
||||
},
|
||||
"node_modules/typedarray": {
|
||||
"version": "0.0.6",
|
||||
"resolved": "https://registry.npmjs.org/typedarray/-/typedarray-0.0.6.tgz",
|
||||
"integrity": "sha512-/aCDEGatGvZ2BIk+HmLf4ifCJFwvKFNb9/JeZPMulfgFracn9QFcAf5GO8B/mweUjSoblS5In0cWhqpfs/5PQA==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/undefsafe": {
|
||||
"version": "2.0.5",
|
||||
"resolved": "https://registry.npmjs.org/undefsafe/-/undefsafe-2.0.5.tgz",
|
||||
@@ -5236,6 +5324,12 @@
|
||||
"requires-port": "^1.0.0"
|
||||
}
|
||||
},
|
||||
"node_modules/util-deprecate": {
|
||||
"version": "1.0.2",
|
||||
"resolved": "https://registry.npmjs.org/util-deprecate/-/util-deprecate-1.0.2.tgz",
|
||||
"integrity": "sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/utils-merge": {
|
||||
"version": "1.0.1",
|
||||
"resolved": "https://registry.npmjs.org/utils-merge/-/utils-merge-1.0.1.tgz",
|
||||
|
||||
@@ -15,7 +15,9 @@
|
||||
},
|
||||
"dependencies": {
|
||||
"dotenv": "^16.4.5",
|
||||
"express": "^4.19.2"
|
||||
"express": "^4.19.2",
|
||||
"multer": "^2.2.0",
|
||||
"ws": "^8.20.0"
|
||||
},
|
||||
"devDependencies": {
|
||||
"jest": "^29.7.0",
|
||||
|
||||
@@ -216,6 +216,16 @@ function r2v(rx, ry, rz) { return new THREE.Vector3(rx * S, rz * S, -ry * S
|
||||
function r2vArr([rx, ry, rz]) { return r2v(rx, ry, rz); }
|
||||
function r2dir([dx, dy, dz]) { return new THREE.Vector3(dx, dz, -dy).normalize(); }
|
||||
|
||||
/**
|
||||
* True wenn `arr` ein nutzbares [x,y,z] ist (z. B. position_mm). Marker, die
|
||||
* 3b nicht triangulieren konnte (z. B. nur 1 Kamera), haben dieses Feld nicht
|
||||
* — solche Marker werden an allen Stellen, die diese Funktion nutzen, einfach
|
||||
* ignoriert statt einen Crash auszulösen.
|
||||
*/
|
||||
function hasXYZ(arr) {
|
||||
return Array.isArray(arr) && arr.length >= 3 && arr.slice(0, 3).every(Number.isFinite);
|
||||
}
|
||||
|
||||
// ── Renderer ──────────────────────────────────────────────────────────────────
|
||||
const canvas = document.getElementById('cv');
|
||||
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
@@ -357,20 +367,24 @@ function buildSkeletonFK(robot, angles) {
|
||||
const markerSizeM = (m.size ?? 25) * S;
|
||||
const [nx, ny, nz] = m.normal ?? [0, 0, 1];
|
||||
|
||||
// Marker-Orientierung ZUERST im lokalen Link-Frame bauen, DANN die volle
|
||||
// childFrame-Rotation anwenden. So wird der Roll (Drehung des Markers um
|
||||
// seine eigene Normale) korrekt mitgeführt — auch wenn die Link-Drehachse
|
||||
// parallel zur Marker-Normale liegt (z.B. Marker 197: normal [-1,0,0] ∥
|
||||
// Arm1-Achse [-1,0,0]). Eine reine Welt-Normalen-Rekonstruktion würde
|
||||
// genau diesen Anteil verlieren.
|
||||
const nLocal = new THREE.Vector3(nx, nz, -ny).normalize(); // robot→three.js
|
||||
// Marker-Orientierung ZUERST im lokalen ROBOT-Frame bauen (rohe Normale:
|
||||
// Minimal-Rotation [0,0,1]→Normale + Spin um die Normale), DANN über qView
|
||||
// in three.js-Achsen und mit qFrame in die Welt drehen. Würde man die
|
||||
// Normale wie früher schon VOR der Minimal-Rotation nach three.js drehen
|
||||
// (nx,nz,-ny), verdreht eine schräg liegende Normale (z.B. [-1,0,1]) das
|
||||
// Quadrat zusätzlich um ihren Azimut (~45°) um die eigene Achse; der Spin
|
||||
// kann das nicht kompensieren. Der Link-Roll um die Normale bleibt
|
||||
// erhalten, weil qFrame zuletzt wirkt (z.B. Marker 197: normal [-1,0,0] ∥
|
||||
// Arm1-Achse). Gegen triangulierte Ecken geprüft (Capture 20260616_133151,
|
||||
// Marker 146): diese Reihenfolge 0.8°, die alte 45.5°.
|
||||
const nRobot = new THREE.Vector3(nx, ny, nz).normalize(); // rohe robot-Normale
|
||||
const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
|
||||
const qNormalLoc = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLocal);
|
||||
const qSpinLoc = new THREE.Quaternion().setFromAxisAngle(nLocal, spinRad);
|
||||
const qMarkerLoc = qSpinLoc.multiply(qNormalLoc); // Q_spin ∘ Q_normal (lokal)
|
||||
const qNormalLoc = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nRobot);
|
||||
const qSpinLoc = new THREE.Quaternion().setFromAxisAngle(nRobot, spinRad);
|
||||
const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2); // robot→three.js
|
||||
const qFrame = new THREE.Quaternion().setFromRotationMatrix(childFrame);
|
||||
const qMarkerW = qFrame.clone().multiply(qMarkerLoc); // in Welt drehen
|
||||
const normalW = nLocal.clone().applyQuaternion(qFrame).normalize();
|
||||
const qMarkerW = qFrame.clone().multiply(qView).multiply(qSpinLoc.multiply(qNormalLoc)); // lokal → three.js → Welt
|
||||
const normalW = new THREE.Vector3(nx, nz, -ny).applyQuaternion(qFrame).normalize();
|
||||
|
||||
// P1: orientiertes Quadrat (Normale + Roll + Spin in einem Quaternion).
|
||||
// PlaneGeometry hat nativ die +Z-Normale, qMarkerW dreht +Z auf die
|
||||
@@ -403,6 +417,7 @@ function buildSkeletonFK(robot, angles) {
|
||||
}
|
||||
|
||||
for (const obs of _measuredMarkers.markers) {
|
||||
if (!hasXYZ(obs.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
|
||||
const obsLink = (obs.link && obs.link !== 'Board')
|
||||
? obs.link
|
||||
: markerIdToLink[obs.marker_id];
|
||||
@@ -602,6 +617,7 @@ function buildScene(data) {
|
||||
);
|
||||
|
||||
for (const m of a0markers) {
|
||||
if (!hasXYZ(m.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
|
||||
nTriangulated++;
|
||||
// Kein künstlicher Offset – Kugelmittelpunkt exakt an triangulierter Position
|
||||
const mpos = r2vArr(m.position_mm);
|
||||
@@ -625,6 +641,7 @@ function buildScene(data) {
|
||||
!boardMarkers.some(bm => bm.id === m.marker_id)
|
||||
);
|
||||
for (const m of unknownTriangulated) {
|
||||
if (!hasXYZ(m.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
|
||||
nUnknown++;
|
||||
const mpos = r2vArr(m.position_mm);
|
||||
gMeasured.add(makeSphere(mpos, 0.0055, 0x3b82f6));
|
||||
@@ -733,9 +750,9 @@ function buildTable(data) {
|
||||
let dist = null, dz = null, edge = null;
|
||||
let state = 'none'; // 'tri', '1cam', 'unk'
|
||||
|
||||
if (meas) {
|
||||
if (meas && hasXYZ(meas.position_mm)) {
|
||||
[x, y, z] = meas.position_mm;
|
||||
[nx, ny, nz] = meas.normal;
|
||||
if (Array.isArray(meas.normal)) [nx, ny, nz] = meas.normal;
|
||||
edge = meas.edge_length_mm;
|
||||
state = model ? 'tri' : 'unk';
|
||||
|
||||
@@ -745,7 +762,10 @@ function buildTable(data) {
|
||||
dist = Math.sqrt(dx*dx + dy*dy + ddz*ddz);
|
||||
dz = ddz;
|
||||
}
|
||||
} else if (cameras.length > 0) {
|
||||
} else if (cameras.length > 0 || meas) {
|
||||
// meas ohne position_mm (z.B. 1-Kamera-Marker, noch nicht trianguliert)
|
||||
// zählt wie "gesehen, aber nicht trianguliert" — gleicher Stand wie
|
||||
// cameras.length>0, nur eben aus 3b statt aus den rohen Detektionen.
|
||||
state = '1cam';
|
||||
}
|
||||
|
||||
@@ -1049,7 +1069,8 @@ async function loadData(specificRunDir = null) {
|
||||
buildTable(data);
|
||||
// Fremd-Marker für Verbindungslinien merken (Marker, die nicht in Board-Link stehen)
|
||||
const bIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
|
||||
_primaryFremdMarkers = (data.measuredMarkers?.markers ?? []).filter(m => !bIds.has(m.marker_id));
|
||||
_primaryFremdMarkers = (data.measuredMarkers?.markers ?? [])
|
||||
.filter(m => !bIds.has(m.marker_id) && hasXYZ(m.position_mm));
|
||||
const measTotal = data.measuredMarkers?.markers?.length ?? 0;
|
||||
vlog(`Basis: run=${data.runDir} gesamt=${measTotal} fremd=${_primaryFremdMarkers.length} boardIDs=${bIds.size}` +
|
||||
(_primaryFremdMarkers.length ? ` (${_primaryFremdMarkers.map(m => m.marker_id).join(' ')})` : ''));
|
||||
@@ -1087,7 +1108,7 @@ async function loadCompareData() {
|
||||
// Board-Marker-IDs aus Robot.json dieses Runs
|
||||
const boardIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
|
||||
for (const m of markers) {
|
||||
if (!boardIds.has(m.marker_id)) {
|
||||
if (!boardIds.has(m.marker_id) && hasXYZ(m.position_mm)) {
|
||||
_compareFremdMarkers.push(m); // für Linien
|
||||
gCompare.add(makeSphere(r2vArr(m.position_mm), 0.006, 0xf97316)); // orange Kugel
|
||||
}
|
||||
@@ -1120,7 +1141,7 @@ async function loadPositionC() {
|
||||
const markers = data.measuredMarkers?.markers ?? [];
|
||||
const boardIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
|
||||
for (const m of markers) {
|
||||
if (!boardIds.has(m.marker_id)) {
|
||||
if (!boardIds.has(m.marker_id) && hasXYZ(m.position_mm)) {
|
||||
_positionCFremdMarkers.push(m);
|
||||
gPositionC.add(makeSphere(r2vArr(m.position_mm), 0.006, 0x22d3ee)); // cyan
|
||||
}
|
||||
|
||||
@@ -346,11 +346,17 @@ function setHomingProgress(step, total, text) {
|
||||
if (txt) txt.textContent = text || `Schritt ${step} / ${total}`;
|
||||
}
|
||||
|
||||
// Schreibt das G92-Kommando ins Eingabefeld — nur die tatsächlich bestimmten
|
||||
// Achsen, identisch zu dem, was "An Roboter senden" via server/buildG92.cjs
|
||||
// sendet (fehlende/unbeobachtbare Achsen werden weggelassen, nicht 0-gefüllt).
|
||||
function writePartialGCode(state) {
|
||||
const axisMap = { x: 'X', y: 'Y', z: 'Z', a: 'A', b: 'B', c: 'C', e: 'E' };
|
||||
const parts = [];
|
||||
for (const [key, axis] of Object.entries(axisMap)) {
|
||||
if (state[key] != null) parts.push(`${axis}${Number(state[key]).toFixed(2)}`);
|
||||
const num = Number(state[key]);
|
||||
if (state[key] != null && Number.isFinite(num)) {
|
||||
parts.push(`${axis}${num.toFixed(2)}`);
|
||||
}
|
||||
}
|
||||
if (!parts.length) return;
|
||||
const el = document.getElementById('gcodePayload');
|
||||
@@ -549,6 +555,10 @@ async function runHoming() {
|
||||
if (evt.state) {
|
||||
_homingState = evt.state;
|
||||
showHomingResult(evt.state);
|
||||
// Finales G92 ins Feld — auch wenn der Lauf über den Fallback
|
||||
// (5_pose_estimation → analysis 'robot_state' statt 'state_*')
|
||||
// lief und progressiv kein G92 geschrieben wurde.
|
||||
writePartialGCode(evt.state);
|
||||
if (btnSend) {
|
||||
btnSend.disabled = false;
|
||||
btnSend.style.opacity = '';
|
||||
@@ -593,7 +603,8 @@ async function sendHomingToRobot() {
|
||||
});
|
||||
const data = await res.json();
|
||||
if (res.ok) {
|
||||
appendLog('✅ State erfolgreich an Roboter gesendet');
|
||||
appendLog(`✅ An Roboter gesendet: ${data.gcode ?? ''}`);
|
||||
if (data.note) appendLog(`ℹ ${data.note}`);
|
||||
setHomingStatus('✓ Gesendet', 'done');
|
||||
} else {
|
||||
appendLog(`❌ Fehler beim Senden: ${data.error ?? JSON.stringify(data)}`);
|
||||
@@ -605,6 +616,23 @@ async function sendHomingToRobot() {
|
||||
}
|
||||
}
|
||||
|
||||
// Transport für die G-Code-/Befehl-Buttons (data-cmd). Schickt eine rohe
|
||||
// Zeile über das Backend an den Driver-WebSocket (POST /api/robot/gcode).
|
||||
// Liegt ein Payload-Feld vor (z.B. das G92 aus #gcodePayload), wird dessen
|
||||
// Inhalt gesendet, sonst der cmd-Name selbst. Ersetzt den toten WSS-Altpfad.
|
||||
window.sendCommand = async function (cmd, payload) {
|
||||
const line = (payload && payload.trim()) ? payload.trim() : String(cmd ?? '').trim();
|
||||
if (!line) throw new Error('Leere Befehlszeile');
|
||||
const res = await fetch('/api/robot/gcode', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify({ line }),
|
||||
});
|
||||
const data = await res.json().catch(() => ({}));
|
||||
if (!res.ok) throw new Error(data.error ?? `HTTP ${res.status}`);
|
||||
return data;
|
||||
};
|
||||
|
||||
async function onCommandClick(btn) {
|
||||
const cmd = btn.dataset.cmd;
|
||||
const payloadSelector = btn.dataset.payload;
|
||||
|
||||
@@ -483,39 +483,23 @@ function transformDirByT(T, dir) {
|
||||
];
|
||||
}
|
||||
|
||||
function makeMarkerSquare(pos, normal, size, color) {
|
||||
// Marker-Quadrat mit vorab berechneter Orientierung (Quaternion). Die
|
||||
// BoxGeometry ist dünn in lokal-Z, ihre Normale ist also lokal +Z — quat
|
||||
// dreht +Z auf die Marker-Normale inkl. Link-Rotation und Spin.
|
||||
//
|
||||
// Die Orientierung MUSS im lokalen Link-Frame gebaut und erst danach in die
|
||||
// Szene gedreht werden (siehe Aufrufer). Würde man wie früher
|
||||
// setFromUnitVectors([0,0,1], welt_normale) NACH dem robot→three.js-
|
||||
// Achsentausch anwenden, verdreht eine schräg liegende Normale (z.B.
|
||||
// [-1,0,1]) das Quadrat zusätzlich um ihren Azimut (~45°) um die eigene
|
||||
// Achse, und der Spin fehlt ganz. Gegen triangulierte Ecken geprüft
|
||||
// (Capture 20260616_133151, Marker 146): lokale Variante 0.8°, alte 45.5°.
|
||||
function makeMarkerSquareQuat(pos, quat, size, color) {
|
||||
const geo = new THREE.BoxGeometry(size, size, size * 0.1);
|
||||
const mat = new THREE.MeshPhongMaterial({
|
||||
color,
|
||||
shininess: 40
|
||||
});
|
||||
|
||||
const mat = new THREE.MeshPhongMaterial({ color, shininess: 40 });
|
||||
const m = new THREE.Mesh(geo, mat);
|
||||
m.position.copy(pos);
|
||||
|
||||
// Fallback falls keine gültige Normale vorhanden
|
||||
let nx = 0, ny = 0, nz = 1;
|
||||
|
||||
if (Array.isArray(normal) && normal.length >= 3) {
|
||||
nx = Number(normal[0]) || 0;
|
||||
ny = Number(normal[1]) || 0;
|
||||
nz = Number(normal[2]) || 1;
|
||||
} else if (normal instanceof THREE.Vector3) {
|
||||
nx = normal.x;
|
||||
ny = normal.y;
|
||||
nz = normal.z;
|
||||
}
|
||||
|
||||
const n = new THREE.Vector3(nx, ny, nz);
|
||||
|
||||
if (n.lengthSq() > 1e-12) {
|
||||
n.normalize();
|
||||
m.quaternion.setFromUnitVectors(
|
||||
new THREE.Vector3(0, 0, 1),
|
||||
n
|
||||
);
|
||||
}
|
||||
|
||||
m.quaternion.copy(quat);
|
||||
return m;
|
||||
}
|
||||
|
||||
@@ -837,8 +821,19 @@ function rebuild() {
|
||||
// ── model markers + normals ──
|
||||
const modelPositions = {};
|
||||
const modelNormals = {};
|
||||
// robot→three.js view rotation (x,y,z)->(x,z,-y) == Rot_x(-90°). Applied LAST,
|
||||
// so the marker orientation can be built in the robot/link frame first.
|
||||
const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2);
|
||||
for (const [lname, ld] of Object.entries(links)) {
|
||||
const col = linkColor(lname);
|
||||
// link rotation in the robot frame (from FK), as a quaternion
|
||||
const Tl = T[lname] || I4();
|
||||
const qLink = new THREE.Quaternion().setFromRotationMatrix(new THREE.Matrix4().set(
|
||||
Tl[0], Tl[1], Tl[2], 0,
|
||||
Tl[4], Tl[5], Tl[6], 0,
|
||||
Tl[8], Tl[9], Tl[10], 0,
|
||||
0, 0, 0, 1
|
||||
));
|
||||
for (const m of (ld.markers||[])) {
|
||||
if (!m.position) continue;
|
||||
const mid = m.id;
|
||||
@@ -849,8 +844,16 @@ function rebuild() {
|
||||
modelPositions[mid] = wp;
|
||||
modelNormals[mid] = nWorld;
|
||||
|
||||
const sq = makeMarkerSquare(r2vArr(wp), r2vDir(...nWorld), 0.022, col);
|
||||
gModel.add(sq);
|
||||
// Orientierung ZUERST im lokalen Link-Frame (robot): Minimal-Rotation
|
||||
// [0,0,1]→Normale, dann Spin um diese Normale; DANN qLink (Link-Drehung)
|
||||
// und qView (in die Szene). So bleibt der Roll des Links um die Normale
|
||||
// erhalten und der Spin-Azimut-Twist entfällt (siehe makeMarkerSquareQuat).
|
||||
const nLR = new THREE.Vector3(nLocal[0], nLocal[1], nLocal[2]).normalize();
|
||||
const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
|
||||
const qNormal = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLR);
|
||||
const qSpin = new THREE.Quaternion().setFromAxisAngle(nLR, spinRad);
|
||||
const qMarker = qView.clone().multiply(qLink).multiply(qSpin.multiply(qNormal));
|
||||
gModel.add(makeMarkerSquareQuat(r2vArr(wp), qMarker, 0.022, col));
|
||||
|
||||
// normal arrow (length = half a marker size = ~12.5mm → 0.0125m)
|
||||
const arr = makeNormalArrow(r2vArr(wp), nWorld, 0.018, col);
|
||||
|
||||
@@ -106,6 +106,13 @@
|
||||
const mc = mapC.get(id);
|
||||
if (!mb || !mc) continue;
|
||||
|
||||
// Fehlende position_mm (z.B. Einzelkamera-Marker, von 3b nicht
|
||||
// trianguliert) ignorieren statt crashen — Marker bleibt im skipped-Log.
|
||||
if (!Array.isArray(ma.position_mm) || !Array.isArray(mb.position_mm) || !Array.isArray(mc.position_mm)) {
|
||||
skipped.push({ id, reason: 'fehlende position_mm (z.B. Einzelkamera-Marker, nicht trianguliert)' });
|
||||
continue;
|
||||
}
|
||||
|
||||
const P1 = ma.position_mm.map(Number);
|
||||
const P2 = mb.position_mm.map(Number);
|
||||
const P3 = mc.position_mm.map(Number);
|
||||
|
||||
@@ -59,11 +59,14 @@ def load_cameras(eval_dir: str) -> Dict[str, dict]:
|
||||
R = np.array(w2c["rotation_matrix"], dtype=float).reshape(3, 3)
|
||||
t = np.array(w2c["translation_m"], dtype=float).reshape(3)
|
||||
markers: Dict[int, np.ndarray] = {}
|
||||
confidence: Dict[int, float] = {}
|
||||
for d in det.get("detections", []):
|
||||
pts = d.get("image_points_px")
|
||||
if pts is not None:
|
||||
markers[int(d["marker_id"])] = np.array(pts, dtype=float).reshape(4, 2)
|
||||
cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers)
|
||||
mid = int(d["marker_id"])
|
||||
markers[mid] = np.array(pts, dtype=float).reshape(4, 2)
|
||||
confidence[mid] = float(d.get("confidence", 1.0))
|
||||
cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers, confidence=confidence)
|
||||
return cams
|
||||
|
||||
|
||||
@@ -156,6 +159,9 @@ def main() -> None:
|
||||
normal, center = corner_plane_normal(corners3d)
|
||||
edge_mm = float(np.mean([np.linalg.norm(corners3d[(i + 1) % 4] - corners3d[i]) for i in range(4)]) * 1000.0)
|
||||
|
||||
confidences = [cams[c]["confidence"].get(mid, 1.0) for c in cam_ids]
|
||||
weight = float(np.mean(confidences))
|
||||
|
||||
markers_out.append({
|
||||
"marker_id": int(mid),
|
||||
"link": marker_info.get(mid, {}).get("link", "unknown"),
|
||||
@@ -166,6 +172,7 @@ def main() -> None:
|
||||
"corners_m": [[float(v) for v in c] for c in corners3d],
|
||||
"num_cameras": len(cam_ids),
|
||||
"edge_length_mm": edge_mm,
|
||||
"weight": round(weight, 4),
|
||||
})
|
||||
|
||||
# camera poses in world (for viewer frusta): centre C = -R^T t, view axis = R[2]
|
||||
|
||||
@@ -161,10 +161,20 @@ def compute_rotation_axis(
|
||||
for mid in common_ids:
|
||||
# ── Mindest-Bewegungs-Filter ───────────────────────────────────────────
|
||||
# Marker die sich kaum bewegen liefern degenerate Umkreismittelpunkte.
|
||||
# Wir vergleichen die Zentren (position_mm) der drei Messungen.
|
||||
cA = np.array(mA[mid].get('position_mm', [0, 0, 0]), dtype=float)
|
||||
cB = np.array(mB[mid].get('position_mm', [0, 0, 0]), dtype=float)
|
||||
cC = np.array(mC[mid].get('position_mm', [0, 0, 0]), dtype=float)
|
||||
# Wir vergleichen die Zentren der drei Messungen.
|
||||
# Fehlt position_mm in einer Messung (z.B. Einzelkamera-Marker) → überspringen.
|
||||
cA_raw = mA[mid].get('position_mm')
|
||||
cB_raw = mB[mid].get('position_mm')
|
||||
cC_raw = mC[mid].get('position_mm')
|
||||
if cA_raw is None or cB_raw is None or cC_raw is None:
|
||||
skipped.append({
|
||||
'marker_id': mid,
|
||||
'reason': 'fehlende position_mm in mindestens einer Messung (z.B. Einzelkamera-Marker)',
|
||||
})
|
||||
continue
|
||||
cA = np.array(cA_raw, dtype=float)
|
||||
cB = np.array(cB_raw, dtype=float)
|
||||
cC = np.array(cC_raw, dtype=float)
|
||||
max_movement = max(
|
||||
np.linalg.norm(cB - cA),
|
||||
np.linalg.norm(cC - cB),
|
||||
|
||||
@@ -20,9 +20,15 @@ Four switchable methods (robot.json -> pose_estimation.method):
|
||||
global_ba : all variables jointly, position + normal residuals, robust loss
|
||||
hybrid : sequential_fk init -> global_ba refine (default, most stable)
|
||||
|
||||
Observation input:
|
||||
marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal)
|
||||
marker_observation = "center_point" -> aruco_positions_*.json (pos only)
|
||||
Observation input (robot.json -> pose_estimation.marker_observation):
|
||||
"corner_pose" (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker
|
||||
"corner_points" -> aruco_marker_poses.json: 12 corner residuals for
|
||||
robot-link markers (4 triangulated corners vs FK
|
||||
corners; no separate normal), 1 center residual for
|
||||
root-link (Board: floor/rail) markers whose spin is
|
||||
uncalibrated. Robust loss acts per corner. Opt-in;
|
||||
needs `corners_m`. Links via corner_point_links.
|
||||
"center_point" -> aruco_positions_*.json: position only
|
||||
|
||||
Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
|
||||
--from-state <json> seed/init state (flat {var: value}, or the
|
||||
@@ -82,12 +88,21 @@ DEFAULT_CFG: Dict[str, Any] = {
|
||||
"marker_observation": "corner_pose",
|
||||
"use_normals": True,
|
||||
"normal_weight": 100.0,
|
||||
"use_marker_weight": False,
|
||||
"robust_loss": "huber",
|
||||
"huber_delta_mm": 8.0,
|
||||
"max_iterations": 200,
|
||||
"min_cameras_per_marker": 2,
|
||||
"finger_block_joints": ["b", "c", "e"],
|
||||
"per_link_method": {},
|
||||
# Nur im marker_observation="corner_points"-Modus relevant: welche Links die
|
||||
# 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter);
|
||||
# der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum.
|
||||
# Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die
|
||||
# Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist
|
||||
# zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich,
|
||||
# z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"].
|
||||
"corner_point_links": None,
|
||||
# One switch: if set to a link name (e.g. "Arm1"), that link's
|
||||
# jointToParent.origin Y/Z is fit together with the normal pose (same
|
||||
# global_ba solve) and the result is written back into robot.json
|
||||
@@ -110,8 +125,12 @@ def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
|
||||
def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Load marker observations. Accepts aruco_marker_poses.json (with measured
|
||||
normal + num_cameras) or aruco_positions_*.json (position only).
|
||||
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int}
|
||||
normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json
|
||||
(position only).
|
||||
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None,
|
||||
link:str, n_cams:int, weight:float}
|
||||
corners_mm (aus `corners_m`, m→mm) speist den marker_observation=
|
||||
"corner_points"-Modus in residual_vector().
|
||||
"""
|
||||
data = json.load(open(path, "r", encoding="utf-8"))
|
||||
out: Dict[int, Dict[str, Any]] = {}
|
||||
@@ -134,7 +153,15 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i
|
||||
nn = np.linalg.norm(nv)
|
||||
if nn > 1e-9:
|
||||
nrm = nv / nn
|
||||
out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams}
|
||||
corners_mm = None
|
||||
cm = m.get("corners_m")
|
||||
if cm is not None:
|
||||
arr = np.array(cm, dtype=float)
|
||||
if arr.shape == (4, 3):
|
||||
corners_mm = arr * 1000.0
|
||||
out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm,
|
||||
"link": m.get("link", "?"), "n_cams": n_cams,
|
||||
"weight": float(m.get("weight", 1.0))}
|
||||
return out
|
||||
|
||||
|
||||
@@ -230,11 +257,29 @@ def analyze_chain(fk: RobotFK) -> Dict[str, Any]:
|
||||
for x in pending:
|
||||
var_block[x] = len(blocks) - 1
|
||||
|
||||
# subtree_markers[L] = L's own markers + all descendants' markers. Lets
|
||||
# observability() credit a block whose own link saw nothing this capture
|
||||
# but whose CHILD link did (e.g. Ellbow has no visible markers, but Arm2's
|
||||
# markers still constrain z through the chain — same idea as 4b's
|
||||
# Fallback-1, just for confidence reporting here, not for the fit itself).
|
||||
children: Dict[str, List[str]] = defaultdict(list)
|
||||
for ln, ld in links.items():
|
||||
p = ld.get("parent")
|
||||
if p:
|
||||
children[p].append(ln)
|
||||
subtree_markers: Dict[str, List[int]] = {}
|
||||
for ln in reversed(topo):
|
||||
ids = list(link_markers.get(ln, []))
|
||||
for c in children.get(ln, []):
|
||||
ids.extend(subtree_markers.get(c, []))
|
||||
subtree_markers[ln] = ids
|
||||
|
||||
return {
|
||||
"ordered_vars": ordered_vars,
|
||||
"var_type": var_type,
|
||||
"var_links": dict(var_links),
|
||||
"link_markers": link_markers,
|
||||
"subtree_markers": subtree_markers,
|
||||
"blocks": blocks,
|
||||
}
|
||||
|
||||
@@ -248,21 +293,87 @@ def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, n
|
||||
return fk.all_markers_world(T) # mid -> {world_mm, normal_world, link, local_mm}
|
||||
|
||||
|
||||
def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set:
|
||||
"""
|
||||
Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen.
|
||||
Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links
|
||||
(der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt
|
||||
ein Center-Residuum).
|
||||
"""
|
||||
explicit = cfg.get("corner_point_links")
|
||||
if isinstance(explicit, list) and explicit:
|
||||
return set(explicit)
|
||||
links = fk.links
|
||||
roots = {ln for ln, ld in links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in links}
|
||||
return set(links.keys()) - roots
|
||||
|
||||
|
||||
def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
|
||||
marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
|
||||
"""Position (mm) + optional normal (scaled) residuals over the given markers."""
|
||||
"""
|
||||
Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation:
|
||||
|
||||
"corner_pose" (Default): 3 Position (mm) + optional 3 Normale
|
||||
(×normal_weight) je Marker — wie bisher.
|
||||
"corner_points": 12 Eck-Residuen (4 Ecken × xyz, mm) NUR für Marker
|
||||
auf den `corner_point_links` (z.B. Hand/Finger),
|
||||
KEINE separate Normale (Orientierung steckt in den
|
||||
Ecken). Alle übrigen Marker verhalten sich wie im
|
||||
Default-Modus (Center + optionale Normale) — außer
|
||||
dem Root-Link (Board: Boden-/Rail-Marker, Spin
|
||||
unkalibriert), der nur Center bekommt ("ein Punkt
|
||||
pro Marker"). So lassen sich Ecken gezielt für
|
||||
Hand/Finger scharfschalten, ohne Arme/Board zu
|
||||
verändern.
|
||||
"""
|
||||
model = model_markers(fk, state)
|
||||
res: List[float] = []
|
||||
use_mw = bool(cfg.get("use_marker_weight", False))
|
||||
obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower()
|
||||
|
||||
if obs_mode == "corner_points":
|
||||
corner_links = _resolve_corner_links(fk, cfg)
|
||||
roots = {ln for ln, ld in fk.links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in fk.links}
|
||||
w_n = float(cfg.get("normal_weight", 30.0))
|
||||
use_n = bool(cfg.get("use_normals", True))
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
|
||||
mm = model[mid]
|
||||
link = mm.get("link")
|
||||
oc = obs[mid].get("corners_mm")
|
||||
mc = mm.get("corners_world")
|
||||
if link in corner_links and oc is not None and mc is not None:
|
||||
dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw # (4,3)
|
||||
res.extend(dc.reshape(-1).tolist()) # 12 Werte
|
||||
continue
|
||||
# Nicht-Eck-Marker verhalten sich wie im Default-Modus: Center +
|
||||
# optionale Normale — AUSSER auf dem Root-Link (Board: Boden-/Rail-
|
||||
# Marker mit unkalibriertem Spin), der nur Center bekommt ("ein
|
||||
# Punkt pro Marker"). So bleiben Arme/Board unverändert, wenn nur
|
||||
# Hand/Finger über corner_point_links auf Ecken laufen.
|
||||
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
|
||||
res.extend(dp.tolist())
|
||||
if link not in roots and use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
|
||||
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw
|
||||
res.extend(dn.tolist())
|
||||
return np.asarray(res, dtype=float)
|
||||
|
||||
# Default: Center (mm) + optionale Normale (skaliert)
|
||||
w_n = float(cfg.get("normal_weight", 30.0))
|
||||
use_n = bool(cfg.get("use_normals", True))
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
mm = model[mid]
|
||||
dp = np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]
|
||||
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
|
||||
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
|
||||
res.extend(dp.tolist())
|
||||
if use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
|
||||
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n
|
||||
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw
|
||||
res.extend(dn.tolist())
|
||||
return np.asarray(res, dtype=float)
|
||||
|
||||
@@ -518,16 +629,25 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict
|
||||
driven by markers-per-variable in that block:
|
||||
high : >= 2 markers per variable (well over-determined)
|
||||
medium : >= 1 marker per variable
|
||||
low : fewer markers than variables (under-determined — distrust!)
|
||||
none : no markers at all (variable left at 0)
|
||||
low : fewer markers than variables (under-determined — distrust!),
|
||||
OR no own markers seen but a child link's markers were
|
||||
(indirect evidence through the chain, e.g. Ellbow via Arm2)
|
||||
none : no markers at all, not even indirectly (variable left at 0)
|
||||
"""
|
||||
info: Dict[str, Dict[str, Any]] = {}
|
||||
subtree_markers = chain.get("subtree_markers", {})
|
||||
for block in chain["blocks"]:
|
||||
seen = [m for m in block["markers"] if m in obs]
|
||||
indirect = False
|
||||
if not seen and block["anchor"]:
|
||||
seen = [m for m in subtree_markers.get(block["anchor"], []) if m in obs]
|
||||
indirect = bool(seen)
|
||||
nvars = max(1, len(block["vars"]))
|
||||
ratio = len(seen) / nvars
|
||||
if len(seen) == 0:
|
||||
conf = "none"
|
||||
elif indirect:
|
||||
conf = "low" # indirect/coupled through a child link, not direct
|
||||
elif ratio >= 2.0:
|
||||
conf = "high"
|
||||
elif ratio >= 1.0:
|
||||
@@ -537,7 +657,7 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict
|
||||
for v in block["vars"]:
|
||||
info[v] = {"observable": len(seen) > 0, "n_markers": len(seen),
|
||||
"block_vars": len(block["vars"]), "confidence": conf,
|
||||
"block_anchor": block["anchor"]}
|
||||
"block_anchor": block["anchor"], "indirect": indirect}
|
||||
return info
|
||||
|
||||
|
||||
@@ -615,6 +735,9 @@ def main() -> None:
|
||||
ap.add_argument("-robot", "--robot", required=True)
|
||||
ap.add_argument("-out", "--out", default=None)
|
||||
ap.add_argument("--method", default=None, help="override robot.json method")
|
||||
ap.add_argument("--marker-observation", default=None, dest="marker_observation",
|
||||
help="override robot.json marker_observation "
|
||||
"(corner_pose | corner_points | center_point)")
|
||||
ap.add_argument("--from-state", default=None, metavar="JSON",
|
||||
help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
|
||||
"written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
|
||||
@@ -625,6 +748,8 @@ def main() -> None:
|
||||
cfg = load_pose_cfg(robot_data)
|
||||
if args.method:
|
||||
cfg["method"] = args.method
|
||||
if args.marker_observation:
|
||||
cfg["marker_observation"] = args.marker_observation
|
||||
|
||||
fk = RobotFK(robot_data)
|
||||
obs = load_observations(args.markers, cfg.get("use_normals", True),
|
||||
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -192,14 +192,109 @@ class RobotFK:
|
||||
"""Transform a local marker position → world (mm)."""
|
||||
return transform_point(transforms.get(link_name, np.eye(4)), local_pos)
|
||||
|
||||
# ── Marker-Eckpunkte (Mehrpunkt-Residuen, doc/Homing_5_Pose_MultiPoint_Weighted.md Schritt 3) ──
|
||||
|
||||
@staticmethod
|
||||
def _shortest_arc_R(normal: Sequence[float]) -> np.ndarray:
|
||||
"""
|
||||
Rotationsmatrix [0,0,1] → normal (kürzester Bogen).
|
||||
|
||||
Repliziert THREE.Quaternion.setFromUnitVectors(vFrom=[0,0,1], vTo=n)
|
||||
exakt (inkl. antiparallelem Sonderfall), damit die hier erzeugten
|
||||
Ecken zur visuell verifizierten Orientierungs-Konvention in
|
||||
boardViewer.html passen (qNormalLoc dort).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
nn = float(np.linalg.norm(n))
|
||||
n = n / nn if nn > 1e-12 else np.array([0.0, 0.0, 1.0])
|
||||
# three.js: quat = (cross([0,0,1], n), dot([0,0,1], n) + 1), dann normalisieren.
|
||||
r = float(n[2]) + 1.0
|
||||
if r < 1e-12:
|
||||
# antiparallel (n == [0,0,-1]): three.js else-Zweig für vFrom=[0,0,1] → (0,-1,0,0)
|
||||
qx, qy, qz, qw = 0.0, -1.0, 0.0, 0.0
|
||||
else:
|
||||
qx, qy, qz, qw = -float(n[1]), float(n[0]), 0.0, r # cross([0,0,1], n) = (-ny, nx, 0)
|
||||
ql = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||||
qx, qy, qz, qw = qx / ql, qy / ql, qz / ql, qw / ql
|
||||
return np.array([
|
||||
[1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
|
||||
[2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
|
||||
[2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
|
||||
])
|
||||
|
||||
@staticmethod
|
||||
def _marker_plane_corners(half: float) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Eckpunkte in der Marker-Ebene (+Z = Normale), in DERSELBEN
|
||||
Reihenfolge wie die von 3b_corner_marker_poses.py triangulierten
|
||||
`corners_m` (ArUco 0..3, im Uhrzeigersinn von der Vorderseite gesehen).
|
||||
|
||||
Ecke 0 zeigt in Richtung (+h, +h) — dieselbe Konvention wie der visuell
|
||||
kalibrierte Orientierungszeiger (1,1,0) in boardViewer.html. Damit passt
|
||||
sie zu den manuell/visuell gesetzten `spin`-Werten der ARM-Marker, die
|
||||
im Homing die Gelenkwinkel bestimmen (gegen echte corners_m am
|
||||
Seed-Pose verifiziert, test_robot_fk_corners.py, RMS ~1 mm).
|
||||
|
||||
Hinweis: Die Board-Referenzmarker (Set A0) sind ~90° anders kalibriert,
|
||||
ihre Eckreihenfolge passt unter dieser Konvention NICHT — egal, weil
|
||||
Board der Root-Link ist: ihr Eck-Residuum ist konstant bzgl. der
|
||||
Gelenkvariablen und beeinflusst die Schätzung nicht (der robuste
|
||||
Huber-Verlust dämpft es als Ausreißer). Siehe Doc-Notiz.
|
||||
|
||||
Die Drehrichtung 0→1→2→3 ist so, dass 3b's `corner_plane_normal()`
|
||||
(outward = -cross(e01,e02)) wieder +Z liefert — identisch zur
|
||||
Beobachtungs-Konvention.
|
||||
"""
|
||||
h = float(half)
|
||||
return np.array([
|
||||
[ h, h, 0.0], # 0
|
||||
[ h, -h, 0.0], # 1
|
||||
[-h, -h, 0.0], # 2
|
||||
[-h, h, 0.0], # 3
|
||||
])
|
||||
|
||||
@classmethod
|
||||
def marker_corners_local(cls,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Marker-Ecken im LINK-lokalen Frame (mm), Reihenfolge wie `corners_m`.
|
||||
|
||||
Orientierung = Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale,
|
||||
exakt wie boardViewer.html (qSpinLoc.multiply(qNormalLoc)).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
R = _rot_axis_angle(n, float(spin_deg)) @ cls._shortest_arc_R(n)
|
||||
plane = cls._marker_plane_corners(float(size_mm) / 2.0) # (4,3)
|
||||
return np.asarray(position, dtype=float) + (R @ plane.T).T # (4,3)
|
||||
|
||||
@classmethod
|
||||
def marker_corners_world(cls,
|
||||
transforms: Dict[str, np.ndarray],
|
||||
link_name: str,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""Die 4 Marker-Ecken im Weltframe (mm), Reihenfolge wie `corners_m`."""
|
||||
T = transforms.get(link_name, np.eye(4))
|
||||
local = cls.marker_corners_local(position, normal, size_mm, spin_deg)
|
||||
return np.array([transform_point(T, c) for c in local])
|
||||
|
||||
def all_markers_world(self,
|
||||
transforms: Dict[str, np.ndarray]
|
||||
) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Returns
|
||||
-------
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world}
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world, corners_world}
|
||||
|
||||
corners_world: (4,3) Welt-mm in `corners_m`-Reihenfolge (für den
|
||||
marker_observation="corner_points"-Modus in 5_pose_estimation.py).
|
||||
"""
|
||||
default_size = float((self.robot.get("markerDefaults", {}) or {}).get("size", 25.0))
|
||||
result: Dict[int, Dict[str, Any]] = {}
|
||||
for lname, ldata in self.links.items():
|
||||
T = transforms.get(lname, np.eye(4))
|
||||
@@ -210,11 +305,15 @@ class RobotFK:
|
||||
continue
|
||||
loc = np.array(m["position"], dtype=float)
|
||||
nor = np.array(m.get("normal", [0, 0, 1]), dtype=float)
|
||||
size_mm = float(m.get("size", default_size))
|
||||
spin_deg = float(m.get("spin", 0.0))
|
||||
local_corners = self.marker_corners_local(loc, nor, size_mm, spin_deg)
|
||||
result[mid] = {
|
||||
"world_mm": transform_point(T, loc),
|
||||
"local_mm": loc,
|
||||
"link": lname,
|
||||
"normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12),
|
||||
"corners_world": np.array([transform_point(T, c) for c in local_corners]),
|
||||
}
|
||||
return result
|
||||
|
||||
|
||||
502
scripts/robot_rendering.json
Normal file
502
scripts/robot_rendering.json
Normal file
@@ -0,0 +1,502 @@
|
||||
{
|
||||
"_label": "todo3_2026-06-11",
|
||||
"coordinateSystem": {"handedness": "right", "x": "right", "y": "backward", "z": "up"},
|
||||
"units": {"_owner": "appRobotDriver", "length": "mm", "rotation": "degree"},
|
||||
"kinematics": {
|
||||
"_owner": "appRobotDriver",
|
||||
"type": "arm3segmentlinearx"
|
||||
},
|
||||
"motion": {
|
||||
"_owner": "appRobotDriver",
|
||||
"defaultFeedrate": 2300,
|
||||
"speedMode": "legacy",
|
||||
"speedModeOptions": ["legacy", "correct"]
|
||||
},
|
||||
"controllers": {
|
||||
"_owner": "appRobotDriver",
|
||||
"base": { "ip": "fluidNcBase.local", "port": 2300, "protocol": "telnet", "axes": ["x", "y", "z"] },
|
||||
"elbow": { "ip": "fluidNcEllbow.local", "port": 5000, "protocol": "telnet", "axes": ["a", null, null] },
|
||||
"hand": { "ip": "fluidNcHand.local", "port": 5000, "protocol": "telnet", "axes": ["c", "e", "b"] }
|
||||
},
|
||||
"vision_config": {"MarkerType": "DICT_4X4_250", "MarkerSize": 0.025},
|
||||
"renderingInfo": {
|
||||
"width": 1280,
|
||||
"height": 720,
|
||||
"renderDefaults": {"width": 1280, "height": 720, "dofFStop": 11},
|
||||
"cameraPosition__1": [-10, -800, 500],
|
||||
"cameraPosition__2": [-500, 300, 1200],
|
||||
"cameraPosition__3": [-200, -900, 200],
|
||||
"cameraPosition__4": [1200, 200, 300],
|
||||
"cameraPosition_a": [-300, -800, 500],
|
||||
"cameraPosition": [-200, 200, 1400],
|
||||
"cameraPosition_c": [600, -500, 600],
|
||||
"cameraTarget": [200, -200, 180],
|
||||
"cameraUpVector": [0, 0, 1],
|
||||
"lightPosition": [-500, -500, 500],
|
||||
"lightTarget": [0, 0, 0],
|
||||
"lightUpVector": [0, 0, 1],
|
||||
"metric": "mm",
|
||||
"showSkeleton": true,
|
||||
"showMarkers": true,
|
||||
"backgroundColor": [0.7, 0.85, 1.0],
|
||||
"backgroundStrength": 0.2,
|
||||
"sunEnergy": 0.35,
|
||||
"areaEnergy": 120,
|
||||
"exposure": -1.5,
|
||||
"lensDirt": true,
|
||||
"lensDirtStrength": 0.08,
|
||||
"dofEnabled": true,
|
||||
"dofFStop": 11.0,
|
||||
"arucoDust": true,
|
||||
"arucoDustStrength": 1.6,
|
||||
"markerOffsetMaxMm": 4.0,
|
||||
"markerOffsetSeed": 0,
|
||||
"markerRotationMaxDeg": 3,
|
||||
"motionBlur": true,
|
||||
"motionBlurMaxPx": 5.5,
|
||||
"focalErrorPct": 0.5,
|
||||
"principalErrorPx": 3.0,
|
||||
"residualDistortion": [0.02, -0.01],
|
||||
"localizedBlur": false,
|
||||
"localizedBlurStrength": 0.15,
|
||||
"vignette": true,
|
||||
"vignetteStrength": 0.08,
|
||||
"sensorNoise": true,
|
||||
"sensorNoiseStrength": 0.01,
|
||||
"lensDistortion": true,
|
||||
"lensDistortionStrength": 0.002,
|
||||
"materials": {
|
||||
"wood": {"baseColor": [0.72, 0.52, 0.33], "roughness": 0.85, "metallic": 0.0},
|
||||
"plaWhite": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.45, "metallic": 0.0},
|
||||
"steel": {"baseColor": [0.72, 0.72, 0.75], "roughness": 0.25, "metallic": 1.0},
|
||||
"powderCoatBlue": {"baseColor": [0.15, 0.25, 0.7], "roughness": 0.55, "metallic": 0.0},
|
||||
"defaultPlastic": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.4, "metallic": 0.0},
|
||||
"skeletonRed": {"baseColor": [0.85, 0.2, 0.2], "roughness": 0.35, "metallic": 0.0},
|
||||
"markerBlack": {"baseColor": [0.04, 0.04, 0.04], "roughness": 0.8, "metallic": 0.0}
|
||||
},
|
||||
"skeletonDefaults": {"radius": 4, "color": [0.85, 0.2, 0.2]},
|
||||
"markerDefaults": {"size": 25, "thickness": 1, "color": [0.04, 0.04, 0.04]},
|
||||
"defaultPosition": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3}
|
||||
},
|
||||
"defaultPosition__": {"x": 10, "y": 4, "z": 20, "a": 10, "b": 2, "c": 9, "e": 1},
|
||||
"defaultPosition": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
|
||||
"recognized": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
|
||||
"constraint_rules": {
|
||||
"rigid_distance": {"enabled": true, "mode": "mst", "weight": 1.0},
|
||||
"joint_axis_projection": {"enabled": true, "max_pairs": 2, "weight": 0.35},
|
||||
"chain_axis_projection": {"enabled": false, "max_depth": 3, "max_pairs": 2, "weight": 0.15},
|
||||
"axis_alignment_threshold": 0.95
|
||||
},
|
||||
"observation_weighting": {"enabled": true, "distance_weight": true, "marker_size_weight": true, "view_angle_weight": true},
|
||||
"multiview_calculation": {
|
||||
"combine_mode": "mean",
|
||||
"size_ref_px": 50.0,
|
||||
"border_ref_px": 120.0,
|
||||
"center_ref_norm": 0.01,
|
||||
"sharpness_ref": 2500.0,
|
||||
"homography_ref": 0.18,
|
||||
"size_factor": 0.3,
|
||||
"aspect_factor": 0.3,
|
||||
"border_factor": 0.01,
|
||||
"center_factor": 0.01,
|
||||
"sharpness_factor": 0.5,
|
||||
"homography_factor": 0.2,
|
||||
"normal_visibility_factor": 0.01,
|
||||
"spin_factor": 0.3,
|
||||
"weight_floor": 0.3
|
||||
},
|
||||
"pose_estimation": {
|
||||
"method": "hybrid",
|
||||
"marker_observation": "corner_pose",
|
||||
"use_normals": true,
|
||||
"normal_weight": 100.0,
|
||||
"robust_loss": "huber",
|
||||
"huber_delta_mm": 8.0,
|
||||
"max_iterations": 200,
|
||||
"min_cameras_per_marker": 2,
|
||||
"finger_block_joints": ["b", "c", "e"],
|
||||
"per_link_method": {}
|
||||
},
|
||||
"robot_test_poses": {
|
||||
"4": {"x": 70, "y": 50, "z": -70, "a": 120, "b": 50, "c": 30, "e": 20},
|
||||
"5": {"x": 180, "y": 86, "z": -120, "a": -60, "b": 22, "c": 91, "e": 10},
|
||||
"6": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3},
|
||||
"7": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9},
|
||||
"8": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3},
|
||||
"9": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
|
||||
"9a": {
|
||||
"x": 60,
|
||||
"y": -2,
|
||||
"z": 95,
|
||||
"a": 200,
|
||||
"b": 60,
|
||||
"c": 9,
|
||||
"e": 8,
|
||||
"rendering": {"width": 1440, "height": 1080, "dofFStop": 11}
|
||||
},
|
||||
"9b": {
|
||||
"x": 60,
|
||||
"y": -2,
|
||||
"z": 95,
|
||||
"a": 200,
|
||||
"b": 60,
|
||||
"c": 9,
|
||||
"e": 8,
|
||||
"rendering": {"width": 4896, "height": 3264, "dofFStop": 5.6}
|
||||
},
|
||||
"10": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4},
|
||||
"11": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
|
||||
"12": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6}
|
||||
},
|
||||
"test_camera_positions": {
|
||||
"a": [-300, -800, 800],
|
||||
"b": [300, -900, 1200],
|
||||
"c": [300, -900, 400],
|
||||
"d": [700, -800, 400],
|
||||
"e": [1200, -900, 400],
|
||||
"f": [500, -300, 1400],
|
||||
"g": [-200, 200, 1400]
|
||||
},
|
||||
"test_camera_targets": {
|
||||
"a": [210, -100, 180],
|
||||
"b": [310, -80, 180],
|
||||
"c": [210, -100, 150],
|
||||
"d": [210, -100, 150],
|
||||
"e": [210, -100, 50],
|
||||
"f": [200, -200, 180],
|
||||
"g": [200, -200, 180]
|
||||
},
|
||||
"movements": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
|
||||
"state_pose_params": {
|
||||
"numbers_of_Elements_to_consider_start": 3,
|
||||
"numbers_of_Elements_to_consider_final": 5,
|
||||
"solver_in_between_geometrical": false,
|
||||
"solver_after_geometrical": false,
|
||||
"geometric_passes_per_stage": 2,
|
||||
"revolute_search_coarse_deg": 5.0,
|
||||
"revolute_search_fine_deg": 1.0,
|
||||
"root_pose_min_markers": 3,
|
||||
"use_marker_normals_flip_tiebreak": true,
|
||||
"normal_flip_weight": 0.05
|
||||
},
|
||||
"links": {
|
||||
"_owner": "appRobotDriver",
|
||||
"Board": {
|
||||
"parent": null,
|
||||
"size": [1000, 200, 25],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"skeleton": {"from": [0, 0, 16], "to": [1000, 0, 16], "radius": 4, "color": [0.85, 0.2, 0.2]},
|
||||
"markers": [
|
||||
{"id": 210, "set": "Brett", "position": [20, -20, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 211, "set": "Brett", "position": [250, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 215, "set": "Brett", "position": [250, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 214, "set": "Brett", "position": [350, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 208, "set": "Brett", "position": [350, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 206, "set": "Brett", "position": [650, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 205, "set": "Brett", "position": [750, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 207, "set": "Brett", "position": [750, -10, 0.3], "normal": [0, 0, 1]},
|
||||
{"id": 217, "set": "Brett", "position": [650, -90, 0.3], "normal": [0, 0, 1]},
|
||||
{
|
||||
"id": 46,
|
||||
"set": "A0",
|
||||
"position": [536.71, 185.44, -27.3],
|
||||
"normal": [0, 0, 1],
|
||||
"spin": 90,
|
||||
"info": "is placed on a white paper, A0_60Arucos_25mm_Seet223.pdf, with the following marker placements:"
|
||||
},
|
||||
{"id": 47, "set": "A0", "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 48, "set": "A0", "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 49, "set": "A0", "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 50, "set": "A0", "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 51, "set": "A0", "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 52, "set": "A0", "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 53, "set": "A0", "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 54, "set": "A0", "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 55, "set": "A0", "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 56, "set": "A0", "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 57, "set": "A0", "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 58, "set": "A0", "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 59, "set": "A0", "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 60, "set": "A0", "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 61, "set": "A0", "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 62, "set": "A0", "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 63, "set": "A0", "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 64, "set": "A0", "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 65, "set": "A0", "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 66, "set": "A0", "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 67, "set": "A0", "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 68, "set": "A0", "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 69, "set": "A0", "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 70, "set": "A0", "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 71, "set": "A0", "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 72, "set": "A0", "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 73, "set": "A0", "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 74, "set": "A0", "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 75, "set": "A0", "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 76, "set": "A0", "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 77, "set": "A0", "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 78, "set": "A0", "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 79, "set": "A0", "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 80, "set": "A0", "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 81, "set": "A0", "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 82, "set": "A0", "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 83, "set": "A0", "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 84, "set": "A0", "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 85, "set": "A0", "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 86, "set": "A0", "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 87, "set": "A0", "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 88, "set": "A0", "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 89, "set": "A0", "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 90, "set": "A0", "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 91, "set": "A0", "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 92, "set": "A0", "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 93, "set": "A0", "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 94, "set": "A0", "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 95, "set": "A0", "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 96, "set": "A0", "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 97, "set": "A0", "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 98, "set": "A0", "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 99, "set": "A0", "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 100, "set": "A0", "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 101, "set": "A0", "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 102, "set": "A0", "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 103, "set": "A0", "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 104, "set": "A0", "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90},
|
||||
{"id": 105, "set": "A0", "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Board.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "wood"
|
||||
},
|
||||
{
|
||||
"stlFile": "surfaces/BoardRail.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "steel"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Base": {
|
||||
"parent": "Board",
|
||||
"size": [150, 200, 150],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, 0, 16],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "x",
|
||||
"feedrate": 2000,
|
||||
"controller": "base"
|
||||
},
|
||||
"skeleton": {"from": [0, 108, 45], "to": [110, 108, 45], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Base.stl",
|
||||
"originOfModel": [-30, 0, -35],
|
||||
"rotationOfModelDegree": [0, 0, 0],
|
||||
"material": "plaWhite"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Arm1": {
|
||||
"parent": "Base",
|
||||
"size": [70, 250, 70],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint1",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [110, 108, 45],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "y",
|
||||
"feedrate": 2300,
|
||||
"controller": "base"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.2, 0.2, 0.9]},
|
||||
"markers": [
|
||||
{"id": 198, "name": "aruco_198", "position": [0, -160, 35], "normal": [0, 0, 1], "size": 25, "spin": 0},
|
||||
{"id": 229, "name": "aruco_229", "position": [0, -250, 35], "normal": [0, 0, 1], "size": 25, "spin": 0},
|
||||
{"id": 242, "name": "aruco_242", "position": [0, -250, -35], "normal": [0, 0, -1], "size": 25, "spin": 0},
|
||||
{"id": 243, "name": "aruco_243", "position": [0, -285, 0], "normal": [0, -1, 0], "size": 25, "spin": 0}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Holm.stl",
|
||||
"originOfModel__": [-25, 29, -28.5],
|
||||
"originOfModel": [-29, 25, 28.5],
|
||||
"rotationOfModelDegree__": [0, 0, 0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "powderCoatBlue"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Ellbow": {
|
||||
"parent": "Arm1",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint2",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "z",
|
||||
"feedrate": 2300,
|
||||
"controller": "base"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [90, 0, 0], "radius": 4, "color": [0.9, 0.2, 0.2]},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Ellebogen.stl",
|
||||
"originOfModel": [90, 0, 0],
|
||||
"rotationOfModelDegree": [0, -90, -90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
{"id": 244, "name": "aruco_244", "position": [125, 0, 0], "normal": [1, 0, 0], "size": 25, "spin": 0},
|
||||
{"id": 245, "name": "aruco_245", "position": [90, 0, -35], "normal": [0, 0, -1], "size": 25, "spin": 0},
|
||||
{"id": 246, "name": "aruco_246", "position": [90, 0, 35], "normal": [0, 0, 1], "size": 25},
|
||||
{"id": 247, "name": "aruco_247", "position": [52.5, 0, 35], "normal": [0, 0, 1], "size": 25},
|
||||
{"id": 248, "name": "aruco_248", "position": [52.5, 0, -35], "normal": [0, 0, -1], "size": 25},
|
||||
{"id": 232, "name": "aruco_232", "position": [90, 24.75, -24.75], "normal": [0, 1, -1], "size": 25},
|
||||
{"id": 231, "name": "aruco_231", "position": [90, 24.75, 24.75], "normal": [0, 1, 1], "size": 25}
|
||||
]
|
||||
},
|
||||
"Arm2": {
|
||||
"parent": "Ellbow",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [90, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "a",
|
||||
"feedrate": 2300,
|
||||
"controller": "elbow"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.95, 0.85, 0.2]},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Unterarm.stl",
|
||||
"originOfModel": [0, -250, 0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
{"id": 120, "position": [24.75, -112, -24.75], "normal": [1, 0, -1]},
|
||||
{"id": 122, "name": "aruco_122", "position": [-35, -112, 0], "normal": [-1, 0, 0]},
|
||||
{"id": 218, "name": "aruco_218", "position": [35, -112, 0], "normal": [1, 0, 0]},
|
||||
{"id": 113, "name": "aruco_113", "position": [0, -182, 30], "normal": [0, 0, 1]},
|
||||
{"id": 114, "name": "aruco_114", "position": [24.75, -182, -24.75], "normal": [1, 0, -1]},
|
||||
{"id": 115, "name": "aruco_115", "position": [-24.75, -182, -24.75], "normal": [-1, 0, -1]},
|
||||
{"id": 124, "name": "aruco_124", "position": [-35, -219, 0], "normal": [-1, 0, 0]},
|
||||
{"id": 219, "name": "aruco_219", "position": [35, -219, 0], "normal": [1, 0, 0]}
|
||||
]
|
||||
},
|
||||
"Hand": {
|
||||
"parent": "Arm2",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint4",
|
||||
"type": "revolute",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "b",
|
||||
"feedrate": 2300,
|
||||
"controller": "hand"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -35, 0], "radius": 4, "color": [0.95, 0.55, 0.15]}
|
||||
},
|
||||
"Palm": {
|
||||
"parent": "Hand",
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [0, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "c",
|
||||
"feedrate": 2300,
|
||||
"controller": "hand"
|
||||
},
|
||||
"skeleton": {"from": [-50, -35, 0], "to": [50, -35, 0], "radius": 7, "color": [0.95, 0.2, 0.2]}
|
||||
},
|
||||
"FingerA": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [4, -35, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e",
|
||||
"feedrate": 2000,
|
||||
"controller": "hand"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [
|
||||
{"id": 40, "position": [12, -24, -17.1], "normal": [-10.98, 0, -23.56]},
|
||||
{"id": 41, "position": [1.5, -2.2, 25.8], "normal": [0, -25.6, 9.5]},
|
||||
{"id": 42, "position": [13.9, -40, 0], "normal": [1, -0.35, 0.4], "spin": 27}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [24, 0, -9.1],
|
||||
"rotationOfModelDegree": [90, -90, 0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
},
|
||||
"FingerB": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [-4, -35, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e",
|
||||
"feedrate": 2000,
|
||||
"controller": "hand"
|
||||
},
|
||||
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
|
||||
"markers": [
|
||||
{"id": 43, "position": [-12, -24, 17.1], "normal": [10.98, 0, 23.56], "spin": 90},
|
||||
{"id": 44, "position": [-1.5, -2.2, -25.8], "normal": [0, -25.6, -9.5], "spin": 90},
|
||||
{"id": 45, "position": [-13.9, -40, 0], "normal": [-1, -0.35, -0.4], "spin": -27}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [-24, 0, 9.1],
|
||||
"rotationOfModelDegree": [90, 90, 0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
48
server/buildG92.cjs
Normal file
48
server/buildG92.cjs
Normal file
@@ -0,0 +1,48 @@
|
||||
/**
|
||||
* buildG92.cjs
|
||||
* Baut aus einem Homing-State {x,y,z,a,b,c,e} einen G92-G-Code-String.
|
||||
*
|
||||
* G92 setzt am appRobotDriver die Motorposition OHNE Bewegung (intern als M92
|
||||
* verarbeitet, siehe appRobotDriver/doc/API.md + robot/RobotController.js) —
|
||||
* exakt die Homing-Semantik. Die Achsbuchstaben bilden 1:1 auf die Motorachsen
|
||||
* ab: X→xMotor, Y→alpha, Z→beta, A→a, B→b, C→c, E→e.
|
||||
*
|
||||
* Bekannte Achsen werden immer mit ihrem realen Wert gesendet. Welche Achsen
|
||||
* bekannt sind, hängt vom Pfad ab:
|
||||
* - 5_pose_estimation.py (Fallback) liefert alle 7 (x,y,z,a,b,c,e),
|
||||
* - die 4b-Primärkette (Arm1→y … Hand→b) liefert nur x,y,z,a,b.
|
||||
* Eine Achse, die wirklich fehlt oder als unbeobachtbar `null` markiert ist,
|
||||
* wird per Default WEGGELASSEN — der Driver lässt nicht genannte Achsen
|
||||
* unverändert (M92 setzt nur Achsen mit endlichem Zahlenwert), statt eine
|
||||
* unbekannte Position fälschlich als 0 zu behaupten. `fillMissingWithZero`
|
||||
* erzwingt bei Bedarf das alte 0-Auffüllen.
|
||||
*
|
||||
* CommonJS, damit Jest (CJS) und der ESM-Server dieselbe Funktion nutzen
|
||||
* (gleiches Muster wie spinNormalize.cjs / homingXEstimate.cjs).
|
||||
*/
|
||||
|
||||
// Reihenfolge + Achsbuchstaben wie vom Driver erwartet.
|
||||
const AXES = [
|
||||
['x', 'X'], ['y', 'Y'], ['z', 'Z'],
|
||||
['a', 'A'], ['b', 'B'], ['c', 'C'], ['e', 'E'],
|
||||
];
|
||||
|
||||
/**
|
||||
* @param {Record<string, number|null>} state flacher Joint-State (accumulated_state)
|
||||
* @param {{decimals?: number, fillMissingWithZero?: boolean}} [opts]
|
||||
* @returns {string} z.B. "G92 X164.57 Y-2.09 Z60.58 A86.75 B-46.97 C-64.91 E22.59"
|
||||
*/
|
||||
function buildG92(state = {}, { decimals = 2, fillMissingWithZero = false } = {}) {
|
||||
const parts = [];
|
||||
for (const [key, axis] of AXES) {
|
||||
const num = Number(state?.[key]);
|
||||
if (state?.[key] != null && Number.isFinite(num)) {
|
||||
parts.push(`${axis}${num.toFixed(decimals)}`);
|
||||
} else if (fillMissingWithZero) {
|
||||
parts.push(`${axis}${(0).toFixed(decimals)}`);
|
||||
}
|
||||
}
|
||||
return `G92 ${parts.join(' ')}`;
|
||||
}
|
||||
|
||||
module.exports = { buildG92, AXES };
|
||||
77
server/driverClient.js
Normal file
77
server/driverClient.js
Normal file
@@ -0,0 +1,77 @@
|
||||
/**
|
||||
* driverClient.js – WebSocket-Transport zum appRobotDriver
|
||||
*
|
||||
* Der Driver nimmt Steuerbefehle als Plain-Text-G-Code über einen WebSocket
|
||||
* entgegen (wss://…:2096, self-signed), NICHT über HTTP — siehe
|
||||
* appRobotDriver/doc/API.md. Ein früher angenommenes `POST /api/state` existiert
|
||||
* dort nicht (war Platzhalter, vgl. doc/accessRobotAPI.md). G92 setzt am Driver
|
||||
* die Motorposition ohne Bewegung (intern M92) = exakt die Homing-Semantik.
|
||||
*
|
||||
* DRIVER_WS_URL nicht gesetzt → kein Kontakt, klarer 501-Fehler (analog zum
|
||||
* früheren ROBOT_URL-Verhalten).
|
||||
*/
|
||||
import { WebSocket } from 'ws';
|
||||
|
||||
const DRIVER_WS_URL = process.env.DRIVER_WS_URL || '';
|
||||
|
||||
/** true, wenn ein Driver-WebSocket konfiguriert ist. */
|
||||
export function isDriverConfigured() {
|
||||
return Boolean(DRIVER_WS_URL);
|
||||
}
|
||||
|
||||
/**
|
||||
* Öffnet eine kurzlebige WS-Verbindung zum Driver, sendet eine G-Code-Zeile und
|
||||
* wartet auf die erste Antwort (Positions-JSON bzw. Fehler-Envelope). Der Driver
|
||||
* broadcastet nach jedem G-Code das aktuelle Positions-JSON an alle Clients —
|
||||
* der Sender ist selbst Client und bekommt es zurück.
|
||||
*
|
||||
* @param {string} line z.B. "G92 X1 Y2 …"
|
||||
* @param {{timeoutMs?: number}} [opts]
|
||||
* @returns {Promise<{ok:boolean, sent:string, response?:any, error?:string, note?:string}>}
|
||||
*/
|
||||
export function sendGcode(line, { timeoutMs = 4000 } = {}) {
|
||||
const text = String(line ?? '').trim();
|
||||
if (!text) {
|
||||
return Promise.reject(Object.assign(new Error('Leere G-Code-Zeile'), { statusCode: 400 }));
|
||||
}
|
||||
if (!DRIVER_WS_URL) {
|
||||
return Promise.reject(Object.assign(
|
||||
new Error('DRIVER_WS_URL ist nicht konfiguriert'), { statusCode: 501 }));
|
||||
}
|
||||
|
||||
return new Promise((resolve, reject) => {
|
||||
// Self-signed Cert am Driver → Zertifikatsprüfung deaktivieren (interner Hop).
|
||||
const ws = new WebSocket(DRIVER_WS_URL, { rejectUnauthorized: false });
|
||||
let settled = false;
|
||||
|
||||
const finish = (fn, arg) => {
|
||||
if (settled) return;
|
||||
settled = true;
|
||||
clearTimeout(timer);
|
||||
try { ws.close(); } catch { /* egal */ }
|
||||
fn(arg);
|
||||
};
|
||||
|
||||
// Gesendet, aber keine Antwort rechtzeitig: kein harter Fehler — der Befehl
|
||||
// ist raus, der Driver antwortet nur evtl. nicht broadcastfähig.
|
||||
const timer = setTimeout(() => {
|
||||
finish(resolve, { ok: true, sent: text, response: null, note: 'keine Antwort (Timeout)' });
|
||||
}, timeoutMs);
|
||||
|
||||
ws.on('open', () => ws.send(text));
|
||||
|
||||
ws.on('message', (data) => {
|
||||
const raw = data.toString();
|
||||
let parsed;
|
||||
try { parsed = JSON.parse(raw); } catch { parsed = raw; }
|
||||
if (parsed && typeof parsed === 'object' && parsed.type === 'error') {
|
||||
finish(resolve, { ok: false, sent: text, error: parsed.message || raw, response: parsed });
|
||||
} else {
|
||||
finish(resolve, { ok: true, sent: text, response: parsed });
|
||||
}
|
||||
});
|
||||
|
||||
ws.on('error', (err) => finish(reject, Object.assign(
|
||||
new Error(`Driver-WS-Fehler: ${err.message}`), { statusCode: 502 })));
|
||||
});
|
||||
}
|
||||
@@ -5,18 +5,18 @@
|
||||
* atomisches Write per Temp-Datei ist hier nicht nötig – die Datei wird direkt
|
||||
* überschrieben; bei Bedarf Backup-Strategie ergänzen).
|
||||
*/
|
||||
import fsPromises from 'fs/promises';
|
||||
import { createRequire } from 'module';
|
||||
import { fetchRobot, pushRobot } from './robotConfig.js';
|
||||
const { normalizeSpinDeg } = createRequire(import.meta.url)('./spinNormalize.cjs');
|
||||
|
||||
// ── I/O ───────────────────────────────────────────────────────────────────────
|
||||
|
||||
async function readRobot(robotPath) {
|
||||
return JSON.parse(await fsPromises.readFile(robotPath, 'utf8'));
|
||||
async function readRobot(_robotPath) {
|
||||
return fetchRobot();
|
||||
}
|
||||
|
||||
async function writeRobot(robotPath, data) {
|
||||
await fsPromises.writeFile(robotPath, JSON.stringify(data, null, 2), 'utf8');
|
||||
async function writeRobot(_robotPath, data) {
|
||||
return pushRobot(data);
|
||||
}
|
||||
|
||||
// ── Aktion 1: Marker nach Z-Bereich zuordnen ─────────────────────────────────
|
||||
@@ -374,6 +374,12 @@ export async function assignMarkerId(robotPath, { markerId, set, link, extraMark
|
||||
return { changed: false, error: 'Link muss angegeben werden, um einen neuen Marker hinzuzufügen.' };
|
||||
}
|
||||
|
||||
if (!Array.isArray(em.position_mm)) {
|
||||
return {
|
||||
changed: false,
|
||||
error: `Marker ${id} hat keine triangulierte Position (position_mm fehlt – z.B. Einzelkamera-Marker).`,
|
||||
};
|
||||
}
|
||||
const newMarker = {
|
||||
id,
|
||||
position: em.position_mm.map(v => Math.round(Number(v) * 100) / 100),
|
||||
|
||||
@@ -151,6 +151,108 @@ export async function runHoming({
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Führt den Homing-Ablauf offline aus (Bilder und NPZ bereits im runDir).
|
||||
* Identische 4b-Kette wie runHoming — ohne Webcam-Zugriff und ohne SSE-Stream.
|
||||
* send() akkumuliert Logs; done-Event trägt den finalen State.
|
||||
*
|
||||
* @param {{
|
||||
* robotJsonPath: string,
|
||||
* runDir: string,
|
||||
* send: (obj: object) => void,
|
||||
* runScript: (args: string[], send: Function) => Promise<number>,
|
||||
* runBoardPipelineOffline:(runDir: string, send: Function) => Promise<void>,
|
||||
* SCRIPT_4B: string,
|
||||
* SCRIPT_5POSE: string,
|
||||
* }} opts
|
||||
*/
|
||||
export async function runHomingOffline({
|
||||
robotJsonPath,
|
||||
runDir,
|
||||
send,
|
||||
runScript,
|
||||
runBoardPipelineOffline,
|
||||
SCRIPT_4B,
|
||||
SCRIPT_5POSE,
|
||||
}) {
|
||||
send({ type: 'log', text: `▶ Homing-Offline: ${path.basename(runDir)}` });
|
||||
send({ type: 'log', text: `▶ Robot-JSON: ${robotJsonPath}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
// ── Schritt 1: Marker-Triangulierung (Bilder liegen bereits im runDir) ──────
|
||||
send({ type: 'step', step: 1, total: 5, text: 'Marker-Triangulierung …' });
|
||||
await runBoardPipelineOffline(runDir, send);
|
||||
|
||||
const arucoJson = path.join(runDir, 'aruco_marker_poses.json');
|
||||
try {
|
||||
await fsPromises.access(arucoJson);
|
||||
} catch {
|
||||
send({ type: 'error', text: '❌ aruco_marker_poses.json fehlt – Script 3b hat nicht funktioniert.' });
|
||||
send({ type: 'done', exitCode: -1 });
|
||||
return;
|
||||
}
|
||||
|
||||
// ── Schritt 2: X-Position schätzen ──────────────────────────────────────────
|
||||
const xMm = estimateXFromMarkers(arucoJson, robotJsonPath);
|
||||
send({ type: 'log', text: `▶ Geschätzte X-Position: ${xMm.toFixed(1)} mm` });
|
||||
send({ type: 'analysis', key: 'x_mm', value: xMm });
|
||||
|
||||
// ── Schritte 3–5 (2–4): 4b-Kette Arm1 → Ellbow → Arm2 → Hand ───────────────
|
||||
const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand'];
|
||||
let fromState = null;
|
||||
let chainComplete = true;
|
||||
|
||||
for (let i = 0; i < links.length; i++) {
|
||||
const link = links[i];
|
||||
send({ type: 'step', step: 2 + i, total: 5, text: `Gelenkwinkel ${link} …` });
|
||||
send({ type: 'log', text: `\n─── 4b: ${link} ${'─'.repeat(35 - link.length)}` });
|
||||
|
||||
const outputPath = path.join(runDir, `state_${link}.json`);
|
||||
const args = [SCRIPT_4B, '--robot', robotJsonPath, '--aruco', arucoJson, '--link', link, '--output', outputPath];
|
||||
if (fromState) args.push('--from-state', fromState);
|
||||
else args.push('--x-mm', String(xMm));
|
||||
|
||||
const exit = await runScript(args, send);
|
||||
if (exit !== 0) {
|
||||
send({ type: 'log', text: `⚠ 4b ${link} Exit ${exit} — falle auf 5_pose_estimation.py zurück` });
|
||||
chainComplete = false;
|
||||
break;
|
||||
}
|
||||
fromState = outputPath;
|
||||
|
||||
try {
|
||||
const stateData = JSON.parse(await fsPromises.readFile(outputPath, 'utf8'));
|
||||
send({ type: 'analysis', key: `state_${link}`, value: stateData.accumulated_state ?? stateData });
|
||||
} catch { /* ignorieren */ }
|
||||
}
|
||||
|
||||
// ── Endergebnis ──────────────────────────────────────────────────────────────
|
||||
try {
|
||||
let finalState;
|
||||
if (chainComplete) {
|
||||
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
|
||||
finalState = finalData.accumulated_state ?? finalData;
|
||||
} else {
|
||||
send({ type: 'step', step: 5, total: 5, text: '5_pose_estimation.py (Fallback) …' });
|
||||
const poseOut = path.join(runDir, 'robot_state.json');
|
||||
const args = [SCRIPT_5POSE, arucoJson, '-robot', robotJsonPath, '-out', poseOut];
|
||||
if (fromState) args.push('--from-state', fromState);
|
||||
const exit = await runScript(args, send);
|
||||
if (exit !== 0) throw new Error(`5_pose_estimation.py Exit ${exit}`);
|
||||
const poseData = JSON.parse(await fsPromises.readFile(poseOut, 'utf8'));
|
||||
finalState = Object.fromEntries(
|
||||
Object.entries(poseData.movements).map(([k, v]) => [k, v.value])
|
||||
);
|
||||
}
|
||||
send({ type: 'log', text: '' });
|
||||
send({ type: 'log', text: '✅ Homing-Offline abgeschlossen' });
|
||||
send({ type: 'done', exitCode: 0, state: finalState });
|
||||
} catch (err) {
|
||||
send({ type: 'error', text: `❌ Endzustand konnte nicht gelesen werden: ${err.message}` });
|
||||
send({ type: 'done', exitCode: -1 });
|
||||
}
|
||||
}
|
||||
|
||||
/** Timestamp-String YYYYMMDD_HHmmss */
|
||||
function makeTimestamp() {
|
||||
const now = new Date();
|
||||
|
||||
44
server/robotConfig.js
Normal file
44
server/robotConfig.js
Normal file
@@ -0,0 +1,44 @@
|
||||
import fsPromises from 'fs/promises';
|
||||
import path from 'path';
|
||||
import { fileURLToPath } from 'url';
|
||||
|
||||
const __dirname = path.dirname(fileURLToPath(import.meta.url));
|
||||
|
||||
const ROBOT_URL = process.env.ROBOT_URL || '';
|
||||
const ROBOT_JSON = process.env.ROBOT_JSON
|
||||
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
|
||||
|
||||
/**
|
||||
* Lädt robot.json.
|
||||
* Reihenfolge: (1) ROBOT_URL/api/robot/config, (2) lokale Datei als Fallback.
|
||||
* Schreibt das Ergebnis immer in die lokale Cache-Datei (für Python-Skripte).
|
||||
*/
|
||||
export async function fetchRobot() {
|
||||
if (ROBOT_URL) {
|
||||
const res = await fetch(new URL('/api/robot/config', ROBOT_URL));
|
||||
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
|
||||
const data = await res.json();
|
||||
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
|
||||
return data;
|
||||
}
|
||||
return JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
|
||||
}
|
||||
|
||||
/**
|
||||
* Speichert robot.json.
|
||||
* Schreibt immer in lokale Cache-Datei; sendet zusätzlich an Driver wenn konfiguriert.
|
||||
*/
|
||||
export async function pushRobot(data) {
|
||||
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
|
||||
if (ROBOT_URL) {
|
||||
const res = await fetch(new URL('/api/robot/config', ROBOT_URL), {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(data),
|
||||
});
|
||||
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
|
||||
}
|
||||
}
|
||||
|
||||
/** Pfad zur lokalen Cache-Datei – wird an Python-Skripte als -robot-Argument übergeben. */
|
||||
export const robotCachePath = ROBOT_JSON;
|
||||
369
server/server.js
369
server/server.js
@@ -9,7 +9,11 @@ import process from 'process';
|
||||
import { spawn } from 'child_process';
|
||||
import { WebcamClient } from './webcamClient.js';
|
||||
import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis, assignFixedMarkersToLink, setJointOriginYZ, setArmMarkerSpin } from './editRobot.js';
|
||||
import { runHoming } from './homingOrchestrator.js';
|
||||
import multer from 'multer';
|
||||
import { runHoming, runHomingOffline } from './homingOrchestrator.js';
|
||||
import { fetchRobot, robotCachePath } from './robotConfig.js';
|
||||
import { sendGcode, isDriverConfigured } from './driverClient.js';
|
||||
import { buildG92 } from './buildG92.cjs';
|
||||
|
||||
const __filename = fileURLToPath(import.meta.url);
|
||||
const __dirname = path.dirname(__filename);
|
||||
@@ -22,7 +26,8 @@ const publicDir = path.join(__dirname, '..', 'public');
|
||||
const snapshotsDir = path.join(publicDir, 'snapshots');
|
||||
const WEBCAM_URL = process.env.WEBCAM_URL || '';
|
||||
const BODYTRACKER_URL = process.env.BODYTRACKER_URL || '';
|
||||
const ROBOT_URL = process.env.ROBOT_URL || '';
|
||||
// Roboter-Transport läuft über den Driver-WebSocket (DRIVER_WS_URL,
|
||||
// server/driverClient.js), nicht mehr über HTTP ROBOT_URL.
|
||||
const HTTPS_KEY_PATH = process.env.HTTPS_KEY_PATH || path.join(__dirname, '..', 'https', 'localhost.key');
|
||||
const HTTPS_CERT_PATH = process.env.HTTPS_CERT_PATH || path.join(__dirname, '..', 'https', 'localhost.pem');
|
||||
const HTTPS_PASSPHRASE = process.env.HTTPS_PASSPHRASE || 'abcd';
|
||||
@@ -437,10 +442,9 @@ app.post('/api/calibration/compute', async (req, res) => {
|
||||
|
||||
// ── Board-Erkennung ───────────────────────────────────────────────────────────
|
||||
|
||||
const boardDataDir = path.join(__dirname, '..', 'data', 'board');
|
||||
const homingDataDir = path.join(__dirname, '..', 'data', 'homing');
|
||||
const ROBOT_JSON = process.env.ROBOT_JSON
|
||||
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
|
||||
const boardDataDir = path.join(__dirname, '..', 'data', 'board');
|
||||
const homingDataDir = path.join(__dirname, '..', 'data', 'homing');
|
||||
const homingOfflineDataDir = path.join(__dirname, '..', 'data', 'homing-offline');
|
||||
const SCRIPT_1 = path.join(__dirname, '..', 'scripts', '1_detect_aruco_observations.py');
|
||||
const SCRIPT_2 = path.join(__dirname, '..', 'scripts', '2_estimate_camera_from_observations.py');
|
||||
const SCRIPT_3B = path.join(__dirname, '..', 'scripts', '3b_corner_marker_poses.py');
|
||||
@@ -496,6 +500,12 @@ function runScript(args, send) {
|
||||
* @param {{ refSet?: string }} [opts]
|
||||
*/
|
||||
async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
try {
|
||||
await fetchRobot();
|
||||
} catch (err) {
|
||||
send({ type: 'log', text: `⚠ robot.json-Cache: Driver nicht erreichbar – nutze lokale Datei (${err.message})` });
|
||||
}
|
||||
|
||||
// Kameras ermitteln
|
||||
if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert');
|
||||
const camData = await new WebcamClient(WEBCAM_URL).getCameras();
|
||||
@@ -503,24 +513,31 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
// Pro Kamera: Foto → Script 1 → Script 2
|
||||
for (const camId of cameraIds) {
|
||||
// Phase 1: alle Kameras gleichzeitig fotografieren (Modus-Umschaltung parallel)
|
||||
send({ type: 'log', text: 'Fotos aufnehmen …' });
|
||||
const snapResults = await Promise.all(
|
||||
cameraIds.map(async (camId) => {
|
||||
let snapResp;
|
||||
for (let attempt = 1; attempt <= 2; attempt++) {
|
||||
snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
|
||||
if (snapResp.status !== 503) break;
|
||||
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
|
||||
}
|
||||
if (!snapResp.ok) return { camId, imgPath: null, error: `HTTP ${snapResp.status}` };
|
||||
const imgPath = path.join(runDir, `${camId}.jpg`);
|
||||
await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer()));
|
||||
return { camId, imgPath, error: null };
|
||||
})
|
||||
);
|
||||
|
||||
// Phase 2: Scripts 1 + 2 pro Kamera (sequenziell, damit Logs lesbar bleiben)
|
||||
for (const { camId, imgPath, error } of snapResults) {
|
||||
send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` });
|
||||
|
||||
// Snapshot
|
||||
send({ type: 'log', text: 'Foto aufnehmen …' });
|
||||
let snapResp;
|
||||
for (let attempt = 1; attempt <= 2; attempt++) {
|
||||
snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true);
|
||||
if (snapResp.status !== 503) break;
|
||||
if (attempt < 2) await new Promise(r => setTimeout(r, 2000));
|
||||
}
|
||||
if (!snapResp.ok) {
|
||||
send({ type: 'log', text: `⚠ HTTP ${snapResp.status} – Kamera übersprungen` });
|
||||
if (error) {
|
||||
send({ type: 'log', text: `⚠ ${error} – Kamera übersprungen` });
|
||||
continue;
|
||||
}
|
||||
const imgPath = path.join(runDir, `${camId}.jpg`);
|
||||
await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer()));
|
||||
send({ type: 'log', text: `✅ Foto: ${camId}.jpg` });
|
||||
|
||||
// NPZ suchen – neueste Session, die eine NPZ für diese Kamera enthält
|
||||
@@ -538,7 +555,7 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
SCRIPT_1,
|
||||
'-i', imgPath,
|
||||
'-npz', npzPath,
|
||||
'-robot', ROBOT_JSON,
|
||||
'-robot', robotCachePath,
|
||||
'-cameraId', camId,
|
||||
'-outDir', runDir,
|
||||
'--saveDebugImage',
|
||||
@@ -556,7 +573,7 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
continue;
|
||||
}
|
||||
send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' });
|
||||
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', ROBOT_JSON, '-outDir', runDir];
|
||||
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', robotCachePath, '-outDir', runDir];
|
||||
if (refSet) script2Args.push('--refSet', refSet);
|
||||
const exit2 = await runScript(script2Args, send);
|
||||
if (exit2 !== 0) send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` });
|
||||
@@ -574,7 +591,7 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
const exit3b = await runScript([
|
||||
SCRIPT_3B,
|
||||
'--evalDir', runDir,
|
||||
'--robot', ROBOT_JSON,
|
||||
'--robot', robotCachePath,
|
||||
], send);
|
||||
if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` });
|
||||
} else {
|
||||
@@ -583,6 +600,120 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) {
|
||||
send({ type: 'log', text: '' });
|
||||
}
|
||||
|
||||
/**
|
||||
* Board-Pipeline für Offline-Homing: Bilder und NPZs liegen bereits im runDir.
|
||||
* Kein Webcam-Zugriff, keine NPZ-Suche — Scripts 1, 2, 3b werden identisch aufgerufen.
|
||||
*
|
||||
* Dateinamen-Konvention im runDir:
|
||||
* {cameraId}.jpg – Kamerabild
|
||||
* {cameraId}_calibration.npz – Kalibrierung
|
||||
* robot_run.json – robot.json für diesen Lauf
|
||||
*
|
||||
* @param {string} runDir
|
||||
* @param {Function} send
|
||||
* @param {{ refSet?: string }} [opts]
|
||||
*/
|
||||
async function runBoardPipelineOffline(runDir, send, { refSet } = {}) {
|
||||
const robotRunPath = path.join(runDir, 'robot_run.json');
|
||||
|
||||
const allFiles = await fsPromises.readdir(runDir);
|
||||
const cameraIds = allFiles
|
||||
.filter(f => /^[a-zA-Z0-9]+\.jpg$/.test(f))
|
||||
.map(f => path.basename(f, '.jpg'))
|
||||
.sort();
|
||||
|
||||
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
|
||||
send({ type: 'log', text: '' });
|
||||
|
||||
for (const camId of cameraIds) {
|
||||
send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` });
|
||||
|
||||
const imgPath = path.join(runDir, `${camId}.jpg`);
|
||||
const npzPath = path.join(runDir, `${camId}_calibration.npz`);
|
||||
|
||||
try { await fsPromises.access(npzPath); } catch {
|
||||
send({ type: 'log', text: `⚠ Keine NPZ für ${camId} – übersprungen` });
|
||||
continue;
|
||||
}
|
||||
|
||||
send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' });
|
||||
const exit1 = await runScript([
|
||||
SCRIPT_1,
|
||||
'-i', imgPath,
|
||||
'-npz', npzPath,
|
||||
'-robot', robotRunPath,
|
||||
'-cameraId', camId,
|
||||
'-outDir', runDir,
|
||||
'--saveDebugImage',
|
||||
], send);
|
||||
if (exit1 !== 0) {
|
||||
send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` });
|
||||
continue;
|
||||
}
|
||||
|
||||
const detJson = path.join(runDir, `${camId}_aruco_detection.json`);
|
||||
try { await fsPromises.access(detJson); } catch {
|
||||
send({ type: 'log', text: '⚠ Detection-JSON fehlt – Script 2 übersprungen' });
|
||||
continue;
|
||||
}
|
||||
send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' });
|
||||
const script2Args = [SCRIPT_2, '-i', detJson, '-robot', robotRunPath, '-outDir', runDir];
|
||||
if (refSet) script2Args.push('--refSet', refSet);
|
||||
const exit2 = await runScript(script2Args, send);
|
||||
if (exit2 !== 0) send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` });
|
||||
|
||||
send({ type: 'log', text: '' });
|
||||
}
|
||||
|
||||
send({ type: 'log', text: '─── 3b: Marker-Triangulierung ────────────────────────────' });
|
||||
const runFiles3b = await fsPromises.readdir(runDir);
|
||||
const numPoses = runFiles3b.filter(f => f.endsWith('_camera_pose.json')).length;
|
||||
if (numPoses >= 2) {
|
||||
send({ type: 'log', text: `▷ 3b_corner_marker_poses (${numPoses} Kamera-Posen)` });
|
||||
const exit3b = await runScript([SCRIPT_3B, '--evalDir', runDir, '--robot', robotRunPath], send);
|
||||
if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` });
|
||||
} else {
|
||||
send({ type: 'log', text: `⚠ Nur ${numPoses} Kamera-Pose(n) – Script 3b braucht ≥2 Kameras` });
|
||||
}
|
||||
send({ type: 'log', text: '' });
|
||||
}
|
||||
|
||||
// ── Multer-Setup für Offline-Homing ──────────────────────────────────────────
|
||||
|
||||
async function prepareOfflineRunDir(req, res, next) {
|
||||
try {
|
||||
const ts = makeTimestamp();
|
||||
const runDir = path.join(homingOfflineDataDir, ts);
|
||||
await fsPromises.mkdir(runDir, { recursive: true });
|
||||
req.offlineRunDir = runDir;
|
||||
req.offlineTs = ts;
|
||||
next();
|
||||
} catch (err) {
|
||||
res.status(500).json({ error: String(err) });
|
||||
}
|
||||
}
|
||||
|
||||
const offlineMulter = multer({
|
||||
storage: multer.diskStorage({
|
||||
destination: (req, file, cb) => cb(null, req.offlineRunDir),
|
||||
filename: (req, file, cb) => {
|
||||
const safe = path.basename(file.originalname).replace(/[^a-zA-Z0-9_.-]/g, '_');
|
||||
cb(null, safe);
|
||||
},
|
||||
}),
|
||||
}).fields([
|
||||
{ name: 'images', maxCount: 10 },
|
||||
{ name: 'calibrations', maxCount: 10 },
|
||||
{ name: 'robot', maxCount: 1 },
|
||||
]);
|
||||
|
||||
function runOfflineUpload(req, res, next) {
|
||||
offlineMulter(req, res, (err) => {
|
||||
if (err) return res.status(400).json({ error: `Upload-Fehler: ${err.message}` });
|
||||
next();
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* POST /api/board/run
|
||||
* 1. Erstellt data/board/{timestamp}/
|
||||
@@ -612,13 +743,13 @@ app.post('/api/board/run', async (req, res) => {
|
||||
|
||||
// Robot-JSON laden und Marker-Anzahl loggen
|
||||
let robotData = null;
|
||||
try { robotData = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8')); } catch {}
|
||||
try { robotData = JSON.parse(await fsPromises.readFile(robotCachePath, 'utf8')); } catch {}
|
||||
const boardMarkers = robotData?.links?.Board?.markers ?? [];
|
||||
const boardMarkerCount = boardMarkers.length;
|
||||
const refMarkerCount = refSet
|
||||
? boardMarkers.filter(m => m.set === refSet).length
|
||||
: boardMarkerCount;
|
||||
send({ type: 'log', text: `▶ Robot-JSON: ${ROBOT_JSON}` });
|
||||
send({ type: 'log', text: `▶ Robot-JSON: ${robotCachePath}` });
|
||||
send({ type: 'log', text: `▶ Board-Marker: ${boardMarkerCount} (links.Board.markers)` });
|
||||
send({ type: 'log', text: `▶ Referenz-Set: ${refSet ? `"${refSet}" (${refMarkerCount} Marker)` : 'alle'}` });
|
||||
send({ type: 'log', text: '' });
|
||||
@@ -703,7 +834,7 @@ app.get('/api/board/latest', async (req, res) => {
|
||||
const runDir = path.join(dataDir, runName);
|
||||
|
||||
let robot = null;
|
||||
try { robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8')); } catch {}
|
||||
try { robot = JSON.parse(await fsPromises.readFile(robotCachePath, 'utf8')); } catch {}
|
||||
|
||||
let files = [];
|
||||
try { files = await fsPromises.readdir(runDir); } catch {}
|
||||
@@ -746,7 +877,7 @@ app.get('/api/board/latest', async (req, res) => {
|
||||
measuredMarkers = JSON.parse(raw);
|
||||
} catch {}
|
||||
|
||||
return res.json({ runDir: runName, robotFile: path.basename(ROBOT_JSON), robot, detections, cameraPoses, measuredMarkers });
|
||||
return res.json({ runDir: runName, robotFile: path.basename(robotCachePath), robot, detections, cameraPoses, measuredMarkers });
|
||||
} catch (err) {
|
||||
return res.status(500).json({ error: String(err) });
|
||||
}
|
||||
@@ -771,7 +902,7 @@ app.post('/api/homing/run', async (req, res) => {
|
||||
try {
|
||||
await fsPromises.mkdir(homingDataDir, { recursive: true });
|
||||
await runHoming({
|
||||
robotJsonPath: ROBOT_JSON,
|
||||
robotJsonPath: robotCachePath,
|
||||
homingDir: homingDataDir,
|
||||
send,
|
||||
runScript,
|
||||
@@ -789,31 +920,70 @@ app.post('/api/homing/run', async (req, res) => {
|
||||
if (!res.writableEnded) res.end();
|
||||
});
|
||||
|
||||
/**
|
||||
* Konvertiert den FK-State (von 4b_revolute_angle.py / 5_pose_estimation.py)
|
||||
* in die G92-Driver-Konvention (appRobotDriver/doc/Info_G92.md).
|
||||
*
|
||||
* Unterschiede:
|
||||
* b: FK b=0 = gerade Hand; Driver B=180° = gerade Hand → B = 180 − b
|
||||
* c: FK c=0 = neutral Roll; Driver C=90° = neutral → C = c + 90
|
||||
* z: 4b misst Ellbogen RELATIV zu Arm1; Driver braucht absoluten Winkel → Z = y + z
|
||||
*/
|
||||
function fkStateToDriverG92(s) {
|
||||
const d = { ...s };
|
||||
if (d.b != null) d.b = 180 - d.b;
|
||||
if (d.c != null) d.c = d.c + 90;
|
||||
if (d.z != null && d.y != null) d.z = d.y + d.z;
|
||||
return d;
|
||||
}
|
||||
|
||||
/**
|
||||
* POST /api/homing/send-state
|
||||
* Sendet { state: { x, y, z, a, b, c, e } } an ROBOT_URL/api/state.
|
||||
* Baut aus { state: { x, y, z, a, b[, c, e] } } ein G92 und sendet es als
|
||||
* Plain-Text-G-Code über den Driver-WebSocket (DRIVER_WS_URL). G92 setzt am
|
||||
* Driver die Motorposition ohne Bewegung (intern M92) = Homing.
|
||||
* Bekannte Achsen werden real gesendet; wirklich fehlende/unbeobachtbare
|
||||
* Achsen (z.B. c/Palm, e/Greifer in der 4b-Kette) werden weggelassen — der
|
||||
* Driver lässt sie unverändert (siehe server/buildG92.cjs).
|
||||
*/
|
||||
app.post('/api/homing/send-state', async (req, res) => {
|
||||
try {
|
||||
const { state } = req.body ?? {};
|
||||
if (!state) return res.status(400).json({ error: '"state" fehlt' });
|
||||
if (!ROBOT_URL) return res.status(501).json({ error: 'ROBOT_URL ist nicht konfiguriert' });
|
||||
if (!isDriverConfigured())
|
||||
return res.status(501).json({ error: 'DRIVER_WS_URL ist nicht konfiguriert' });
|
||||
|
||||
const url = new URL('/api/state', ROBOT_URL).toString();
|
||||
const upstream = await fetch(url, {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(state),
|
||||
});
|
||||
if (!upstream.ok) {
|
||||
const text = await upstream.text();
|
||||
return res.status(upstream.status).json({ error: `Robot-Fehler: ${text}` });
|
||||
}
|
||||
const result = await upstream.json().catch(() => ({}));
|
||||
return res.json({ ok: true, result });
|
||||
const gcode = buildG92(fkStateToDriverG92(state));
|
||||
const result = await sendGcode(gcode);
|
||||
if (!result.ok)
|
||||
return res.status(502).json({ error: `Robot-Fehler: ${result.error}`, gcode });
|
||||
return res.json({ ok: true, gcode, result: result.response, note: result.note });
|
||||
} catch (err) {
|
||||
console.error('homing/send-state error:', err);
|
||||
return res.status(500).json({ error: String(err) });
|
||||
return res.status(err.statusCode || 500).json({ error: String(err.message || err) });
|
||||
}
|
||||
});
|
||||
|
||||
/**
|
||||
* POST /api/robot/gcode { line: "G92 X… Y…" }
|
||||
* Sendet eine beliebige G-Code-Zeile über den Driver-WebSocket. Transport für
|
||||
* die G-Code-/Befehl-Buttons im Frontend (window.sendCommand) — ersetzt den
|
||||
* toten WSS-Altpfad.
|
||||
*/
|
||||
app.post('/api/robot/gcode', async (req, res) => {
|
||||
try {
|
||||
const line = (req.body?.line ?? '').toString().trim();
|
||||
if (!line) return res.status(400).json({ error: '"line" fehlt' });
|
||||
if (!isDriverConfigured())
|
||||
return res.status(501).json({ error: 'DRIVER_WS_URL ist nicht konfiguriert' });
|
||||
|
||||
const result = await sendGcode(line);
|
||||
if (!result.ok)
|
||||
return res.status(502).json({ error: `Robot-Fehler: ${result.error}`, line });
|
||||
return res.json({ ok: true, line, result: result.response, note: result.note });
|
||||
} catch (err) {
|
||||
console.error('robot/gcode error:', err);
|
||||
return res.status(err.statusCode || 500).json({ error: String(err.message || err) });
|
||||
}
|
||||
});
|
||||
|
||||
@@ -861,6 +1031,92 @@ app.get('/api/homing/run-data', async (req, res) => {
|
||||
}
|
||||
});
|
||||
|
||||
/**
|
||||
* POST /api/homing/run-offline
|
||||
* Vollständiger Homing-Ablauf ohne Live-Kameras.
|
||||
* Bilder, NPZs und robot.json werden per multipart/form-data hochgeladen.
|
||||
* Antwortet synchron mit { ok, runDir, state, files, log }.
|
||||
*
|
||||
* Felder:
|
||||
* images – ein oder mehrere JPEG-Dateien, Name muss {cameraId}.jpg sein
|
||||
* calibrations – je eine NPZ pro Kamera, Name muss {cameraId}_calibration.npz sein
|
||||
* robot – robot.json für diesen Lauf (einmalig, wird nicht dauerhaft gespeichert)
|
||||
* refSet – (Text, optional) Referenz-Set für Script 2, z. B. "A0"
|
||||
*/
|
||||
app.post('/api/homing/run-offline',
|
||||
prepareOfflineRunDir,
|
||||
runOfflineUpload,
|
||||
async (req, res) => {
|
||||
const runDir = req.offlineRunDir;
|
||||
const ts = req.offlineTs;
|
||||
const log = [];
|
||||
|
||||
// robot.json validieren und als robot_run.json speichern
|
||||
const robotFile = req.files?.robot?.[0];
|
||||
if (!robotFile) {
|
||||
return res.status(400).json({ error: '"robot" fehlt – robot.json muss hochgeladen werden', log });
|
||||
}
|
||||
let robotRunPath;
|
||||
try {
|
||||
const content = await fsPromises.readFile(robotFile.path, 'utf8');
|
||||
JSON.parse(content); // Syntaxprüfung
|
||||
robotRunPath = path.join(runDir, 'robot_run.json');
|
||||
await fsPromises.rename(robotFile.path, robotRunPath);
|
||||
} catch (err) {
|
||||
return res.status(400).json({ error: `robot.json ungültig: ${err.message}`, log });
|
||||
}
|
||||
|
||||
// Mindestens ein Bild erforderlich
|
||||
if (!req.files?.images?.length) {
|
||||
return res.status(400).json({ error: 'Mindestens ein Bild ("images") fehlt', log });
|
||||
}
|
||||
|
||||
const refSet = req.body?.refSet || undefined;
|
||||
|
||||
// Logs und done-Event akkumulieren
|
||||
let finalState = null;
|
||||
let exitCode = -1;
|
||||
const send = (obj) => {
|
||||
if (obj.type === 'log') log.push(obj.text);
|
||||
if (obj.type === 'done') { finalState = obj.state ?? null; exitCode = obj.exitCode; }
|
||||
};
|
||||
|
||||
try {
|
||||
await runHomingOffline({
|
||||
robotJsonPath: robotRunPath,
|
||||
runDir,
|
||||
send,
|
||||
runScript,
|
||||
runBoardPipelineOffline: (dir, s) => runBoardPipelineOffline(dir, s, { refSet }),
|
||||
SCRIPT_4B,
|
||||
SCRIPT_5POSE,
|
||||
});
|
||||
} catch (err) {
|
||||
console.error('homing/run-offline error:', err);
|
||||
return res.status(500).json({ error: String(err), log });
|
||||
}
|
||||
|
||||
// Zu wenige Kameras → aruco_marker_poses.json fehlt
|
||||
if (exitCode !== 0) {
|
||||
const arucoExists = await fsPromises.access(path.join(runDir, 'aruco_marker_poses.json'))
|
||||
.then(() => true).catch(() => false);
|
||||
const status = arucoExists ? 500 : 422;
|
||||
return res.status(status).json({ error: 'Homing fehlgeschlagen', log });
|
||||
}
|
||||
|
||||
// Alle JSON-Ausgabedateien einlesen (robot_run.json ausgenommen)
|
||||
const allFiles = await fsPromises.readdir(runDir).catch(() => []);
|
||||
const files = {};
|
||||
for (const f of allFiles.sort()) {
|
||||
if (f.endsWith('.json') && f !== 'robot_run.json') {
|
||||
try { files[f] = JSON.parse(await fsPromises.readFile(path.join(runDir, f), 'utf8')); } catch {}
|
||||
}
|
||||
}
|
||||
|
||||
return res.json({ ok: true, runDir: ts, state: finalState, files, log });
|
||||
}
|
||||
);
|
||||
|
||||
// ── Robot-JSON bearbeiten ─────────────────────────────────────────────────────
|
||||
|
||||
/**
|
||||
@@ -890,7 +1146,7 @@ app.post('/api/robot/assign-by-z', async (req, res) => {
|
||||
}
|
||||
} catch { /* kein 3b-Output vorhanden – nur bestehende robot.json-Marker bearbeiten */ }
|
||||
|
||||
const result = await assignByZRange(ROBOT_JSON, { zMin, zMax, set, link, extraMarkers });
|
||||
const result = await assignByZRange(robotCachePath, { zMin, zMax, set, link, extraMarkers });
|
||||
const added = result.changes.filter(c => c.action === 'added').length;
|
||||
const updated = result.changes.filter(c => c.action === 'updated').length;
|
||||
console.log(`robot/assign-by-z z=[${zMin}..${zMax}] set="${set}" link="${link}" → ${updated} aktualisiert, ${added} neu (von ${extraMarkers.length} 3b-Markern)`);
|
||||
@@ -915,7 +1171,7 @@ app.post('/api/robot/remove-marker', async (req, res) => {
|
||||
if (!['set', 'link'].includes(removeFrom)) {
|
||||
return res.status(400).json({ error: 'removeFrom muss "set" oder "link" sein' });
|
||||
}
|
||||
const result = await removeMarkerAssignment(ROBOT_JSON, { markerId, removeFrom });
|
||||
const result = await removeMarkerAssignment(robotCachePath, { markerId, removeFrom });
|
||||
console.log(`robot/remove-marker id=${markerId} from=${removeFrom} → changed=${result.changed}`);
|
||||
return res.json(result);
|
||||
} catch (err) {
|
||||
@@ -930,7 +1186,7 @@ app.post('/api/robot/remove-marker', async (req, res) => {
|
||||
*/
|
||||
app.get('/api/robot', async (req, res) => {
|
||||
try {
|
||||
const robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
|
||||
const robot = await fetchRobot();
|
||||
return res.json(robot);
|
||||
} catch (err) {
|
||||
return res.status(500).json({ error: String(err) });
|
||||
@@ -944,7 +1200,7 @@ app.get('/api/robot', async (req, res) => {
|
||||
*/
|
||||
app.get('/api/robot/board-sets', async (req, res) => {
|
||||
try {
|
||||
const robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
|
||||
const robot = await fetchRobot();
|
||||
const markers = robot?.links?.Board?.markers ?? [];
|
||||
const sets = [...new Set(markers.map(m => m.set).filter(Boolean))].sort();
|
||||
return res.json({ sets });
|
||||
@@ -975,7 +1231,7 @@ app.post('/api/robot/align-sets', async (req, res) => {
|
||||
}
|
||||
} catch { /* kein 3b-Output vorhanden */ }
|
||||
|
||||
const result = await alignSetToMeasured(ROBOT_JSON, { setToMove, extraMarkers });
|
||||
const result = await alignSetToMeasured(robotCachePath, { setToMove, extraMarkers });
|
||||
if (result.error) return res.status(400).json(result);
|
||||
|
||||
console.log(
|
||||
@@ -1010,7 +1266,7 @@ app.post('/api/robot/assign-id', async (req, res) => {
|
||||
}
|
||||
} catch { /* kein 3b-Output vorhanden */ }
|
||||
|
||||
const result = await assignMarkerId(ROBOT_JSON, { markerId, set, link, extraMarkers });
|
||||
const result = await assignMarkerId(robotCachePath, { markerId, set, link, extraMarkers });
|
||||
if (!result.changed && result.error) return res.status(400).json(result);
|
||||
|
||||
console.log(
|
||||
@@ -1036,7 +1292,7 @@ app.post('/api/robot/adopt-x-axis', async (req, res) => {
|
||||
if (!Array.isArray(direction) || direction.length < 3) {
|
||||
return res.status(400).json({ error: '"direction" muss ein Array [vx,vy,vz] sein.' });
|
||||
}
|
||||
const result = await adoptXAxis(ROBOT_JSON, { direction });
|
||||
const result = await adoptXAxis(robotCachePath, { direction });
|
||||
console.log(
|
||||
`robot/adopt-x-axis dir=[${direction.map(v => Number(v).toFixed(4)).join(', ')}]` +
|
||||
` → ${result.numChanged} Marker, Ursprung=[${result.origin.join(', ')}]` +
|
||||
@@ -1064,7 +1320,7 @@ app.post('/api/robot/assign-fixed-markers', async (req, res) => {
|
||||
if (!targetLink) {
|
||||
return res.status(400).json({ error: '"targetLink" muss angegeben werden.' });
|
||||
}
|
||||
const result = await assignFixedMarkersToLink(ROBOT_JSON, { markerIds, targetLink, measuredPositions });
|
||||
const result = await assignFixedMarkersToLink(robotCachePath, { markerIds, targetLink, measuredPositions });
|
||||
console.log(
|
||||
`robot/assign-fixed-markers [${markerIds.join(',')}] → ${targetLink}` +
|
||||
` added=${result.numAdded} alreadyPresent=${result.numAlreadyPresent}`,
|
||||
@@ -1089,7 +1345,7 @@ app.post('/api/robot/set-joint-origin', async (req, res) => {
|
||||
if (!Number.isFinite(Number(y)) || !Number.isFinite(Number(z))) {
|
||||
return res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' });
|
||||
}
|
||||
const result = await setJointOriginYZ(ROBOT_JSON, { linkName, y: Number(y), z: Number(z) });
|
||||
const result = await setJointOriginYZ(robotCachePath, { linkName, y: Number(y), z: Number(z) });
|
||||
if (!result.changed) {
|
||||
return res.status(400).json({ error: result.error });
|
||||
}
|
||||
@@ -1115,7 +1371,7 @@ app.post('/api/robot/set-arm-marker-spin', async (req, res) => {
|
||||
if (!linkName) return res.status(400).json({ error: '"linkName" muss angegeben werden.' });
|
||||
if (markerId == null) return res.status(400).json({ error: '"markerId" muss angegeben werden.' });
|
||||
if (!Number.isFinite(Number(spin))) return res.status(400).json({ error: '"spin" muss eine Zahl sein.' });
|
||||
const result = await setArmMarkerSpin(ROBOT_JSON, { linkName, markerId, spin: Number(spin) });
|
||||
const result = await setArmMarkerSpin(robotCachePath, { linkName, markerId, spin: Number(spin) });
|
||||
if (!result.changed) return res.status(400).json({ error: result.error });
|
||||
console.log(`robot/set-arm-marker-spin ${linkName}#${markerId}: ${result.oldSpin}° → ${result.newSpin}°`);
|
||||
return res.json(result);
|
||||
@@ -1268,6 +1524,13 @@ async function startServer() {
|
||||
await checkServiceReachability('BODYTRACKER_URL', new URL('/v1/health', BODYTRACKER_URL).toString());
|
||||
}
|
||||
|
||||
try {
|
||||
await fetchRobot();
|
||||
console.log('✅ robot.json geladen und gecacht.');
|
||||
} catch (err) {
|
||||
console.warn(`⚠ robot.json: Driver nicht erreichbar – nutze lokale Datei: ${err.message}`);
|
||||
}
|
||||
|
||||
const server = await createHttpsServer();
|
||||
const isHttps = Boolean(server);
|
||||
const listenServer = server || app;
|
||||
|
||||
215
test/assignMarkerId.test.js
Normal file
215
test/assignMarkerId.test.js
Normal file
@@ -0,0 +1,215 @@
|
||||
/**
|
||||
* assignMarkerId.test.js
|
||||
* ======================
|
||||
* Integration-Test für server/editRobot.js → assignMarkerId().
|
||||
*
|
||||
* Testet insbesondere den Guard für fehlende position_mm (Marker ohne
|
||||
* triangulierte Position, z.B. Einzelkamera-Marker nach Schritt 5).
|
||||
*
|
||||
* Technisch: editRobot.js ist ein ES-Modul — es wird über den dünnen Runner
|
||||
* test/fixtures/runAssignMarkerId.mjs per spawnSync aufgerufen (gleiche
|
||||
* Strategie wie yAxisRotation.test.js für das Python-Skript).
|
||||
* Datei-I/O läuft gegen echte Temp-Dateien (os.tmpdir()).
|
||||
*/
|
||||
|
||||
const { spawnSync } = require('child_process');
|
||||
const os = require('os');
|
||||
const fs = require('fs');
|
||||
const path = require('path');
|
||||
|
||||
const RUNNER = path.join(__dirname, 'fixtures', 'runAssignMarkerId.mjs');
|
||||
|
||||
// ── Hilfsfunktionen ───────────────────────────────────────────────────────────
|
||||
|
||||
function callAssignMarkerId(robotPath, params) {
|
||||
const proc = spawnSync('node', [RUNNER, robotPath, JSON.stringify(params)], {
|
||||
encoding: 'utf-8',
|
||||
});
|
||||
if (proc.error) throw proc.error;
|
||||
const stdout = (proc.stdout ?? '').trim();
|
||||
if (!stdout) throw new Error(`Kein Output.\nstderr: ${proc.stderr}`);
|
||||
return JSON.parse(stdout);
|
||||
}
|
||||
|
||||
function makeTempRobot(content) {
|
||||
const p = path.join(
|
||||
os.tmpdir(),
|
||||
`robot_test_${Date.now()}_${Math.random().toString(36).slice(2)}.json`
|
||||
);
|
||||
fs.writeFileSync(p, JSON.stringify(content, null, 2), 'utf8');
|
||||
return p;
|
||||
}
|
||||
|
||||
const ROBOT_WITH_42 = () => ({
|
||||
links: {
|
||||
Arm1: { markers: [{ id: 42, set: 'A0', position: [100, 200, 300] }] },
|
||||
},
|
||||
});
|
||||
|
||||
const ROBOT_EMPTY = () => ({
|
||||
links: { Arm1: { markers: [] } },
|
||||
});
|
||||
|
||||
// ── Eingabe-Validierung ───────────────────────────────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Eingabe-Validierung', () => {
|
||||
test('ungültige Marker-ID → changed: false', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: -1, link: 'Arm1' });
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/Ungültige Marker-ID/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('kein link für neuen Marker → changed: false', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 99,
|
||||
extraMarkers: [{ marker_id: 99, position_mm: [10, 20, 30] }],
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/Link/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
|
||||
// ── Guard: fehlende position_mm ───────────────────────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Guard: fehlende position_mm (z.B. Einzelkamera-Marker)', () => {
|
||||
test('extraMarker ohne position_mm → changed: false, kein Crash', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 55,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 55 }], // kein position_mm
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/position_mm fehlt/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('extraMarker mit position_mm: null → changed: false, kein Crash', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 56,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 56, position_mm: null }],
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/position_mm fehlt/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('extraMarker mit position_mm als String → changed: false, kein Crash', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 57,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 57, position_mm: '[1,2,3]' }],
|
||||
});
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/position_mm fehlt/);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
|
||||
// ── Normalfall: Marker hinzufügen ─────────────────────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Normalfall: neuen Marker hinzufügen', () => {
|
||||
test('gültige position_mm → changed: true, action: added, Datei geschrieben', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 77,
|
||||
link: 'Arm1',
|
||||
extraMarkers: [{ marker_id: 77, position_mm: [10.1, 20.22, 30.333] }],
|
||||
});
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.action).toBe('added');
|
||||
expect(r.change.markerId).toBe(77);
|
||||
expect(r.change.newLink).toBe('Arm1');
|
||||
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
const added = saved.links.Arm1.markers.find(m => m.id === 77);
|
||||
expect(added).toBeDefined();
|
||||
expect(Array.isArray(added.position)).toBe(true);
|
||||
expect(added.position).toHaveLength(3);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('Marker nicht in extraMarkers → changed: false', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: 99, link: 'Arm1', extraMarkers: [] });
|
||||
expect(r.changed).toBe(false);
|
||||
expect(r.error).toMatch(/nicht.*vorhanden/i);
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('mit set → Marker hat set-Wert in der gespeicherten Datei', () => {
|
||||
const p = makeTempRobot(ROBOT_EMPTY());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 78,
|
||||
link: 'Arm1',
|
||||
set: 'A0',
|
||||
extraMarkers: [{ marker_id: 78, position_mm: [1, 2, 3] }],
|
||||
});
|
||||
expect(r.changed).toBe(true);
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
const added = saved.links.Arm1.markers.find(m => m.id === 78);
|
||||
expect(added.set).toBe('A0');
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
|
||||
// ── Normalfall: bestehenden Marker aktualisieren ──────────────────────────────
|
||||
|
||||
describe('assignMarkerId – Normalfall: bestehenden Marker aktualisieren', () => {
|
||||
test('Set ändern → changed: true, action: updated, Datei geändert', () => {
|
||||
const p = makeTempRobot(ROBOT_WITH_42());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: 42, set: 'B0' });
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.action).toBe('updated');
|
||||
expect(r.change.oldSet).toBe('A0');
|
||||
expect(r.change.newSet).toBe('B0');
|
||||
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
expect(saved.links.Arm1.markers.find(m => m.id === 42).set).toBe('B0');
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('in anderen Link verschieben → oldLink / newLink korrekt, Datei geändert', () => {
|
||||
const p = makeTempRobot(ROBOT_WITH_42());
|
||||
try {
|
||||
const r = callAssignMarkerId(p, { markerId: 42, link: 'Arm2' });
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.oldLink).toBe('Arm1');
|
||||
expect(r.change.newLink).toBe('Arm2');
|
||||
|
||||
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
|
||||
expect(saved.links.Arm1.markers.find(m => m.id === 42)).toBeUndefined();
|
||||
expect(saved.links.Arm2.markers.find(m => m.id === 42)).toBeDefined();
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
|
||||
test('bestehender Marker: fehlende position_mm in extraMarkers ist irrelevant', () => {
|
||||
const p = makeTempRobot(ROBOT_WITH_42());
|
||||
try {
|
||||
// Marker 42 ist in robot.json → position_mm-Guard darf nicht zuschlagen
|
||||
const r = callAssignMarkerId(p, {
|
||||
markerId: 42,
|
||||
set: 'C0',
|
||||
extraMarkers: [{ marker_id: 42 }], // kein position_mm – aber irrelevant
|
||||
});
|
||||
expect(r.changed).toBe(true);
|
||||
expect(r.change.action).toBe('updated');
|
||||
} finally { fs.unlinkSync(p); }
|
||||
});
|
||||
});
|
||||
59
test/boardViewerHasXYZ.test.js
Normal file
59
test/boardViewerHasXYZ.test.js
Normal file
@@ -0,0 +1,59 @@
|
||||
/**
|
||||
* boardViewerHasXYZ.test.js
|
||||
* =========================
|
||||
* Unit-Test für die reine Hilfsfunktion `hasXYZ()` aus public/boardViewer.html.
|
||||
*
|
||||
* boardViewer.html ist kein ladbares JS-Modul (Inline-<script type="module">
|
||||
* mit THREE.js/DOM/fetch-Abhängigkeiten) — ein normales require() ist daher
|
||||
* nicht möglich, ohne die Datei in Module aufzuteilen (nicht Teil dieser
|
||||
* Änderung). Stattdessen wird die `hasXYZ`-Funktionsdefinition per Regex aus
|
||||
* der Datei extrahiert und isoliert ausgewertet — testet exakt die Guard-Logik,
|
||||
* die an allen position_mm-Zugriffsstellen in boardViewer.html verwendet wird,
|
||||
* ohne Three.js/DOM laden zu müssen.
|
||||
*/
|
||||
|
||||
const fs = require('fs');
|
||||
const path = require('path');
|
||||
|
||||
const SRC = fs.readFileSync(path.join(__dirname, '../public/boardViewer.html'), 'utf8');
|
||||
|
||||
const match = SRC.match(/function hasXYZ\(arr\)\s*\{[^}]*\}/);
|
||||
if (!match) {
|
||||
throw new Error('hasXYZ() nicht in boardViewer.html gefunden — Guard wurde entfernt/umbenannt?');
|
||||
}
|
||||
// eslint-disable-next-line no-eval
|
||||
const hasXYZ = eval(`(${match[0]})`);
|
||||
|
||||
describe('boardViewer.html: hasXYZ() (Guard für fehlende position_mm)', () => {
|
||||
test('gültiges [x,y,z] → true', () => {
|
||||
expect(hasXYZ([1, 2, 3])).toBe(true);
|
||||
expect(hasXYZ([0, 0, 0])).toBe(true);
|
||||
expect(hasXYZ([-1.5, 200.25, 0])).toBe(true);
|
||||
});
|
||||
|
||||
test('undefined/null (fehlendes position_mm) → false, kein Crash', () => {
|
||||
expect(() => hasXYZ(undefined)).not.toThrow();
|
||||
expect(hasXYZ(undefined)).toBe(false);
|
||||
expect(hasXYZ(null)).toBe(false);
|
||||
});
|
||||
|
||||
test('zu kurzes Array → false', () => {
|
||||
expect(hasXYZ([1, 2])).toBe(false);
|
||||
expect(hasXYZ([])).toBe(false);
|
||||
});
|
||||
|
||||
test('nicht-numerische/NaN-Werte → false', () => {
|
||||
expect(hasXYZ([1, NaN, 3])).toBe(false);
|
||||
expect(hasXYZ(['a', 'b', 'c'])).toBe(false);
|
||||
expect(hasXYZ([1, Infinity, 3])).toBe(false);
|
||||
});
|
||||
|
||||
test('kein Array (z.B. Objekt oder String) → false', () => {
|
||||
expect(hasXYZ({ x: 1, y: 2, z: 3 })).toBe(false);
|
||||
expect(hasXYZ('1,2,3')).toBe(false);
|
||||
});
|
||||
|
||||
test('Array mit mehr als 3 Werten (z.B. mit Zusatzfeld) → true (erste 3 zählen)', () => {
|
||||
expect(hasXYZ([1, 2, 3, 4])).toBe(true);
|
||||
});
|
||||
});
|
||||
49
test/buildG92.test.js
Normal file
49
test/buildG92.test.js
Normal file
@@ -0,0 +1,49 @@
|
||||
/**
|
||||
* buildG92.test.js
|
||||
* Unit-Tests für server/buildG92.cjs
|
||||
*
|
||||
* Sichert ab, dass aus dem Homing-State der korrekte G92-String entsteht:
|
||||
* bekannte Achsen werden real gesendet, wirklich fehlende/null-Achsen per
|
||||
* Default WEGGELASSEN (Driver lässt sie unverändert). Achsbuchstaben +
|
||||
* Reihenfolge müssen zur Driver-Erwartung passen
|
||||
* (X→xMotor, Y→alpha, Z→beta, A→a, B→b, C→c, E→e).
|
||||
*/
|
||||
|
||||
const { buildG92 } = require('../server/buildG92.cjs');
|
||||
|
||||
describe('buildG92', () => {
|
||||
test('Fallback-State (alle 7 DOF) → alle Achsen mit realem Wert', () => {
|
||||
const state = { x: 164.57045, y: -2.08983, z: 60.58375, a: 86.75125, b: -46.96569, c: -64.90875, e: 22.58589 };
|
||||
expect(buildG92(state)).toBe('G92 X164.57 Y-2.09 Z60.58 A86.75 B-46.97 C-64.91 E22.59');
|
||||
});
|
||||
|
||||
test('4b-Primärkette (nur x,y,z,a,b) → c/e werden weggelassen', () => {
|
||||
const state = { x: 192.72935, y: 35.99125, z: -30.87771, a: -1.69522, b: 12.34 };
|
||||
expect(buildG92(state)).toBe('G92 X192.73 Y35.99 Z-30.88 A-1.70 B12.34');
|
||||
});
|
||||
|
||||
test('Reihenfolge ist immer x,y,z,a,b,c,e (unabhängig von Key-Reihenfolge)', () => {
|
||||
const state = { b: 1, a: 2, x: 3, e: 4, z: 5, y: 6, c: 7 };
|
||||
expect(buildG92(state)).toBe('G92 X3.00 Y6.00 Z5.00 A2.00 B1.00 C7.00 E4.00');
|
||||
});
|
||||
|
||||
test('null/undefined/NaN-Achsen werden weggelassen (keine falsche 0)', () => {
|
||||
const state = { x: 10, y: null, z: undefined, a: 0, b: NaN, c: 'abc' };
|
||||
expect(buildG92(state)).toBe('G92 X10.00 A0.00');
|
||||
});
|
||||
|
||||
test('fillMissingWithZero=true füllt fehlende Achsen wieder mit 0', () => {
|
||||
const state = { x: 10, y: 20 };
|
||||
expect(buildG92(state, { fillMissingWithZero: true }))
|
||||
.toBe('G92 X10.00 Y20.00 Z0.00 A0.00 B0.00 C0.00 E0.00');
|
||||
});
|
||||
|
||||
test('decimals steuert die Nachkommastellen', () => {
|
||||
expect(buildG92({ x: 1.23456 }, { decimals: 3 })).toBe('G92 X1.235');
|
||||
});
|
||||
|
||||
test('leerer State → "G92 " ohne Achsen', () => {
|
||||
expect(buildG92({})).toBe('G92 ');
|
||||
expect(buildG92()).toBe('G92 ');
|
||||
});
|
||||
});
|
||||
25
test/fixtures/runAssignMarkerId.mjs
vendored
Normal file
25
test/fixtures/runAssignMarkerId.mjs
vendored
Normal file
@@ -0,0 +1,25 @@
|
||||
/**
|
||||
* Dünner Runner für assignMarkerId – wird von assignMarkerId.test.js per spawnSync aufgerufen.
|
||||
*
|
||||
* Argumente:
|
||||
* node runAssignMarkerId.mjs <robotPath> <paramsJson>
|
||||
*
|
||||
* Gibt das Ergebnis als JSON-Zeile auf stdout aus.
|
||||
* Wirft der Aufruf, erscheint { __error: "<message>" } + Exit 1.
|
||||
*/
|
||||
import { assignMarkerId } from '../../server/editRobot.js';
|
||||
|
||||
const [, , robotPath, paramsJson] = process.argv;
|
||||
if (!robotPath || !paramsJson) {
|
||||
process.stderr.write('Usage: runAssignMarkerId.mjs <robotPath> <paramsJson>\n');
|
||||
process.exit(2);
|
||||
}
|
||||
|
||||
try {
|
||||
const params = JSON.parse(paramsJson);
|
||||
const result = await assignMarkerId(robotPath, params);
|
||||
process.stdout.write(JSON.stringify(result) + '\n');
|
||||
} catch (err) {
|
||||
process.stdout.write(JSON.stringify({ __error: err.message }) + '\n');
|
||||
process.exit(1);
|
||||
}
|
||||
169
test/test_robot_fk_corners.py
Normal file
169
test/test_robot_fk_corners.py
Normal file
@@ -0,0 +1,169 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
test_robot_fk_corners.py
|
||||
========================
|
||||
Isolierter Test für RobotFK.marker_corners_world/_local (doc/
|
||||
Homing_5_Pose_MultiPoint_Weighted.md, Schritt 3) — der Baustein für den
|
||||
marker_observation="corner_points"-Modus.
|
||||
|
||||
Kein pytest nötig (das Repo testet sonst per Jest): plain asserts + main(),
|
||||
Aufruf: python test/test_robot_fk_corners.py
|
||||
|
||||
Geprüft wird die KONVENTION (Eckreihenfolge/Spin/Winding), denn ein Versatz
|
||||
dort gäbe systematischen Bias statt Verbesserung:
|
||||
|
||||
A) Selbst-Konsistenz je Marker (versch. Normalen/Spins):
|
||||
- Schwerpunkt der 4 Ecken == Marker-Center
|
||||
- alle 4 Kanten == size, planar
|
||||
- die aus den Ecken via 3b-Konvention (corner_plane_normal) zurück-
|
||||
gewonnene Normale == Marker-Normale → Orientierung + Winding stimmen
|
||||
B) Reale Daten: ROBOTER-Marker (Arm1/Ellbow/Arm2) am Seed-Pose eines echten
|
||||
Captures — die FK-Ecken müssen zu den triangulierten `corners_m` passen,
|
||||
UND die Identitäts-Paarung (Ecke i ↔ Ecke i) muss jede zyklische/
|
||||
gespiegelte Umordnung schlagen → beweist Ecke-0 + Drehrichtung gegen echte
|
||||
Triangulation. Nur Roboter-Marker, weil deren Spin kalibriert/verifiziert
|
||||
ist; die Board/Rail-Marker (Root-Link) sind spin-unkalibriert und werden im
|
||||
Fit bewusst per Center-Residuum behandelt (nicht über Ecken).
|
||||
"""
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
|
||||
HERE = os.path.dirname(os.path.abspath(__file__))
|
||||
sys.path.insert(0, os.path.join(HERE, "..", "scripts"))
|
||||
from robot_fk import RobotFK # noqa: E402
|
||||
|
||||
ROBOT_JSON = os.path.join(HERE, "..", "scripts", "robot_1781069752019.json")
|
||||
CAP_DIR = os.path.join(HERE, "homing", "20260616_133151")
|
||||
CAPTURE = os.path.join(CAP_DIR, "aruco_marker_poses.json")
|
||||
SEED = os.path.join(CAP_DIR, "state_Arm2.json")
|
||||
|
||||
|
||||
def recovered_normal(corners3d: np.ndarray) -> np.ndarray:
|
||||
"""3b_corner_marker_poses.py corner_plane_normal() — identisch nachgebaut."""
|
||||
center = corners3d.mean(axis=0)
|
||||
_, _, Vt = np.linalg.svd(corners3d - center)
|
||||
n = Vt[-1]
|
||||
cross = np.cross(corners3d[1] - corners3d[0], corners3d[2] - corners3d[0])
|
||||
if np.dot(n, cross) > 0:
|
||||
n = -n
|
||||
nn = np.linalg.norm(n)
|
||||
return n / nn if nn > 1e-12 else n
|
||||
|
||||
|
||||
def edge_lengths(c: np.ndarray):
|
||||
return [float(np.linalg.norm(c[(i + 1) % 4] - c[i])) for i in range(4)]
|
||||
|
||||
|
||||
# ── A) Selbst-Konsistenz ────────────────────────────────────────────────────
|
||||
|
||||
def test_self_consistency():
|
||||
fk = RobotFK.from_file(ROBOT_JSON)
|
||||
cases = [
|
||||
([0, 0, 1], 0.0),
|
||||
([0, 0, 1], 90.0),
|
||||
([0, 0, 1], 37.5),
|
||||
([0, -1, 0], 90.0),
|
||||
([-1, 0, 0], 0.0),
|
||||
([0, 0, -1], 12.0), # antiparalleler Sonderfall der Minimal-Rotation
|
||||
([1, 1, 0.3], 215.0), # schräge Normale + großer Spin
|
||||
]
|
||||
size = 25.0
|
||||
pos = np.array([100.0, -50.0, 30.0])
|
||||
for normal, spin in cases:
|
||||
local = RobotFK.marker_corners_local(pos, normal, size, spin)
|
||||
assert local.shape == (4, 3), f"shape {local.shape}"
|
||||
# Schwerpunkt == Center
|
||||
d_center = np.linalg.norm(local.mean(axis=0) - pos)
|
||||
assert d_center < 1e-9, f"centroid off by {d_center} (n={normal}, spin={spin})"
|
||||
# Kanten == size, alle gleich (Quadrat)
|
||||
for L in edge_lengths(local):
|
||||
assert abs(L - size) < 1e-9, f"edge {L} != {size} (n={normal}, spin={spin})"
|
||||
# planar: alle 4 in einer Ebene (Abstand zur SVD-Ebene ~0)
|
||||
rel = local - local.mean(axis=0)
|
||||
_, _, Vt = np.linalg.svd(rel)
|
||||
planar = np.max(np.abs(rel @ Vt[-1]))
|
||||
assert planar < 1e-9, f"not planar ({planar})"
|
||||
# Normale aus den Ecken (3b-Konvention) == Marker-Normale
|
||||
n_exp = np.asarray(normal, float) / np.linalg.norm(normal)
|
||||
n_rec = recovered_normal(local)
|
||||
dot = float(np.dot(n_exp, n_rec))
|
||||
assert dot > 0.99999, (
|
||||
f"recovered normal {n_rec} != {n_exp} (dot={dot:.6f}, spin={spin})")
|
||||
print(f"[PASS] A) Selbst-Konsistenz: {len(cases)} Fälle "
|
||||
"(Center, Kanten, planar, Normalen-Rückgewinnung).")
|
||||
|
||||
|
||||
# ── B) Reale Capture-Daten (Board-Marker) ───────────────────────────────────
|
||||
|
||||
def best_pairing_error(model_c: np.ndarray, obs_c: np.ndarray):
|
||||
"""
|
||||
RMS der Eck-Paarung nach Center-Abzug, für Identität und alle 8 Umordnungen
|
||||
(4 zyklische × {vorwärts, rückwärts}). Gibt (rms_identity, rms_best, name_best).
|
||||
"""
|
||||
m = model_c - model_c.mean(axis=0)
|
||||
o = obs_c - obs_c.mean(axis=0)
|
||||
best_rms, best_name, id_rms = None, None, None
|
||||
for reverse in (False, True):
|
||||
base = o[::-1] if reverse else o
|
||||
for roll in range(4):
|
||||
cand = np.roll(base, roll, axis=0)
|
||||
rms = float(np.sqrt(np.mean(np.sum((m - cand) ** 2, axis=1))))
|
||||
name = f"{'rev' if reverse else 'fwd'}+roll{roll}"
|
||||
if best_rms is None or rms < best_rms:
|
||||
best_rms, best_name = rms, name
|
||||
if not reverse and roll == 0:
|
||||
id_rms = rms
|
||||
return id_rms, best_rms, best_name
|
||||
|
||||
|
||||
def test_against_real_capture():
|
||||
if not (os.path.exists(CAPTURE) and os.path.exists(SEED)):
|
||||
print(f"[SKIP] B) Capture/Seed fehlt: {CAP_DIR}")
|
||||
return
|
||||
fk = RobotFK.from_file(ROBOT_JSON)
|
||||
seed = json.load(open(SEED, "r", encoding="utf-8")).get("accumulated_state", {})
|
||||
transforms = fk.compute(seed) # Roboter-Marker brauchen den Pose-Zustand
|
||||
model = fk.all_markers_world(transforms)
|
||||
|
||||
# Root-Link (Board) bestimmen → davon wird NICHT über Ecken validiert.
|
||||
roots = {ln for ln, ld in fk.links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in fk.links}
|
||||
|
||||
data = json.load(open(CAPTURE, "r", encoding="utf-8"))
|
||||
checked = 0
|
||||
for m in data.get("markers", []):
|
||||
mid = m["marker_id"]
|
||||
if mid not in model or not m.get("corners_m"):
|
||||
continue
|
||||
if model[mid]["link"] in roots: # Board/Rail: bewusst Center-only
|
||||
continue
|
||||
obs_c = np.array(m["corners_m"], dtype=float) * 1000.0 # m → mm
|
||||
model_c = np.asarray(model[mid]["corners_world"], float)
|
||||
# Schlecht lokalisierte/fehlassoziierte Marker (Center weit daneben am
|
||||
# Seed) überspringen — sie sagen nichts über die Eckreihenfolge aus.
|
||||
if np.linalg.norm(model[mid]["world_mm"] - obs_c.mean(axis=0)) > 50.0:
|
||||
continue
|
||||
id_rms, best_rms, best_name = best_pairing_error(model_c, obs_c)
|
||||
# 1) Form/Orientierung stimmt: Identitäts-RMS klein (Seed- + Triangulationsrauschen)
|
||||
assert id_rms < 6.0, f"Roboter-Marker {mid}: Identitäts-RMS {id_rms:.2f}mm zu groß"
|
||||
# 2) KEINE Umordnung ist besser als die Identität → Ecke-0 + Winding korrekt
|
||||
assert best_name == "fwd+roll0", (
|
||||
f"Roboter-Marker {mid}: '{best_name}' (RMS {best_rms:.2f}) schlägt Identität "
|
||||
f"(RMS {id_rms:.2f}) → Eckreihenfolge falsch")
|
||||
checked += 1
|
||||
assert checked >= 4, f"zu wenige verwertbare Roboter-Marker geprüft ({checked})"
|
||||
print(f"[PASS] B) Reale Capture-Daten: {checked} ROBOTER-Marker am Seed — "
|
||||
"FK-Ecken passen zu corners_m, Identitäts-Paarung ist optimal.")
|
||||
|
||||
|
||||
def main():
|
||||
test_self_consistency()
|
||||
test_against_real_capture()
|
||||
print("\n[OK] Alle Tests bestanden.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -193,3 +193,42 @@ describe('computeYAxis – Edge Cases', () => {
|
||||
expect(Math.abs(r.axisDir[2])).toBeGreaterThan(0.99);
|
||||
});
|
||||
});
|
||||
|
||||
// ── Fehlende position_mm (z.B. 1-Kamera-Marker, von 3b nicht trianguliert) ───
|
||||
|
||||
describe('computeYAxis – Marker ohne position_mm werden ignoriert statt zu crashen', () => {
|
||||
|
||||
test('position_mm fehlt bei Pos A → Marker landet in skipped, kein Crash', () => {
|
||||
const a = [{ marker_id: 5 }]; // keine position_mm
|
||||
const b = [{ marker_id: 5, position_mm: [0, 0, 0] }];
|
||||
const c = [{ marker_id: 5, position_mm: [0, 1, 0] }];
|
||||
expect(() => computeYAxis(a, b, c)).not.toThrow();
|
||||
const r = computeYAxis(a, b, c);
|
||||
expect(r.ok).toBe(false);
|
||||
expect(r.skipped.some(s => s.id === 5)).toBe(true);
|
||||
});
|
||||
|
||||
test('position_mm fehlt bei Pos B/C → ebenfalls kein Crash', () => {
|
||||
const a = [{ marker_id: 6, position_mm: [0, 0, 0] }];
|
||||
const b = [{ marker_id: 6, position_mm: null }];
|
||||
const c = [{ marker_id: 6 }];
|
||||
expect(() => computeYAxis(a, b, c)).not.toThrow();
|
||||
});
|
||||
|
||||
test('Mix aus gültigen und ungültigen Markern: gültige werden trotzdem genutzt', () => {
|
||||
const R = 100;
|
||||
const angle = (deg) => deg * Math.PI / 180;
|
||||
const mk = (id, deg) => ({
|
||||
marker_id: id,
|
||||
position_mm: [R * Math.cos(angle(deg)), R * Math.sin(angle(deg)), 50],
|
||||
});
|
||||
const a = [mk(1, 0), { marker_id: 2 /* fehlt */ }];
|
||||
const b = [mk(1, 90), { marker_id: 2, position_mm: [1, 2, 3] }];
|
||||
const c = [mk(1, 180), { marker_id: 2 }]; // bei C wieder fehlend
|
||||
const r = computeYAxis(a, b, c);
|
||||
expect(r.ok).toBe(true);
|
||||
expect(r.numMarkers).toBe(1);
|
||||
expect(r.markerData.map(m => m.markerId)).toEqual([1]);
|
||||
expect(r.skipped.some(s => s.id === 2)).toBe(true);
|
||||
});
|
||||
});
|
||||
|
||||
Reference in New Issue
Block a user