1593 lines
42 KiB
JSON
1593 lines
42 KiB
JSON
{
|
|
"schema_version": "1.0",
|
|
"method": "deterministic_geometric_sequential_prefix",
|
|
"created_utc": "2026-05-29T22:10:49Z",
|
|
"root_pose": {
|
|
"translation_m": [
|
|
-0.00010859211742092478,
|
|
0.00042385507834860614,
|
|
-0.00035770640297937134
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.9999997764000005,
|
|
0.0006263290459170604,
|
|
-0.00023433282914781965
|
|
],
|
|
[
|
|
-0.0006263716246644091,
|
|
0.9999997873267732,
|
|
-0.00018167276957779116
|
|
],
|
|
[
|
|
0.00023421899237906197,
|
|
0.00018181950839066228,
|
|
0.999999956041564
|
|
]
|
|
],
|
|
"euler_xyz_deg": [
|
|
0.010417490807070044,
|
|
-0.013419759867825114,
|
|
-0.03588845383117196
|
|
]
|
|
},
|
|
"movements": {
|
|
"x": {
|
|
"value_m": -0.000596897216554421,
|
|
"value_mm": -0.596897216554421,
|
|
"joint_type": "linear",
|
|
"link": "Base"
|
|
},
|
|
"y": {
|
|
"value_rad": -0.058884156231993945,
|
|
"value_deg": -3.3738136322822174,
|
|
"joint_type": "revolute",
|
|
"link": "Arm1"
|
|
},
|
|
"z": {
|
|
"value_rad": 1.951876013739172,
|
|
"value_deg": 111.83425772007364,
|
|
"joint_type": "revolute",
|
|
"link": "Ellbow"
|
|
},
|
|
"a": {
|
|
"value_rad": 1.335934792352644,
|
|
"value_deg": 76.5434253064925,
|
|
"joint_type": "revolute",
|
|
"link": "Arm2"
|
|
},
|
|
"b": {
|
|
"value_rad": 0.0,
|
|
"value_deg": 0.0,
|
|
"joint_type": "revolute",
|
|
"link": "Hand"
|
|
},
|
|
"c": {
|
|
"value_rad": 0.0,
|
|
"value_deg": 0.0,
|
|
"joint_type": "revolute",
|
|
"link": "Palm"
|
|
},
|
|
"e": {
|
|
"value_m": 0.0,
|
|
"value_mm": 0.0,
|
|
"joint_type": "linear",
|
|
"link": "FingerB"
|
|
}
|
|
},
|
|
"link_poses": {
|
|
"Board": {
|
|
"translation_m": [
|
|
-0.00010859211742092478,
|
|
0.00042385507834860614,
|
|
-0.00035770640297937134
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.9999997764000005,
|
|
0.0006263290459170604,
|
|
-0.00023433282914781965
|
|
],
|
|
[
|
|
-0.0006263716246644091,
|
|
0.9999997873267732,
|
|
-0.00018167276957779116
|
|
],
|
|
[
|
|
0.00023421899237906197,
|
|
0.00018181950839066228,
|
|
0.999999956041564
|
|
]
|
|
]
|
|
},
|
|
"Base": {
|
|
"translation_m": [
|
|
-0.0007092385257748967,
|
|
0.00042132219351465196,
|
|
0.015642153089021036
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.9999997764000005,
|
|
0.0006263290459170604,
|
|
-0.00023433282914781965
|
|
],
|
|
[
|
|
-0.0006263716246644091,
|
|
0.9999997873267732,
|
|
-0.00018167276957779116
|
|
],
|
|
[
|
|
0.00023421899237906197,
|
|
0.00018181950839066228,
|
|
0.999999956041564
|
|
]
|
|
]
|
|
},
|
|
"Arm1": {
|
|
"translation_m": [
|
|
0.10934783543787255,
|
|
0.10834422307146208,
|
|
0.0606875517069593
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.999999776399997,
|
|
0.000611452992262134,
|
|
-0.0002707862382571833
|
|
],
|
|
[
|
|
-0.0006263716246644069,
|
|
0.9982559251795456,
|
|
-0.05903147891175731
|
|
],
|
|
[
|
|
0.00023421899237906116,
|
|
0.05903163532513469,
|
|
0.9982560849663261
|
|
]
|
|
]
|
|
},
|
|
"Ellbow": {
|
|
"translation_m": [
|
|
0.10919497218980702,
|
|
-0.14121975822342434,
|
|
0.04592964287567563
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.9999997763972533,
|
|
2.394763040175848e-05,
|
|
0.0006683011743017988
|
|
],
|
|
[
|
|
-0.0006263716246626884,
|
|
-0.31647748461030295,
|
|
0.9485998151968996
|
|
],
|
|
[
|
|
0.00023421899237841852,
|
|
-0.9486000216948743,
|
|
-0.31647739884582166
|
|
]
|
|
]
|
|
},
|
|
"Arm2": {
|
|
"translation_m": [
|
|
0.19919495206555982,
|
|
-0.14127613166964398,
|
|
0.04595072258498969
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.23335822552244836,
|
|
2.3947630401721733e-05,
|
|
-0.9723908360325794
|
|
],
|
|
[
|
|
0.9224117372832784,
|
|
-0.3164774846098173,
|
|
0.2213562482816927
|
|
],
|
|
[
|
|
-0.3077345048889252,
|
|
-0.9486000216934186,
|
|
-0.07387471380203312
|
|
]
|
|
]
|
|
},
|
|
"Hand": {
|
|
"translation_m": [
|
|
0.1991889651579594,
|
|
-0.06215676051718966,
|
|
0.28310072800834435
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.23335822552244836,
|
|
2.3947630401721733e-05,
|
|
-0.9723908360325794
|
|
],
|
|
[
|
|
0.9224117372832784,
|
|
-0.3164774846098173,
|
|
0.2213562482816927
|
|
],
|
|
[
|
|
-0.3077345048889252,
|
|
-0.9486000216934186,
|
|
-0.07387471380203312
|
|
]
|
|
]
|
|
},
|
|
"Palm": {
|
|
"translation_m": [
|
|
0.1991889651579594,
|
|
-0.06215676051718966,
|
|
0.28310072800834435
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.23335822552244836,
|
|
2.3947630401721733e-05,
|
|
-0.9723908360325794
|
|
],
|
|
[
|
|
0.9224117372832784,
|
|
-0.3164774846098173,
|
|
0.2213562482816927
|
|
],
|
|
[
|
|
-0.3077345048889252,
|
|
-0.9486000216934186,
|
|
-0.07387471380203312
|
|
]
|
|
]
|
|
},
|
|
"FingerA": {
|
|
"translation_m": [
|
|
0.20012155989298513,
|
|
-0.047390401606712936,
|
|
0.3150707907480583
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.23335822552244836,
|
|
2.3947630401721733e-05,
|
|
-0.9723908360325794
|
|
],
|
|
[
|
|
0.9224117372832784,
|
|
-0.3164774846098173,
|
|
0.2213562482816927
|
|
],
|
|
[
|
|
-0.3077345048889252,
|
|
-0.9486000216934186,
|
|
-0.07387471380203312
|
|
]
|
|
]
|
|
},
|
|
"FingerB": {
|
|
"translation_m": [
|
|
0.19825469408880553,
|
|
-0.05476969550497916,
|
|
0.3175326667871697
|
|
],
|
|
"rotation_matrix": [
|
|
[
|
|
0.23335822552244836,
|
|
2.3947630401721733e-05,
|
|
-0.9723908360325794
|
|
],
|
|
[
|
|
0.9224117372832784,
|
|
-0.3164774846098173,
|
|
0.2213562482816927
|
|
],
|
|
[
|
|
-0.3077345048889252,
|
|
-0.9486000216934186,
|
|
-0.07387471380203312
|
|
]
|
|
]
|
|
}
|
|
},
|
|
"stage_reports": [
|
|
{
|
|
"method": "deterministic_geometric_prefix",
|
|
"active_links": [
|
|
"Board",
|
|
"Base",
|
|
"Arm1"
|
|
],
|
|
"active_observations": 7,
|
|
"joint_updates": [
|
|
{
|
|
"pass": 0,
|
|
"updates": [
|
|
{
|
|
"link": "Base",
|
|
"joint_variable": "x",
|
|
"joint_type": "linear",
|
|
"old": 0.0,
|
|
"new": 0.007926192045056242,
|
|
"info": {
|
|
"reason": "weighted_projection",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
0.9999997763990008,
|
|
-0.000626371624663783,
|
|
0.00023421899237882783
|
|
],
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"q_i": 0.0072187189649061964,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"q_i": 0.007961618683264957,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"q_i": 0.008598238486997574,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm1",
|
|
"joint_variable": "y",
|
|
"joint_type": "revolute",
|
|
"old": 0.0,
|
|
"new": -0.1780529392099517,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.000626371624663783,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -0.17805293920995172,
|
|
"theta_alt_rad": 2.9635397143798414,
|
|
"score_theta": 0.023858209417714757,
|
|
"score_theta_alt": 0.8762278856863018,
|
|
"best_score": 0.023858209417714757,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
}
|
|
]
|
|
},
|
|
{
|
|
"pass": 1,
|
|
"updates": [
|
|
{
|
|
"link": "Base",
|
|
"joint_variable": "x",
|
|
"joint_type": "linear",
|
|
"old": 0.007926192045056242,
|
|
"new": 0.007926192045056247,
|
|
"info": {
|
|
"reason": "weighted_projection",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
0.9999997763990008,
|
|
-0.000626371624663783,
|
|
0.00023421899237882783
|
|
],
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"q_i": 0.007218718964906196,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"q_i": 0.007961618683264962,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"q_i": 0.00859823848699758,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm1",
|
|
"joint_variable": "y",
|
|
"joint_type": "revolute",
|
|
"old": -0.1780529392099517,
|
|
"new": -0.1780529392099517,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.000626371624663783,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -0.17805293920995172,
|
|
"theta_alt_rad": 2.9635397143798414,
|
|
"score_theta": 0.023858209417714757,
|
|
"score_theta_alt": 0.8762278856863018,
|
|
"best_score": 0.023858209417714757,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
}
|
|
]
|
|
}
|
|
],
|
|
"root_link": "Board",
|
|
"root_pose": {
|
|
"reason": "kabsch",
|
|
"used_markers": [
|
|
210,
|
|
211,
|
|
215,
|
|
214
|
|
]
|
|
},
|
|
"marker_stats": {
|
|
"num_markers_used": 7,
|
|
"mean_error_m": 0.036612631242172615,
|
|
"rms_error_m": 0.05640926160205541,
|
|
"median_error_m": 6.087233578986277e-05,
|
|
"worst_error_m": 0.09420953868705992
|
|
},
|
|
"marker_reports": [
|
|
{
|
|
"marker_id": 198,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
0.0007202617604440409,
|
|
-0.005268103086883809,
|
|
-0.06868917582050779
|
|
],
|
|
"error_norm_m": 0.06889466279791363,
|
|
"predicted_m": [
|
|
0.11776698044974121,
|
|
-0.055331633061161886,
|
|
0.06676827264129218
|
|
],
|
|
"observed_m": [
|
|
0.11704671868929717,
|
|
-0.05006352997427808,
|
|
0.13545744846179997
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 210,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-4.460758735464615e-06,
|
|
-1.9834919135045675e-05,
|
|
3.557547737978705e-06
|
|
],
|
|
"error_norm_m": 2.0639247357833144e-05,
|
|
"predicted_m": [
|
|
0.019878806529812002,
|
|
-0.01958872260251102,
|
|
-5.6658426487134195e-05
|
|
],
|
|
"observed_m": [
|
|
0.019883267288547467,
|
|
-0.019568887683375974,
|
|
-6.02159742251129e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 211,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-6.984434095747005e-06,
|
|
3.988566378927173e-05,
|
|
-1.1304364446612915e-05
|
|
],
|
|
"error_norm_m": 4.204089855236805e-05,
|
|
"predicted_m": [
|
|
0.24988501839227129,
|
|
-0.009732790202916102,
|
|
-9.698631560433007e-07
|
|
],
|
|
"observed_m": [
|
|
0.24989200282636703,
|
|
-0.009772675866705374,
|
|
1.0334501290569615e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 214,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-6.998801225233109e-06,
|
|
-5.991274503321445e-05,
|
|
8.180527397944547e-06
|
|
],
|
|
"error_norm_m": 6.087233578986277e-05,
|
|
"predicted_m": [
|
|
0.3498849960322714,
|
|
-0.009795427365382543,
|
|
2.2452036081862874e-05
|
|
],
|
|
"observed_m": [
|
|
0.34989199483349664,
|
|
-0.009735514620349328,
|
|
1.4271508683918328e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 215,
|
|
"link": "Board",
|
|
"error_m": [
|
|
1.8443994056455137e-05,
|
|
3.986200037897625e-05,
|
|
-4.3371068931049083e-07
|
|
],
|
|
"error_norm_m": 4.392434513945628e-05,
|
|
"predicted_m": [
|
|
0.24983491206859793,
|
|
-0.08973277318905795,
|
|
-1.55154238272963e-05
|
|
],
|
|
"observed_m": [
|
|
0.24981646807454147,
|
|
-0.08977263518943693,
|
|
-1.508171313798581e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
-1.6310178929279662e-05,
|
|
-0.004226441081051802,
|
|
-0.0929206700434215
|
|
],
|
|
"error_norm_m": 0.09301674038339523,
|
|
"predicted_m": [
|
|
0.11771523733460335,
|
|
-0.14390585513997817,
|
|
0.05081194142819521
|
|
],
|
|
"observed_m": [
|
|
0.11773154751353263,
|
|
-0.13967941405892637,
|
|
0.1437326114716167
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
-0.000651560035294721,
|
|
-0.0025077194399967806,
|
|
-0.09417390292623168
|
|
],
|
|
"error_norm_m": 0.09420953868705992,
|
|
"predicted_m": [
|
|
0.1177070695957754,
|
|
-0.17214615213272416,
|
|
0.010161165801531043
|
|
],
|
|
"observed_m": [
|
|
0.11835862963107012,
|
|
-0.16963843269272738,
|
|
0.10433506872776273
|
|
]
|
|
}
|
|
],
|
|
"stage_idx": 0,
|
|
"num_active_links": 3
|
|
},
|
|
{
|
|
"method": "deterministic_geometric_prefix",
|
|
"active_links": [
|
|
"Board",
|
|
"Base",
|
|
"Arm1",
|
|
"Ellbow"
|
|
],
|
|
"active_observations": 7,
|
|
"joint_updates": [
|
|
{
|
|
"pass": 0,
|
|
"updates": [
|
|
{
|
|
"link": "Base",
|
|
"joint_variable": "x",
|
|
"joint_type": "linear",
|
|
"old": 0.007926192045056247,
|
|
"new": 0.007926192045056247,
|
|
"info": {
|
|
"reason": "weighted_projection",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
0.9999997763990008,
|
|
-0.000626371624663783,
|
|
0.00023421899237882783
|
|
],
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"q_i": 0.007218718964906196,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"q_i": 0.007961618683264962,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"q_i": 0.00859823848699758,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm1",
|
|
"joint_variable": "y",
|
|
"joint_type": "revolute",
|
|
"old": -0.1780529392099517,
|
|
"new": -0.1780529392099517,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.000626371624663783,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -0.17805293920995172,
|
|
"theta_alt_rad": 2.9635397143798414,
|
|
"score_theta": 0.023858209417714757,
|
|
"score_theta_alt": 0.8762278856863018,
|
|
"best_score": 0.023858209417714757,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Ellbow",
|
|
"joint_variable": "z",
|
|
"joint_type": "revolute",
|
|
"old": 0.0,
|
|
"new": 0.0,
|
|
"info": {
|
|
"reason": "no_observations"
|
|
}
|
|
}
|
|
]
|
|
},
|
|
{
|
|
"pass": 1,
|
|
"updates": [
|
|
{
|
|
"link": "Base",
|
|
"joint_variable": "x",
|
|
"joint_type": "linear",
|
|
"old": 0.007926192045056247,
|
|
"new": 0.007926192045056247,
|
|
"info": {
|
|
"reason": "weighted_projection",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
0.9999997763990008,
|
|
-0.000626371624663783,
|
|
0.00023421899237882783
|
|
],
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"q_i": 0.007218718964906196,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"q_i": 0.007961618683264962,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"q_i": 0.00859823848699758,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm1",
|
|
"joint_variable": "y",
|
|
"joint_type": "revolute",
|
|
"old": -0.1780529392099517,
|
|
"new": -0.1780529392099517,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 3,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.000626371624663783,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -0.17805293920995172,
|
|
"theta_alt_rad": 2.9635397143798414,
|
|
"score_theta": 0.023858209417714757,
|
|
"score_theta_alt": 0.8762278856863018,
|
|
"best_score": 0.023858209417714757,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Ellbow",
|
|
"joint_variable": "z",
|
|
"joint_type": "revolute",
|
|
"old": 0.0,
|
|
"new": 0.0,
|
|
"info": {
|
|
"reason": "no_observations"
|
|
}
|
|
}
|
|
]
|
|
}
|
|
],
|
|
"root_link": "Board",
|
|
"root_pose": {
|
|
"reason": "kabsch",
|
|
"used_markers": [
|
|
210,
|
|
211,
|
|
215,
|
|
214
|
|
]
|
|
},
|
|
"marker_stats": {
|
|
"num_markers_used": 7,
|
|
"mean_error_m": 0.036612631242172615,
|
|
"rms_error_m": 0.05640926160205541,
|
|
"median_error_m": 6.087233578986277e-05,
|
|
"worst_error_m": 0.09420953868705992
|
|
},
|
|
"marker_reports": [
|
|
{
|
|
"marker_id": 198,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
0.0007202617604440409,
|
|
-0.005268103086883809,
|
|
-0.06868917582050779
|
|
],
|
|
"error_norm_m": 0.06889466279791363,
|
|
"predicted_m": [
|
|
0.11776698044974121,
|
|
-0.055331633061161886,
|
|
0.06676827264129218
|
|
],
|
|
"observed_m": [
|
|
0.11704671868929717,
|
|
-0.05006352997427808,
|
|
0.13545744846179997
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 210,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-4.460758735464615e-06,
|
|
-1.9834919135045675e-05,
|
|
3.557547737978705e-06
|
|
],
|
|
"error_norm_m": 2.0639247357833144e-05,
|
|
"predicted_m": [
|
|
0.019878806529812002,
|
|
-0.01958872260251102,
|
|
-5.6658426487134195e-05
|
|
],
|
|
"observed_m": [
|
|
0.019883267288547467,
|
|
-0.019568887683375974,
|
|
-6.02159742251129e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 211,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-6.984434095747005e-06,
|
|
3.988566378927173e-05,
|
|
-1.1304364446612915e-05
|
|
],
|
|
"error_norm_m": 4.204089855236805e-05,
|
|
"predicted_m": [
|
|
0.24988501839227129,
|
|
-0.009732790202916102,
|
|
-9.698631560433007e-07
|
|
],
|
|
"observed_m": [
|
|
0.24989200282636703,
|
|
-0.009772675866705374,
|
|
1.0334501290569615e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 214,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-6.998801225233109e-06,
|
|
-5.991274503321445e-05,
|
|
8.180527397944547e-06
|
|
],
|
|
"error_norm_m": 6.087233578986277e-05,
|
|
"predicted_m": [
|
|
0.3498849960322714,
|
|
-0.009795427365382543,
|
|
2.2452036081862874e-05
|
|
],
|
|
"observed_m": [
|
|
0.34989199483349664,
|
|
-0.009735514620349328,
|
|
1.4271508683918328e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 215,
|
|
"link": "Board",
|
|
"error_m": [
|
|
1.8443994056455137e-05,
|
|
3.986200037897625e-05,
|
|
-4.3371068931049083e-07
|
|
],
|
|
"error_norm_m": 4.392434513945628e-05,
|
|
"predicted_m": [
|
|
0.24983491206859793,
|
|
-0.08973277318905795,
|
|
-1.55154238272963e-05
|
|
],
|
|
"observed_m": [
|
|
0.24981646807454147,
|
|
-0.08977263518943693,
|
|
-1.508171313798581e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
-1.6310178929279662e-05,
|
|
-0.004226441081051802,
|
|
-0.0929206700434215
|
|
],
|
|
"error_norm_m": 0.09301674038339523,
|
|
"predicted_m": [
|
|
0.11771523733460335,
|
|
-0.14390585513997817,
|
|
0.05081194142819521
|
|
],
|
|
"observed_m": [
|
|
0.11773154751353263,
|
|
-0.13967941405892637,
|
|
0.1437326114716167
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
-0.000651560035294721,
|
|
-0.0025077194399967806,
|
|
-0.09417390292623168
|
|
],
|
|
"error_norm_m": 0.09420953868705992,
|
|
"predicted_m": [
|
|
0.1177070695957754,
|
|
-0.17214615213272416,
|
|
0.010161165801531043
|
|
],
|
|
"observed_m": [
|
|
0.11835862963107012,
|
|
-0.16963843269272738,
|
|
0.10433506872776273
|
|
]
|
|
}
|
|
],
|
|
"stage_idx": 1,
|
|
"num_active_links": 4
|
|
},
|
|
{
|
|
"method": "deterministic_geometric_prefix",
|
|
"active_links": [
|
|
"Board",
|
|
"Base",
|
|
"Arm1",
|
|
"Ellbow",
|
|
"Arm2"
|
|
],
|
|
"active_observations": 8,
|
|
"joint_updates": [
|
|
{
|
|
"pass": 0,
|
|
"updates": [
|
|
{
|
|
"link": "Base",
|
|
"joint_variable": "x",
|
|
"joint_type": "linear",
|
|
"old": 0.007926192045056247,
|
|
"new": 0.005794016107234206,
|
|
"info": {
|
|
"reason": "weighted_projection",
|
|
"used_markers": 4,
|
|
"axis_world": [
|
|
0.9999997763990008,
|
|
-0.000626371624663783,
|
|
0.00023421899237882783
|
|
],
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"q_i": 0.007218718964906196,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"q_i": 0.007961618683264962,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"q_i": 0.00859823848699758,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 122,
|
|
"q_i": -0.0006025117062319173,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm1",
|
|
"joint_variable": "y",
|
|
"joint_type": "revolute",
|
|
"old": -0.1780529392099517,
|
|
"new": -0.23589185759318765,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 4,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.000626371624663783,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -0.23589185759318754,
|
|
"theta_alt_rad": 2.905700795996605,
|
|
"score_theta": 0.07369009246472558,
|
|
"score_theta_alt": 1.3871696424026658,
|
|
"best_score": 0.07369009246472558,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 122,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Ellbow",
|
|
"joint_variable": "z",
|
|
"joint_type": "revolute",
|
|
"old": 0.0,
|
|
"new": 1.9537800882054759,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 1,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.000626371624663783,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -1.1878125653843175,
|
|
"theta_alt_rad": 1.9537800882054759,
|
|
"score_theta": 0.09389488476976013,
|
|
"score_theta_alt": 0.024271154524160916,
|
|
"best_score": 0.024271154524160916,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 122,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm2",
|
|
"joint_variable": "a",
|
|
"joint_type": "revolute",
|
|
"old": 0.0,
|
|
"new": 1.297808413286715,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 1,
|
|
"axis_world": [
|
|
-0.00014000630341583307,
|
|
0.1463823210875183,
|
|
0.9892280811164156
|
|
],
|
|
"axis_id": 2,
|
|
"theta_rad": 1.2978084132867151,
|
|
"theta_alt_rad": -1.8437842403030782,
|
|
"score_theta": 0.01631117339313782,
|
|
"score_theta_alt": 0.038026649549060154,
|
|
"best_score": 0.01631117339313782,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 122,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
}
|
|
]
|
|
},
|
|
{
|
|
"pass": 1,
|
|
"updates": [
|
|
{
|
|
"link": "Base",
|
|
"joint_variable": "x",
|
|
"joint_type": "linear",
|
|
"old": 0.005794016107234206,
|
|
"new": -0.000596897216554421,
|
|
"info": {
|
|
"reason": "weighted_projection",
|
|
"used_markers": 4,
|
|
"axis_world": [
|
|
0.9999997763990008,
|
|
-0.000626371624663783,
|
|
0.00023421899237882783
|
|
],
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"q_i": 0.00721871896490619,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"q_i": 0.007961618683264955,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"q_i": 0.008598238486997579,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 122,
|
|
"q_i": -0.02616616500138641,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm1",
|
|
"joint_variable": "y",
|
|
"joint_type": "revolute",
|
|
"old": -0.23589185759318765,
|
|
"new": -0.058884156231993945,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 4,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.000626371624663783,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -0.058884156231994125,
|
|
"theta_alt_rad": 3.0827084973577996,
|
|
"score_theta": 0.026338477058174103,
|
|
"score_theta_alt": 1.3158364575890724,
|
|
"best_score": 0.026338477058174103,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 198,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"weight": 1.0
|
|
},
|
|
{
|
|
"marker_id": 122,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Ellbow",
|
|
"joint_variable": "z",
|
|
"joint_type": "revolute",
|
|
"old": 1.9537800882054759,
|
|
"new": 1.951876013739172,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 1,
|
|
"axis_world": [
|
|
-0.9999997763990008,
|
|
0.0006263716246637829,
|
|
-0.00023421899237882783
|
|
],
|
|
"axis_id": 0,
|
|
"theta_rad": -1.1897166398506211,
|
|
"theta_alt_rad": 1.951876013739172,
|
|
"score_theta": 0.07582250229968954,
|
|
"score_theta_alt": 0.015787381665384438,
|
|
"best_score": 0.015787381665384438,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 122,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
},
|
|
{
|
|
"link": "Arm2",
|
|
"joint_variable": "a",
|
|
"joint_type": "revolute",
|
|
"old": 1.297808413286715,
|
|
"new": 1.335934792352644,
|
|
"info": {
|
|
"reason": "2d_alignment+normal_tiebreak",
|
|
"used_markers": 1,
|
|
"axis_world": [
|
|
-2.394763040175525e-05,
|
|
0.31647748461026026,
|
|
0.9486000216947464
|
|
],
|
|
"axis_id": 2,
|
|
"theta_rad": 1.335934792352644,
|
|
"theta_alt_rad": -1.8056578612371492,
|
|
"score_theta": 0.015774381349414834,
|
|
"score_theta_alt": 0.03792035021936615,
|
|
"best_score": 0.015774381349414834,
|
|
"per_marker": [
|
|
{
|
|
"marker_id": 122,
|
|
"weight": 1.0
|
|
}
|
|
]
|
|
}
|
|
}
|
|
]
|
|
}
|
|
],
|
|
"root_link": "Board",
|
|
"root_pose": {
|
|
"reason": "kabsch",
|
|
"used_markers": [
|
|
210,
|
|
211,
|
|
215,
|
|
214
|
|
]
|
|
},
|
|
"marker_stats": {
|
|
"num_markers_used": 8,
|
|
"mean_error_m": 0.03760308986215115,
|
|
"rms_error_m": 0.057114446045352985,
|
|
"median_error_m": 0.02503283617989611,
|
|
"worst_error_m": 0.12559610403756494
|
|
},
|
|
"marker_reports": [
|
|
{
|
|
"marker_id": 122,
|
|
"link": "Arm2",
|
|
"error_m": [
|
|
0.02694058600720428,
|
|
0.12176662358072948,
|
|
-0.01488205485651084
|
|
],
|
|
"error_norm_m": 0.12559610403756494,
|
|
"predicted_m": [
|
|
0.19102473203766912,
|
|
-0.1381150641982592,
|
|
0.16296463268576494
|
|
],
|
|
"observed_m": [
|
|
0.16408414603046484,
|
|
-0.2598816877789887,
|
|
0.17784668754227578
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 198,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
-0.007806193248525564,
|
|
-0.0033792967448986536,
|
|
-0.0492759954330408
|
|
],
|
|
"error_norm_m": 0.05000480002400236,
|
|
"predicted_m": [
|
|
0.1092405254407716,
|
|
-0.05344282671917673,
|
|
0.08618145302875917
|
|
],
|
|
"observed_m": [
|
|
0.11704671868929717,
|
|
-0.05006352997427808,
|
|
0.13545744846179997
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 210,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-4.460758735464615e-06,
|
|
-1.9834919135045675e-05,
|
|
3.557547737978705e-06
|
|
],
|
|
"error_norm_m": 2.0639247357833144e-05,
|
|
"predicted_m": [
|
|
0.019878806529812002,
|
|
-0.01958872260251102,
|
|
-5.6658426487134195e-05
|
|
],
|
|
"observed_m": [
|
|
0.019883267288547467,
|
|
-0.019568887683375974,
|
|
-6.02159742251129e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 211,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-6.984434095747005e-06,
|
|
3.988566378927173e-05,
|
|
-1.1304364446612915e-05
|
|
],
|
|
"error_norm_m": 4.204089855236805e-05,
|
|
"predicted_m": [
|
|
0.24988501839227129,
|
|
-0.009732790202916102,
|
|
-9.698631560433007e-07
|
|
],
|
|
"observed_m": [
|
|
0.24989200282636703,
|
|
-0.009772675866705374,
|
|
1.0334501290569615e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 214,
|
|
"link": "Board",
|
|
"error_m": [
|
|
-6.998801225233109e-06,
|
|
-5.991274503321445e-05,
|
|
8.180527397944547e-06
|
|
],
|
|
"error_norm_m": 6.087233578986277e-05,
|
|
"predicted_m": [
|
|
0.3498849960322714,
|
|
-0.009795427365382543,
|
|
2.2452036081862874e-05
|
|
],
|
|
"observed_m": [
|
|
0.34989199483349664,
|
|
-0.009735514620349328,
|
|
1.4271508683918328e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 215,
|
|
"link": "Board",
|
|
"error_m": [
|
|
1.8443994056455137e-05,
|
|
3.986200037897625e-05,
|
|
-4.3371068931049083e-07
|
|
],
|
|
"error_norm_m": 4.392434513945628e-05,
|
|
"predicted_m": [
|
|
0.24983491206859793,
|
|
-0.08973277318905795,
|
|
-1.55154238272963e-05
|
|
],
|
|
"observed_m": [
|
|
0.24981646807454147,
|
|
-0.08977263518943693,
|
|
-1.508171313798581e-05
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
-0.008546052842064611,
|
|
-0.003606445926409463,
|
|
-0.06286400562211966
|
|
],
|
|
"error_norm_m": 0.06354466676486209,
|
|
"predicted_m": [
|
|
0.10918549467146801,
|
|
-0.14328585998533583,
|
|
0.08086860584949704
|
|
],
|
|
"observed_m": [
|
|
0.11773154751353263,
|
|
-0.13967941405892637,
|
|
0.1437326114716167
|
|
]
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"link": "Arm1",
|
|
"error_m": [
|
|
-0.009185058295992282,
|
|
-0.00652028291198109,
|
|
-0.06047153308846681
|
|
],
|
|
"error_norm_m": 0.06151167124394025,
|
|
"predicted_m": [
|
|
0.10917357133507784,
|
|
-0.17615871560470847,
|
|
0.04386353563929592
|
|
],
|
|
"observed_m": [
|
|
0.11835862963107012,
|
|
-0.16963843269272738,
|
|
0.10433506872776273
|
|
]
|
|
}
|
|
],
|
|
"stage_idx": 2,
|
|
"num_active_links": 5
|
|
}
|
|
],
|
|
"markers": [
|
|
{
|
|
"marker_id": 122,
|
|
"link": "Arm2",
|
|
"observed_position_m": [
|
|
0.16408414603046484,
|
|
-0.2598816877789887,
|
|
0.17784668754227578
|
|
],
|
|
"predicted_position_m": [
|
|
0.19102473203766912,
|
|
-0.1381150641982592,
|
|
0.16296463268576494
|
|
],
|
|
"error_m": [
|
|
0.02694058600720428,
|
|
0.12176662358072948,
|
|
-0.01488205485651084
|
|
],
|
|
"error_norm_m": 0.12559610403756494
|
|
},
|
|
{
|
|
"marker_id": 198,
|
|
"link": "Arm1",
|
|
"observed_position_m": [
|
|
0.11704671868929717,
|
|
-0.05006352997427808,
|
|
0.13545744846179997
|
|
],
|
|
"predicted_position_m": [
|
|
0.1092405254407716,
|
|
-0.05344282671917673,
|
|
0.08618145302875917
|
|
],
|
|
"error_m": [
|
|
-0.007806193248525564,
|
|
-0.0033792967448986536,
|
|
-0.0492759954330408
|
|
],
|
|
"error_norm_m": 0.05000480002400236
|
|
},
|
|
{
|
|
"marker_id": 210,
|
|
"link": "Board",
|
|
"observed_position_m": [
|
|
0.019883267288547467,
|
|
-0.019568887683375974,
|
|
-6.02159742251129e-05
|
|
],
|
|
"predicted_position_m": [
|
|
0.019878806529812002,
|
|
-0.01958872260251102,
|
|
-5.6658426487134195e-05
|
|
],
|
|
"error_m": [
|
|
-4.460758735464615e-06,
|
|
-1.9834919135045675e-05,
|
|
3.557547737978705e-06
|
|
],
|
|
"error_norm_m": 2.0639247357833144e-05
|
|
},
|
|
{
|
|
"marker_id": 211,
|
|
"link": "Board",
|
|
"observed_position_m": [
|
|
0.24989200282636703,
|
|
-0.009772675866705374,
|
|
1.0334501290569615e-05
|
|
],
|
|
"predicted_position_m": [
|
|
0.24988501839227129,
|
|
-0.009732790202916102,
|
|
-9.698631560433007e-07
|
|
],
|
|
"error_m": [
|
|
-6.984434095747005e-06,
|
|
3.988566378927173e-05,
|
|
-1.1304364446612915e-05
|
|
],
|
|
"error_norm_m": 4.204089855236805e-05
|
|
},
|
|
{
|
|
"marker_id": 214,
|
|
"link": "Board",
|
|
"observed_position_m": [
|
|
0.34989199483349664,
|
|
-0.009735514620349328,
|
|
1.4271508683918328e-05
|
|
],
|
|
"predicted_position_m": [
|
|
0.3498849960322714,
|
|
-0.009795427365382543,
|
|
2.2452036081862874e-05
|
|
],
|
|
"error_m": [
|
|
-6.998801225233109e-06,
|
|
-5.991274503321445e-05,
|
|
8.180527397944547e-06
|
|
],
|
|
"error_norm_m": 6.087233578986277e-05
|
|
},
|
|
{
|
|
"marker_id": 215,
|
|
"link": "Board",
|
|
"observed_position_m": [
|
|
0.24981646807454147,
|
|
-0.08977263518943693,
|
|
-1.508171313798581e-05
|
|
],
|
|
"predicted_position_m": [
|
|
0.24983491206859793,
|
|
-0.08973277318905795,
|
|
-1.55154238272963e-05
|
|
],
|
|
"error_m": [
|
|
1.8443994056455137e-05,
|
|
3.986200037897625e-05,
|
|
-4.3371068931049083e-07
|
|
],
|
|
"error_norm_m": 4.392434513945628e-05
|
|
},
|
|
{
|
|
"marker_id": 229,
|
|
"link": "Arm1",
|
|
"observed_position_m": [
|
|
0.11773154751353263,
|
|
-0.13967941405892637,
|
|
0.1437326114716167
|
|
],
|
|
"predicted_position_m": [
|
|
0.10918549467146801,
|
|
-0.14328585998533583,
|
|
0.08086860584949704
|
|
],
|
|
"error_m": [
|
|
-0.008546052842064611,
|
|
-0.003606445926409463,
|
|
-0.06286400562211966
|
|
],
|
|
"error_norm_m": 0.06354466676486209
|
|
},
|
|
{
|
|
"marker_id": 243,
|
|
"link": "Arm1",
|
|
"observed_position_m": [
|
|
0.11835862963107012,
|
|
-0.16963843269272738,
|
|
0.10433506872776273
|
|
],
|
|
"predicted_position_m": [
|
|
0.10917357133507784,
|
|
-0.17615871560470847,
|
|
0.04386353563929592
|
|
],
|
|
"error_m": [
|
|
-0.009185058295992282,
|
|
-0.00652028291198109,
|
|
-0.06047153308846681
|
|
],
|
|
"error_norm_m": 0.06151167124394025
|
|
}
|
|
]
|
|
} |