diff --git a/data/robot/robot.json b/data/robot/robot.json new file mode 100644 index 0000000..b9b760c --- /dev/null +++ b/data/robot/robot.json @@ -0,0 +1,2441 @@ +{ + "coordinateSystem": { + "handedness": "right", + "x": "right", + "y": "backward", + "z": "up" + }, + "units": { + "length": "mm", + "rotation": "degree" + }, + "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": { + "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, + "position": [ + 20, + -20, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 211, + "position": [ + 250, + -10, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 215, + "position": [ + 250, + -90, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 214, + "position": [ + 350, + -10, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 208, + "position": [ + 350, + -90, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 206, + "position": [ + 650, + -10, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 205, + "position": [ + 750, + -90, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 207, + "position": [ + 750, + -10, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 217, + "position": [ + 650, + -90, + 0.3 + ], + "normal": [ + 0, + 0, + 1 + ] + }, + { + "id": 46, + "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, + "position": [ + 344.23, + -286.54, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 48, + "position": [ + 688.69, + -320.72, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 49, + "position": [ + 1006.0, + 158.33, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 50, + "position": [ + 573.41, + 211.86, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 51, + "position": [ + 167.8, + -172.08, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 52, + "position": [ + 94.68, + 208.66, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 53, + "position": [ + 486.25, + 212.24, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 54, + "position": [ + 342.27, + -330.59, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 55, + "position": [ + 283.72, + -262.58, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 56, + "position": [ + 498.68, + 168.67, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 57, + "position": [ + 602.86, + -364.05, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 58, + "position": [ + 50.09, + -218.11, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 59, + "position": [ + 626.21, + -278.75, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 60, + "position": [ + 434.36, + 283.81, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 61, + "position": [ + -22.42, + 335.83, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 62, + "position": [ + 404.7, + -175.1, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 63, + "position": [ + 777.4, + -236.15, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 64, + "position": [ + -21.27, + -188.23, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 65, + "position": [ + 803.39, + -297.37, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 66, + "position": [ + 209.75, + -363.23, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 67, + "position": [ + 523.07, + 267.04, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 68, + "position": [ + 573.73, + 170.64, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 69, + "position": [ + 7.61, + -281.21, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 70, + "position": [ + 601.87, + 300.33, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 71, + "position": [ + 749.75, + -284.01, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 72, + "position": [ + 440.99, + 194.32, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 73, + "position": [ + 221.73, + 333.11, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 74, + "position": [ + 93.78, + 144.5, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 75, + "position": [ + -25.7, + 194.58, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 76, + "position": [ + 685.21, + 166.8, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 77, + "position": [ + 18.19, + 191.57, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 78, + "position": [ + 823.11, + -344.38, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 79, + "position": [ + 312.3, + -159.11, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 80, + "position": [ + 863.59, + -335.92, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 81, + "position": [ + 132.14, + 169.03, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 82, + "position": [ + 219.16, + 297.24, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 83, + "position": [ + 44.16, + 339.22, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 84, + "position": [ + 407.49, + 258.42, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 85, + "position": [ + 504.58, + -312.75, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 86, + "position": [ + 362.89, + 292.01, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 87, + "position": [ + 943.63, + -245.76, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 88, + "position": [ + 765.87, + 316.04, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 89, + "position": [ + 988.02, + -369.14, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 90, + "position": [ + 643.17, + 316.43, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 91, + "position": [ + 723.35, + 328.05, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 92, + "position": [ + 645.09, + -184.84, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 93, + "position": [ + 934.88, + 143.6, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 94, + "position": [ + 875.7, + 173.65, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 95, + "position": [ + 186.04, + -274.07, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 96, + "position": [ + 369.77, + -186.49, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 97, + "position": [ + 304.35, + -359.67, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 98, + "position": [ + 575.27, + 315.06, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 99, + "position": [ + 959.16, + -321.55, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 100, + "position": [ + 803.25, + 172.36, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 101, + "position": [ + 117.7, + 298.66, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 102, + "position": [ + 649.69, + -223.0, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 103, + "position": [ + 105.71, + -187.71, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 104, + "position": [ + 826.71, + 239.16, + -27.3 + ], + "normal": [ + 0, + 0, + 1 + ], + "spin": 90 + }, + { + "id": 105, + "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" + }, + "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" + }, + "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" + }, + "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" + }, + "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" + }, + "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" + }, + "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" + }, + "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" + }, + "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" + } + ] + } + } +} \ No newline at end of file diff --git a/data/robot/robot_running_2026_05_30.json b/data/robot/robot_running_2026_05_30.json new file mode 100644 index 0000000..49fbe47 --- /dev/null +++ b/data/robot/robot_running_2026_05_30.json @@ -0,0 +1,595 @@ +{ + "coordinateSystem": { + "handedness": "right", + "x": "right", + "y": "backward", + "z": "up" + }, + "units": { + "length": "mm", + "rotation": "degree" + }, + "vision_config": { + "MarkerType": "DICT_4X4_250", + "MarkerSize": 0.025 + }, + "renderingInfo": { + "width": 1280, + "height": 720, + "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":[300, -500,1450], + "cameraPosition_c":[600, -500,600], + "cameraTarget": [210, -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.70, 0.85, 1.0], + "backgroundStrength": 0.20, + "sunEnergy": 0.35, + "areaEnergy": 120, + "exposure": -1.5, + "lensDirt": true, + "lensDirtStrength": 0.08, + "dofEnabled": true, + "dofFStop": 11, + "arucoDust": false, + "arucoDustStrength": 0.00005, + "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.20, 0.20], + "roughness": 0.35, + "metallic": 0.0 + }, + "markerBlack": { + "baseColor": [0.04, 0.04, 0.04], + "roughness": 0.80, + "metallic": 0.0 + } + }, + "skeletonDefaults": { + "radius": 4, + "color": [0.85, 0.20, 0.20] + }, + "markerDefaults": { + "size": 25, + "thickness": 1, + "color": [0.04, 0.04, 0.04] + } + }, + "defaultPosition__": { + "x": 10, + "y": 4, + "z": 20, + "a": 10, + "b": 2, + "c": 9, + "e": 1 + }, + "defaultPosition": { + "x": 180, + "y": 86, + "z": -120, + "a": -60, + "b": 22, + "c": 91, + "e": 10 + }, + "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 + }, + "robot_test_poses": { + "sim04": {"x": 70, "y": 50,"z": -70,"a": 120,"b": 50,"c": 30,"e": 20}, + "sim05": {"x": 180,"y": 86,"z": -120,"a": -60,"b": 22,"c": 91,"e": 10}, + "sim06": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3}, + "sim07": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9}, + "sim08": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3}, + "sim09": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8}, + "sim09a": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8}, + "sim09b": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8}, + "sim010": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4}, + "sim011": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5}, + "sim012": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6} + }, + "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": 4, + "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": { + "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.20, 0.20] + }, + "markers":[ + {"id":210,"position":[20, -20, 0.3], "normal":[0,0,1]}, + {"id":211,"position":[250, -10, 0.3], "normal":[0,0,1]}, + {"id":215,"position":[250, -90, 0.3], "normal":[0,0,1]}, + {"id":214,"position":[350, -10, 0.3], "normal":[0,0,1]}, + {"id":208,"position":[350, -90, 0.3], "normal":[0,0,1]}, + {"id":206,"position":[650, -10, 0.3], "normal":[0,0,1]}, + {"id":205,"position":[750, -90, 0.3], "normal":[0,0,1]}, + {"id":207,"position":[750, -10, 0.3], "normal":[0,0,1]}, + {"id":217,"position":[650, -90, 0.3], "normal":[0,0,1]}, + + {"id": 46, "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, "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 48, "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 49, "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 50, "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 51, "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 52, "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 53, "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 54, "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 55, "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 56, "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 57, "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 58, "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 59, "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 60, "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 61, "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 62, "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 63, "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 64, "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 65, "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 66, "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 67, "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 68, "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 69, "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 70, "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 71, "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 72, "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 73, "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 74, "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 75, "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 76, "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 77, "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 78, "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 79, "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 80, "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 81, "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 82, "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 83, "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 84, "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 85, "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 86, "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 87, "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 88, "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 89, "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 90, "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 91, "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 92, "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 93, "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 94, "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 95, "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 96, "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 97, "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 98, "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 99, "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 100, "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 101, "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 102, "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 103, "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 104, "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 105, "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" + }, + "skeleton": { + "from": [0, 108, 45], + "to": [110, 108, 45], + "radius": 4, + "color": [0.20, 0.80, 0.20] + }, + "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" + }, + "skeleton": { + "from": [0, 0, 0], + "to": [0, -250, 0], + "radius": 4, + "color": [0.20, 0.20, 0.90] + }, + "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" + }, + + "skeleton": { + "from": [0, 0, 0], + "to": [90, 0, 0], + "radius": 4, + "color": [0.90, 0.20, 0.20] + }, + "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" + }, + + "skeleton": { + "from": [0, 0, 0], + "to": [0, -250, 0], + "radius": 4, + "color": [0.95, 0.85, 0.20] + }, + "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" + }, + + "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" + }, + + "skeleton": { + "from": [-50, -35, 0], + "to": [50, -35, 0], + "radius": 7, + "color": [0.95, 0.20, 0.20] + } + }, + "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" + }, + "skeleton": { + "from": [0, 0,0], + "to": [0, -60, 0], + "radius": 4, + "color": [0.20, 0.80, 0.20] + }, + "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.40], "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" + }, + "skeleton": { + "from": [0, 0,0], + "to": [0, -60, 0], + "radius": 4, + "color": [0.20, 0.80, 0.20] + }, + "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.40], "spin": -27} + ], + "model": [ + { + "stlFile": "surfaces/Finger.stl", + "originOfModel": [-24,0,9.1], + "rotationOfModelDegree": [90, 90,0], + "material": "defaultPlastic" + } + ] + } + } +} \ No newline at end of file diff --git a/data/robot/surfaces/Base.stl b/data/robot/surfaces/Base.stl new file mode 100644 index 0000000..6ba6f3f Binary files /dev/null and b/data/robot/surfaces/Base.stl differ diff --git a/data/robot/surfaces/Board.stl b/data/robot/surfaces/Board.stl new file mode 100644 index 0000000..c6fd521 Binary files /dev/null and b/data/robot/surfaces/Board.stl differ diff --git a/data/robot/surfaces/BoardRail.stl b/data/robot/surfaces/BoardRail.stl new file mode 100644 index 0000000..9a1579a Binary files /dev/null and b/data/robot/surfaces/BoardRail.stl differ diff --git a/data/robot/surfaces/Ellebogen.3mf b/data/robot/surfaces/Ellebogen.3mf new file mode 100644 index 0000000..f9cafc0 Binary files /dev/null and b/data/robot/surfaces/Ellebogen.3mf differ diff --git a/data/robot/surfaces/Ellebogen.stl b/data/robot/surfaces/Ellebogen.stl new file mode 100644 index 0000000..3289c3f Binary files /dev/null and b/data/robot/surfaces/Ellebogen.stl differ diff --git a/data/robot/surfaces/Finger.stl b/data/robot/surfaces/Finger.stl new file mode 100644 index 0000000..073934e Binary files /dev/null and b/data/robot/surfaces/Finger.stl differ diff --git a/data/robot/surfaces/Holm.stl b/data/robot/surfaces/Holm.stl new file mode 100644 index 0000000..2d4bddb Binary files /dev/null and b/data/robot/surfaces/Holm.stl differ diff --git a/data/robot/surfaces/Unterarm.3mf b/data/robot/surfaces/Unterarm.3mf new file mode 100644 index 0000000..50c04b4 Binary files /dev/null and b/data/robot/surfaces/Unterarm.3mf differ diff --git a/data/robot/surfaces/Unterarm.stl b/data/robot/surfaces/Unterarm.stl new file mode 100644 index 0000000..d50d5ca Binary files /dev/null and b/data/robot/surfaces/Unterarm.stl differ diff --git a/data/robot/surfaces/boden.jpg b/data/robot/surfaces/boden.jpg new file mode 100644 index 0000000..f9c65ab Binary files /dev/null and b/data/robot/surfaces/boden.jpg differ diff --git a/scripts/4_robotState_estimation_v6.py b/scripts/4_robotState_estimation_v6.py new file mode 100644 index 0000000..709eb95 --- /dev/null +++ b/scripts/4_robotState_estimation_v6.py @@ -0,0 +1,1217 @@ +#!/usr/bin/env python3 +""" +4_robotState_estimation_v6.py + +Deterministic geometric robot-state estimation from optimized 3D ArUco marker positions. + +Mathematical idea +----------------- +This script does NOT run a generic minimizer by default. It estimates the robot +state q with closed-form geometric rules and sequential prefix expansion. + +Input: + aruco_positions_optimized*.json + robot.json + +Core geometric steps +-------------------- +1) Root pose estimation + The root link pose (Board) is estimated from its observed markers using a + weighted Kabsch fit: + R_root, t_root = argmin || R P_i + t - Q_i ||^2 + with weights driven by marker size. + +2) Linear joints / sliders + For a prismatic joint along axis a, the joint variable q is estimated by + weighted averaging of the projection residuals: + q = mean_i w_i * dot(a_world, p_obs_i - p_pred_i(q=0)) / mean_i w_i + This is a closed-form geometry update, not a numeric optimizer. + +3) Revolute joints + For a revolute joint around axis a, the joint angle is estimated by + projecting the marker vectors into the plane orthogonal to a and solving a + 2D weighted rotation alignment: + theta = atan2( sum_i w_i * cross2(u_i, v_i), + sum_i w_i * dot2(u_i, v_i) ) + where u_i are predicted vectors (q=0) and v_i are observed vectors. + + To handle 180° flip ambiguities, the script optionally compares theta and + theta + pi and uses a weak normal-based tie-break. Marker normals from + robot.json are used as a geometric hint, not as hard image evidence. + +4) Sequential prefix expansion + The chain is estimated link-by-link from the root outwards. Each stage only + activates the first N links (controlled by robot.json::state_pose_params), + re-estimates the active prefix, and keeps the previous prefix as the start + of the next stage. + +This is intentionally designed as a fast, deterministic geometric initializer +for later refinement, not as a full bundle-adjustment solver. + +Notes +----- +- The script uses the robot hierarchy, joint axes, joint origins, and marker + local positions from robot.json. +- No least-squares minimizer is used in the default path. +- Solver-related flags may be present in robot.json for compatibility but are + not used here. + +Dependencies +------------ +numpy only +""" + +from __future__ import annotations + +import argparse +import json +import math +import os +import sys +import time +from dataclasses import dataclass +from typing import Any, Dict, List, Optional, Tuple + +import numpy as np + + +# ============================================================================= +# Path / JSON helpers +# ============================================================================= + +def resolve_path(path: str) -> str: + path = os.path.expanduser(path) + if os.path.isabs(path): + return path + return os.path.abspath(path) + + +def load_json(path: str) -> Any: + with open(resolve_path(path), "r", encoding="utf-8") as f: + return json.load(f) + + +def save_json(path: str, data: Any) -> None: + with open(resolve_path(path), "w", encoding="utf-8") as f: + json.dump(data, f, indent=2) + + +# ============================================================================= +# Small geometry helpers +# ============================================================================= + +def safe_norm(v: np.ndarray, eps: float = 1e-12) -> float: + return float(np.linalg.norm(v) + eps) + + +def normalize(v: np.ndarray, eps: float = 1e-12) -> np.ndarray: + v = np.asarray(v, dtype=np.float64) + return v / safe_norm(v, eps) + + +def wrap_angle_rad(a: float) -> float: + return (a + math.pi) % (2.0 * math.pi) - math.pi + + +def get_length_scale(robot_data: Dict[str, Any]) -> float: + units = robot_data.get("units", {}) or {} + length_unit = str(units.get("length", "")).strip().lower() + if length_unit in ("mm", "millimeter", "millimeters"): + return 1.0 / 1000.0 + if length_unit in ("cm", "centimeter", "centimeters"): + return 1.0 / 100.0 + return 1.0 + + +def rotation_matrix_xyz(rx: float, ry: float, rz: float, degrees: bool = False) -> np.ndarray: + """Rotation order: X then Y then Z.""" + if degrees: + rx, ry, rz = math.radians(rx), math.radians(ry), math.radians(rz) + + cx, sx = math.cos(rx), math.sin(rx) + cy, sy = math.cos(ry), math.sin(ry) + cz, sz = math.cos(rz), math.sin(rz) + + rx_m = np.array([[1.0, 0.0, 0.0], + [0.0, cx, -sx], + [0.0, sx, cx]], dtype=np.float64) + ry_m = np.array([[cy, 0.0, sy], + [0.0, 1.0, 0.0], + [-sy, 0.0, cy]], dtype=np.float64) + rz_m = np.array([[cz, -sz, 0.0], + [sz, cz, 0.0], + [0.0, 0.0, 1.0]], dtype=np.float64) + return rz_m @ ry_m @ rx_m + + +def axis_angle_rotation(axis: np.ndarray, angle_rad: float) -> np.ndarray: + axis = normalize(axis) + x, y, z = axis + c = math.cos(angle_rad) + s = math.sin(angle_rad) + C = 1.0 - c + return np.array([ + [c + x * x * C, x * y * C - z * s, x * z * C + y * s], + [y * x * C + z * s, c + y * y * C, y * z * C - x * s], + [z * x * C - y * s, z * y * C + x * s, c + z * z * C], + ], dtype=np.float64) + + +def make_transform(R: Optional[np.ndarray] = None, t: Optional[np.ndarray] = None) -> np.ndarray: + T = np.eye(4, dtype=np.float64) + if R is not None: + T[:3, :3] = np.asarray(R, dtype=np.float64) + if t is not None: + T[:3, 3] = np.asarray(t, dtype=np.float64).reshape(3) + return T + + +def transform_point(T: np.ndarray, p: np.ndarray) -> np.ndarray: + p4 = np.ones(4, dtype=np.float64) + p4[:3] = np.asarray(p, dtype=np.float64).reshape(3) + return (T @ p4)[:3] + + +def transform_vector(T: np.ndarray, v: np.ndarray) -> np.ndarray: + return T[:3, :3] @ np.asarray(v, dtype=np.float64).reshape(3) + + +def kabsch(P: np.ndarray, Q: np.ndarray, W: Optional[np.ndarray] = None) -> Tuple[np.ndarray, np.ndarray]: + """Weighted Kabsch fit: find R, t so that R @ P + t ≈ Q.""" + P = np.asarray(P, dtype=np.float64) + Q = np.asarray(Q, dtype=np.float64) + assert P.shape == Q.shape and P.shape[1] == 3 + + if W is None: + W = np.ones(len(P), dtype=np.float64) + W = np.asarray(W, dtype=np.float64).reshape(-1) + W = np.maximum(W, 1e-12) + W = W / np.sum(W) + + p_cent = np.sum(P * W[:, None], axis=0) + q_cent = np.sum(Q * W[:, None], axis=0) + + P0 = P - p_cent + Q0 = Q - q_cent + H = (P0 * W[:, None]).T @ Q0 + + U, S, Vt = np.linalg.svd(H) + R = Vt.T @ U.T + if np.linalg.det(R) < 0: + Vt[-1, :] *= -1.0 + R = Vt.T @ U.T + t = q_cent - R @ p_cent + return R, t + + +def principal_axis_id(axis: np.ndarray, threshold: float = 0.95) -> Optional[int]: + a = normalize(np.asarray(axis, dtype=np.float64)) + idx = int(np.argmax(np.abs(a))) + if abs(a[idx]) >= threshold: + return idx + return None + + +def axis_unit_from_id(axis_id: int, sign: float = 1.0) -> np.ndarray: + v = np.zeros(3, dtype=np.float64) + v[axis_id] = 1.0 if sign >= 0 else -1.0 + return v + + +def plane_basis_for_axis(axis: np.ndarray) -> Tuple[np.ndarray, np.ndarray, int]: + """ + Return a 2D basis (e1, e2) spanning the plane orthogonal to axis. + Also return the principal axis index if axis is close to x/y/z. + """ + a = normalize(axis) + axis_id = int(np.argmax(np.abs(a))) + + if axis_id == 0: # x -> yz-plane + e1 = np.array([0.0, 1.0, 0.0], dtype=np.float64) + e2 = np.array([0.0, 0.0, 1.0], dtype=np.float64) + elif axis_id == 1: # y -> zx-plane + e1 = np.array([0.0, 0.0, 1.0], dtype=np.float64) + e2 = np.array([1.0, 0.0, 0.0], dtype=np.float64) + else: # z -> xy-plane + e1 = np.array([1.0, 0.0, 0.0], dtype=np.float64) + e2 = np.array([0.0, 1.0, 0.0], dtype=np.float64) + + return e1, e2, axis_id + + +def project_to_plane(v: np.ndarray, axis: np.ndarray) -> np.ndarray: + axis = normalize(axis) + return np.asarray(v, dtype=np.float64) - np.dot(v, axis) * axis + + +def weighted_mean(values: List[float], weights: List[float]) -> float: + if not values or not weights: + return 0.0 + w = np.asarray(weights, dtype=np.float64) + v = np.asarray(values, dtype=np.float64) + s = float(np.sum(w)) + if s <= 1e-12: + return float(np.mean(v)) + return float(np.sum(v * w) / s) + + +def marker_quality_weight(marker: "MarkerInfo", ref_size: float = 25.0) -> float: + w = 1.0 + if marker.size is not None: + try: + size = float(marker.size) + if size > 0: + w *= max(0.35, math.sqrt(size / ref_size)) + except Exception: + pass + return float(w) + + +def normal_flip_bias(marker: "MarkerInfo", world_up: np.ndarray = np.array([0.0, 0.0, 1.0], dtype=np.float64)) -> float: + """ + Weak bias for top/bottom flip discrimination. + Only markers with a strong ±z local normal contribute. + """ + if marker.local_normal is None: + return 0.0 + n = normalize(marker.local_normal) + if abs(n[2]) < 0.5: + return 0.0 + pref = 1.0 if n[2] >= 0 else -1.0 + # Positive score means "prefer world_up alignment" for top markers, + # negative score means "prefer world_down alignment" for bottom markers. + return pref + + +# ============================================================================= +# Robot structures +# ============================================================================= + +@dataclass +class MarkerInfo: + marker_id: int + name: str + local_pos_m: np.ndarray + local_normal: np.ndarray + size: Optional[float] + spin: Optional[float] + link_name: str + + +@dataclass +class LinkInfo: + name: str + parent: Optional[str] + joint_type: str + joint_axis: Optional[np.ndarray] + joint_variable: Optional[str] + joint_origin_m: np.ndarray + joint_rotation_deg: np.ndarray + mount_position_m: np.ndarray + mount_rotation_deg: np.ndarray + markers: List[MarkerInfo] + + +@dataclass +class StatePoseParams: + numbers_of_elements_to_consider_start: int = 4 + numbers_of_elements_to_consider_final: int = 5 + geometric_passes_per_stage: int = 2 + root_pose_min_markers: int = 2 + use_marker_normals_flip_tiebreak: bool = True + normal_flip_weight: float = 0.05 + + +def load_state_pose_params(robot_data: Dict[str, Any]) -> StatePoseParams: + raw = robot_data.get("state_pose_params", {}) or {} + return StatePoseParams( + numbers_of_elements_to_consider_start=max(1, int(raw.get("numbers_of_Elements_to_consider_start", 4))), + numbers_of_elements_to_consider_final=max(1, int(raw.get("numbers_of_Elements_to_consider_final", 5))), + geometric_passes_per_stage=max(1, int(raw.get("geometric_passes_per_stage", 2))), + root_pose_min_markers=max(1, int(raw.get("root_pose_min_markers", 2))), + use_marker_normals_flip_tiebreak=bool(raw.get("use_marker_normals_flip_tiebreak", True)), + normal_flip_weight=float(raw.get("normal_flip_weight", 0.05)), + ) + + +def parse_robot(robot_data: Dict[str, Any], length_scale: float) -> Tuple[Dict[str, LinkInfo], Dict[int, str], List[str]]: + links = robot_data.get("links", {}) or {} + link_infos: Dict[str, LinkInfo] = {} + marker_by_id: Dict[int, str] = {} + issues: List[str] = [] + + for link_name, link_data in links.items(): + parent = link_data.get("parent", None) + + joint = link_data.get("jointToParent", {}) or {} + joint_type = str(joint.get("type", "")).strip().lower() + axis = joint.get("axis", None) + joint_axis = None + if axis is not None: + axis_arr = np.asarray(axis, dtype=np.float64) + if safe_norm(axis_arr) > 1e-12: + joint_axis = normalize(axis_arr) + + joint_variable = joint.get("variable", None) + joint_origin_m = np.asarray(joint.get("origin", [0, 0, 0]), dtype=np.float64) * float(length_scale) + joint_rotation_deg = np.asarray(joint.get("rotation", [0, 0, 0]), dtype=np.float64) + + mount_position_m = np.asarray(link_data.get("mountPosition", [0, 0, 0]), dtype=np.float64) * float(length_scale) + mount_rotation_deg = np.asarray(link_data.get("mountRotation", [0, 0, 0]), dtype=np.float64) + + markers: List[MarkerInfo] = [] + seen_local: set[int] = set() + for idx, marker in enumerate(link_data.get("markers", []) or []): + marker_id = int(marker.get("id", -1)) + pos = marker.get("position", None) + if marker_id < 0 or pos is None or len(pos) != 3: + issues.append(f"[WARN] link={link_name}: skipped invalid marker entry at index {idx}") + continue + if marker_id in seen_local: + issues.append(f"[WARN] link={link_name}: duplicate marker id {marker_id} inside same link -> skipped") + continue + if marker_id in marker_by_id: + issues.append( + f"[WARN] marker id {marker_id} already assigned to link '{marker_by_id[marker_id]}', " + f"duplicate in link '{link_name}' -> skipped" + ) + continue + + seen_local.add(marker_id) + marker_by_id[marker_id] = link_name + markers.append( + MarkerInfo( + marker_id=marker_id, + name=str(marker.get("name", f"aruco_{marker_id}")), + local_pos_m=np.asarray(pos, dtype=np.float64) * float(length_scale), + local_normal=normalize(np.asarray(marker.get("normal", [0, 0, 1]), dtype=np.float64)), + size=marker.get("size", None), + spin=marker.get("spin", None), + link_name=link_name, + ) + ) + + link_infos[link_name] = LinkInfo( + name=link_name, + parent=parent, + joint_type=joint_type, + joint_axis=joint_axis, + joint_variable=joint_variable, + joint_origin_m=joint_origin_m, + joint_rotation_deg=joint_rotation_deg, + mount_position_m=mount_position_m, + mount_rotation_deg=mount_rotation_deg, + markers=markers, + ) + + return link_infos, marker_by_id, issues + + +def topological_links(link_infos: Dict[str, LinkInfo]) -> List[str]: + roots = [ln for ln, li in link_infos.items() if li.parent is None] + if not roots: + return [] + root = roots[0] + + children: Dict[str, List[str]] = {ln: [] for ln in link_infos} + for ln, li in link_infos.items(): + if li.parent is not None and li.parent in children: + children[li.parent].append(ln) + + order: List[str] = [] + queue = [root] + while queue: + cur = queue.pop(0) + if cur in order: + continue + order.append(cur) + for ch in children.get(cur, []): + queue.append(ch) + + # Append disconnected subtrees if any. + for ln in link_infos: + if ln not in order: + order.append(ln) + + return order + + +def compute_children_map(link_infos: Dict[str, LinkInfo]) -> Dict[str, List[str]]: + children: Dict[str, List[str]] = {ln: [] for ln in link_infos} + for ln, li in link_infos.items(): + if li.parent is not None and li.parent in children: + children[li.parent].append(ln) + return children + + +def subtree_links(link_name: str, children_map: Dict[str, List[str]], active_links: set[str]) -> List[str]: + out: List[str] = [] + queue = [link_name] + while queue: + cur = queue.pop(0) + if cur not in active_links: + continue + out.append(cur) + for ch in children_map.get(cur, []): + if ch in active_links: + queue.append(ch) + return out + + +def marker_ids_in_links(link_infos: Dict[str, LinkInfo], links: List[str]) -> List[int]: + ids: List[int] = [] + for link_name in links: + for m in link_infos[link_name].markers: + ids.append(m.marker_id) + return ids + + +# ============================================================================= +# Observations +# ============================================================================= + +def load_observed_markers(optimized_markers_file: str) -> Dict[int, Dict[str, Any]]: + data = load_json(optimized_markers_file) + if isinstance(data, dict): + markers = data.get("markers", []) or [] + elif isinstance(data, list): + markers = data + else: + markers = [] + + observed: Dict[int, Dict[str, Any]] = {} + for m in markers: + if not isinstance(m, dict) or "marker_id" not in m: + continue + marker_id = int(m["marker_id"]) + pos = m.get("position_m", None) + if pos is None or len(pos) != 3: + continue + obs = dict(m) + obs["marker_id"] = marker_id + obs["position_m"] = np.asarray(pos, dtype=np.float64) + observed[marker_id] = obs + return observed + + +def active_observations_for_links( + observed_markers: Dict[int, Dict[str, Any]], + link_infos: Dict[str, LinkInfo], + active_links: set[str], +) -> Dict[int, Dict[str, Any]]: + out: Dict[int, Dict[str, Any]] = {} + for marker_id, obs in observed_markers.items(): + link = obs.get("link", None) + if link is None or link == "unknown": + continue + if link not in active_links: + continue + if link not in link_infos: + continue + out[marker_id] = obs + return out + + +# ============================================================================= +# Forward kinematics +# ============================================================================= + +def link_static_transform(link: LinkInfo) -> np.ndarray: + Rm = rotation_matrix_xyz(*link.mount_rotation_deg, degrees=True) + return make_transform(Rm, link.mount_position_m) + + +def joint_motion_transform(link: LinkInfo, q_value: float) -> np.ndarray: + joint_type = (link.joint_type or "").strip().lower() + if link.joint_axis is None: + return np.eye(4, dtype=np.float64) + + axis = normalize(link.joint_axis) + if joint_type == "linear": + return make_transform(np.eye(3), axis * float(q_value)) + if joint_type == "revolute": + return make_transform(axis_angle_rotation(axis, float(q_value)), np.zeros(3)) + return np.eye(4, dtype=np.float64) + + +def world_to_link_transform( + link_name: str, + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], + cache: Dict[Tuple[str, Tuple[Tuple[str, float], ...]], np.ndarray], + joint_override: Optional[Dict[str, float]] = None, +) -> np.ndarray: + override_items = tuple(sorted((joint_override or {}).items())) + cache_key = (link_name, override_items) + if cache_key in cache: + return cache[cache_key] + + link = link_infos[link_name] + if link.parent is None: + T = make_transform(state["root_R"], state["root_t"]) + cache[cache_key] = T + return T + + parent_T = world_to_link_transform(link.parent, link_infos, state, cache, joint_override=joint_override) + + # Parent -> joint origin + T = parent_T @ make_transform(np.eye(3), link.joint_origin_m) + + # Joint motion + joint_values = state["joint_values"] + q = joint_override.get(link.name, joint_values.get(link.joint_variable, 0.0)) if joint_override else joint_values.get(link.joint_variable, 0.0) + T = T @ joint_motion_transform(link, q) + + # Static rotation after the joint + R_static = rotation_matrix_xyz(*link.joint_rotation_deg, degrees=True) + T = T @ make_transform(R_static, np.zeros(3)) + + # Mount transform / link-local offset + T = T @ link_static_transform(link) + + cache[cache_key] = T + return T + + +def predict_marker_positions( + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], + joint_override: Optional[Dict[str, float]] = None, + active_links: Optional[set[str]] = None, +) -> Dict[int, np.ndarray]: + pred: Dict[int, np.ndarray] = {} + cache: Dict[Tuple[str, Tuple[Tuple[str, float], ...]], np.ndarray] = {} + for link_name, link in link_infos.items(): + if active_links is not None and link_name not in active_links: + continue + T = world_to_link_transform(link_name, link_infos, state, cache, joint_override=joint_override) + for marker in link.markers: + pred[marker.marker_id] = transform_point(T, marker.local_pos_m) + return pred + + +def world_joint_axis_for_link( + link_name: str, + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], +) -> np.ndarray: + link = link_infos[link_name] + if link.parent is None or link.joint_axis is None: + return np.array([1.0, 0.0, 0.0], dtype=np.float64) + + cache: Dict[Tuple[str, Tuple[Tuple[str, float], ...]], np.ndarray] = {} + parent_T = world_to_link_transform(link.parent, link_infos, state, cache) + axis_world = transform_vector(parent_T, link.joint_axis) + return normalize(axis_world) + + +def world_joint_origin_for_link( + link_name: str, + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], +) -> np.ndarray: + link = link_infos[link_name] + if link.parent is None: + return np.asarray(state["root_t"], dtype=np.float64).copy() + + cache: Dict[Tuple[str, Tuple[Tuple[str, float], ...]], np.ndarray] = {} + parent_T = world_to_link_transform(link.parent, link_infos, state, cache) + return transform_point(parent_T, link.joint_origin_m) + + +# ============================================================================= +# Geometric estimation +# ============================================================================= + +def initial_root_pose( + root_link: LinkInfo, + observed_markers: Dict[int, Dict[str, Any]], + min_markers: int = 2, +) -> Tuple[np.ndarray, np.ndarray, Dict[str, Any]]: + P = [] + Q = [] + W = [] + matched = [] + + for marker in root_link.markers: + if marker.marker_id in observed_markers: + obs = np.asarray(observed_markers[marker.marker_id]["position_m"], dtype=np.float64) + P.append(marker.local_pos_m) + Q.append(obs) + W.append(marker_quality_weight(marker)) + matched.append(marker.marker_id) + + if len(P) >= min_markers: + R, t = kabsch(np.asarray(P), np.asarray(Q), np.asarray(W)) + return R, t, {"reason": "kabsch", "used_markers": matched} + + if len(P) >= 1: + marker = root_link.markers[0] + obs = np.asarray(observed_markers[marker.marker_id]["position_m"], dtype=np.float64) + return np.eye(3, dtype=np.float64), obs - marker.local_pos_m, {"reason": "single_marker_fallback", "used_markers": matched} + + return np.eye(3, dtype=np.float64), np.zeros(3, dtype=np.float64), {"reason": "no_root_markers"} + + +def estimate_linear_joint_value( + link_name: str, + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], + observed_markers: Dict[int, Dict[str, Any]], + active_links: set[str], + children_map: Dict[str, List[str]], +) -> Tuple[float, Dict[str, Any]]: + link = link_infos[link_name] + if link.joint_variable is None: + return 0.0, {"reason": "no_joint_variable"} + + subtree = subtree_links(link_name, children_map, active_links) + obs_ids = [mid for mid in marker_ids_in_links(link_infos, subtree) if mid in observed_markers] + if not obs_ids: + return float(state["joint_values"].get(link.joint_variable, 0.0)), {"reason": "no_observations"} + + axis_world = world_joint_axis_for_link(link_name, link_infos, state) + + pred0 = predict_marker_positions(link_infos, state, joint_override={link_name: 0.0}, active_links=active_links) + + num = 0.0 + den = 0.0 + per_marker = [] + for mid in obs_ids: + if mid not in pred0: + continue + marker = next((m for m in link_infos[observed_markers[mid]["link"]].markers if m.marker_id == mid), None) + w = marker_quality_weight(marker) if marker is not None else 1.0 + p_obs = np.asarray(observed_markers[mid]["position_m"], dtype=np.float64) + p0 = pred0[mid] + q_i = float(np.dot(axis_world, p_obs - p0)) + num += w * q_i + den += w + per_marker.append({"marker_id": mid, "q_i": q_i, "weight": w}) + + if den <= 1e-12: + return float(state["joint_values"].get(link.joint_variable, 0.0)), {"reason": "zero_weight"} + + q = num / den + return float(q), { + "reason": "weighted_projection", + "used_markers": len(per_marker), + "axis_world": [float(x) for x in axis_world], + "per_marker": per_marker, + } + + +def revolute_score_with_normals( + link_name: str, + q_value: float, + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], + observed_markers: Dict[int, Dict[str, Any]], + active_links: set[str], +) -> float: + """ + Weak tiebreak for q vs q + pi using marker normals. + Lower is better. + """ + link = link_infos[link_name] + if not link.markers: + return 0.0 + + pred = predict_marker_positions(link_infos, state, joint_override={link_name: q_value}, active_links=active_links) + T_link = world_to_link_transform(link_name, link_infos, state, cache={}, joint_override={link_name: q_value}) + R_link = T_link[:3, :3] + up = np.array([0.0, 0.0, 1.0], dtype=np.float64) + + score = 0.0 + for marker in link.markers: + if marker.marker_id not in observed_markers: + continue + if marker.local_normal is None: + continue + n_local = normalize(marker.local_normal) + if abs(n_local[2]) < 0.5: + continue + n_world = normalize(R_link @ n_local) + sign_pref = 1.0 if n_local[2] >= 0 else -1.0 + score += marker_quality_weight(marker) * (1.0 - sign_pref * float(np.dot(n_world, up))) + + return float(score) + + +def estimate_revolute_joint_value( + link_name: str, + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], + observed_markers: Dict[int, Dict[str, Any]], + active_links: set[str], + children_map: Dict[str, List[str]], + use_normal_tiebreak: bool = True, + normal_weight: float = 0.05, +) -> Tuple[float, Dict[str, Any]]: + link = link_infos[link_name] + if link.joint_variable is None: + return 0.0, {"reason": "no_joint_variable"} + + subtree = subtree_links(link_name, children_map, active_links) + obs_ids = [mid for mid in marker_ids_in_links(link_infos, subtree) if mid in observed_markers] + if not obs_ids: + return float(state["joint_values"].get(link.joint_variable, 0.0)), {"reason": "no_observations"} + + q0 = float(state["joint_values"].get(link.joint_variable, 0.0)) + + # Prediction with joint set to zero (only upstream geometry active). + pred0 = predict_marker_positions(link_infos, state, joint_override={link_name: 0.0}, active_links=active_links) + origin_world = world_joint_origin_for_link(link_name, link_infos, state) + axis_world = world_joint_axis_for_link(link_name, link_infos, state) + e1, e2, axis_id = plane_basis_for_axis(axis_world) + + S = 0.0 + C = 0.0 + used = 0 + per_marker = [] + + for mid in obs_ids: + if mid not in pred0: + continue + marker = next((m for m in link_infos[observed_markers[mid]["link"]].markers if m.marker_id == mid), None) + w = marker_quality_weight(marker) if marker is not None else 1.0 + + p_obs = np.asarray(observed_markers[mid]["position_m"], dtype=np.float64) + p0 = pred0[mid] + + u = project_to_plane(p0 - origin_world, axis_world) + v = project_to_plane(p_obs - origin_world, axis_world) + + u2 = np.array([np.dot(u, e1), np.dot(u, e2)], dtype=np.float64) + v2 = np.array([np.dot(v, e1), np.dot(v, e2)], dtype=np.float64) + + nu = float(np.linalg.norm(u2)) + nv = float(np.linalg.norm(v2)) + if nu <= 1e-9 or nv <= 1e-9: + continue + + S += w * float(u2[0] * v2[1] - u2[1] * v2[0]) + C += w * float(u2[0] * v2[0] + u2[1] * v2[1]) + used += 1 + per_marker.append({"marker_id": mid, "weight": w}) + + if used == 0: + return q0, {"reason": "no_valid_vectors"} + + theta = math.atan2(S, C) + theta_alt = wrap_angle_rad(theta + math.pi) + + # Evaluate both candidates deterministically, choose the better one. + def score_candidate(qcand: float) -> float: + pred = predict_marker_positions(link_infos, state, joint_override={link_name: qcand}, active_links=active_links) + err = 0.0 + for mid in obs_ids: + if mid not in pred: + continue + marker = next((m for m in link_infos[observed_markers[mid]["link"]].markers if m.marker_id == mid), None) + w = marker_quality_weight(marker) if marker is not None else 1.0 + p_obs = np.asarray(observed_markers[mid]["position_m"], dtype=np.float64) + d = pred[mid] - p_obs + err += w * float(np.dot(d, d)) + if use_normal_tiebreak: + err += normal_weight * revolute_score_with_normals(link_name, qcand, link_infos, state, observed_markers, active_links) + return float(err) + + score_theta = score_candidate(theta) + score_alt = score_candidate(theta_alt) + + best_q = theta if score_theta <= score_alt else theta_alt + best_score = min(score_theta, score_alt) + + return wrap_angle_rad(best_q), { + "reason": "2d_alignment" + ("+normal_tiebreak" if use_normal_tiebreak else ""), + "used_markers": used, + "axis_world": [float(x) for x in axis_world], + "axis_id": axis_id, + "theta_rad": float(theta), + "theta_alt_rad": float(theta_alt), + "score_theta": float(score_theta), + "score_theta_alt": float(score_alt), + "best_score": float(best_score), + "per_marker": per_marker, + } + + +def estimate_joint_geometrically( + link_name: str, + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], + observed_markers: Dict[int, Dict[str, Any]], + active_links: set[str], + children_map: Dict[str, List[str]], + params: StatePoseParams, +) -> Tuple[float, Dict[str, Any]]: + link = link_infos[link_name] + jt = (link.joint_type or "").strip().lower() + + if jt == "linear": + return estimate_linear_joint_value(link_name, link_infos, state, observed_markers, active_links, children_map) + if jt == "revolute": + return estimate_revolute_joint_value( + link_name, + link_infos, + state, + observed_markers, + active_links, + children_map, + use_normal_tiebreak=params.use_marker_normals_flip_tiebreak, + normal_weight=params.normal_flip_weight, + ) + + return float(state["joint_values"].get(link.joint_variable, 0.0)) if link.joint_variable else 0.0, {"reason": "unsupported_joint_type"} + + +# ============================================================================= +# Scoring / reporting +# ============================================================================= + +def build_state_template(link_infos: Dict[str, LinkInfo]) -> Dict[str, Any]: + return { + "root_R": np.eye(3, dtype=np.float64), + "root_t": np.zeros(3, dtype=np.float64), + "joint_values": {li.joint_variable: 0.0 for li in link_infos.values() if li.joint_variable}, + } + + +def copy_state(state: Dict[str, Any]) -> Dict[str, Any]: + return { + "root_R": np.asarray(state["root_R"], dtype=np.float64).copy(), + "root_t": np.asarray(state["root_t"], dtype=np.float64).copy(), + "joint_values": dict(state["joint_values"]), + } + + +def marker_error_report( + link_infos: Dict[str, LinkInfo], + state: Dict[str, Any], + observed_markers: Dict[int, Dict[str, Any]], + active_links: Optional[set[str]] = None, +) -> Tuple[List[Dict[str, Any]], Dict[str, Any]]: + pred = predict_marker_positions(link_infos, state, active_links=active_links) + marker_reports: List[Dict[str, Any]] = [] + errors = [] + + for marker_id, obs in observed_markers.items(): + if marker_id not in pred: + continue + p_obs = np.asarray(obs["position_m"], dtype=np.float64) + p_pred = pred[marker_id] + err = p_pred - p_obs + err_norm = float(np.linalg.norm(err)) + errors.append(err_norm) + marker_reports.append( + { + "marker_id": int(marker_id), + "link": obs.get("link", "unknown"), + "error_m": [float(x) for x in err], + "error_norm_m": err_norm, + "predicted_m": [float(x) for x in p_pred], + "observed_m": [float(x) for x in p_obs], + } + ) + + if errors: + arr = np.asarray(errors, dtype=np.float64) + stats = { + "num_markers_used": int(len(errors)), + "mean_error_m": float(np.mean(arr)), + "rms_error_m": float(math.sqrt(np.mean(arr ** 2))), + "median_error_m": float(np.median(arr)), + "worst_error_m": float(np.max(arr)), + } + else: + stats = { + "num_markers_used": 0, + "mean_error_m": None, + "rms_error_m": None, + "median_error_m": None, + "worst_error_m": None, + } + + return marker_reports, stats + + +def summarize_state(state: Dict[str, Any], link_infos: Dict[str, LinkInfo]) -> Dict[str, Any]: + movements: Dict[str, Any] = {} + for link_name, link in link_infos.items(): + q = state["joint_values"].get(link.joint_variable, None) if link.joint_variable else None + if q is None: + continue + jt = (link.joint_type or "").strip().lower() + if jt == "linear": + movements[link.joint_variable] = { + "value_m": float(q), + "value_mm": float(q * 1000.0), + "joint_type": jt, + "link": link_name, + } + elif jt == "revolute": + movements[link.joint_variable] = { + "value_rad": float(q), + "value_deg": float(math.degrees(q)), + "joint_type": jt, + "link": link_name, + } + else: + movements[link.joint_variable] = { + "value": float(q), + "joint_type": jt, + "link": link_name, + } + + root_R = np.asarray(state["root_R"], dtype=np.float64) + root_t = np.asarray(state["root_t"], dtype=np.float64) + return { + "root_pose": { + "translation_m": [float(x) for x in root_t], + "rotation_matrix": [[float(x) for x in row] for row in root_R], + "euler_xyz_deg": [float(x) for x in np.degrees(np.array([ + math.atan2(root_R[2, 1], root_R[2, 2]), + math.atan2(-root_R[2, 0], math.sqrt(root_R[0, 0] ** 2 + root_R[1, 0] ** 2)), + math.atan2(root_R[1, 0], root_R[0, 0]), + ]))], + }, + "movements": movements, + } + + +# ============================================================================= +# Sequential geometric estimation +# ============================================================================= + +def optimize_geometric_prefix( + link_infos: Dict[str, LinkInfo], + observed_markers: Dict[int, Dict[str, Any]], + initial_state: Dict[str, Any], + active_links: List[str], + params: StatePoseParams, +) -> Tuple[Dict[str, Any], Dict[str, Any]]: + active_set = set(active_links) + children_map = compute_children_map(link_infos) + stage_obs = active_observations_for_links(observed_markers, link_infos, active_set) + + state = copy_state(initial_state) + stage_info: Dict[str, Any] = { + "method": "deterministic_geometric_prefix", + "active_links": active_links, + "active_observations": len(stage_obs), + "joint_updates": [], + } + + root_link_name = next((ln for ln in active_links if link_infos[ln].parent is None), None) + if root_link_name is not None: + root_link = link_infos[root_link_name] + root_R, root_t, root_info = initial_root_pose(root_link, stage_obs, min_markers=params.root_pose_min_markers) + state["root_R"] = root_R + state["root_t"] = root_t + stage_info["root_link"] = root_link_name + stage_info["root_pose"] = root_info + + active_joint_links = [ln for ln in active_links if link_infos[ln].parent is not None] + + for pass_idx in range(params.geometric_passes_per_stage): + pass_updates = [] + for link_name in active_joint_links: + link = link_infos[link_name] + if not link.joint_variable: + continue + q_old = float(state["joint_values"].get(link.joint_variable, 0.0)) + q_new, info = estimate_joint_geometrically( + link_name, + link_infos, + state, + stage_obs, + active_set, + children_map, + params, + ) + state["joint_values"][link.joint_variable] = q_new + pass_updates.append( + { + "link": link_name, + "joint_variable": link.joint_variable, + "joint_type": link.joint_type, + "old": q_old, + "new": q_new, + "info": info, + } + ) + stage_info["joint_updates"].append({"pass": pass_idx, "updates": pass_updates}) + + marker_reports, stats = marker_error_report(link_infos, state, stage_obs, active_links=active_set) + stage_info["marker_stats"] = stats + stage_info["marker_reports"] = marker_reports[:] + + return state, stage_info + + +def sequential_geometric_estimation( + link_infos: Dict[str, LinkInfo], + observed_markers: Dict[int, Dict[str, Any]], + params: StatePoseParams, +) -> Tuple[Dict[str, Any], List[Dict[str, Any]]]: + ordered = topological_links(link_infos) + if not ordered: + return build_state_template(link_infos), [] + + start_n = max(1, min(int(params.numbers_of_elements_to_consider_start), len(ordered))) + final_n = max(start_n, min(int(params.numbers_of_elements_to_consider_final), len(ordered))) + + state = build_state_template(link_infos) + stage_reports: List[Dict[str, Any]] = [] + + for n in range(start_n, final_n + 1): + active_links = ordered[:n] + state, stage_info = optimize_geometric_prefix(link_infos, observed_markers, state, active_links, params) + stage_info["stage_idx"] = len(stage_reports) + stage_info["num_active_links"] = len(active_links) + stage_info["active_links"] = active_links + + stage_obs = active_observations_for_links(observed_markers, link_infos, set(active_links)) + marker_reports, stats = marker_error_report(link_infos, state, stage_obs, active_links=set(active_links)) + stage_info["marker_stats"] = stats + stage_info["marker_reports"] = marker_reports + stage_reports.append(stage_info) + + mean_mm = None if stats["mean_error_m"] is None else stats["mean_error_m"] * 1000.0 + print( + f"[INFO] Stage {stage_info['stage_idx']} | links={len(active_links)} | " + f"markers_used={stats['num_markers_used']} | " + f"mean_error={'n/a' if mean_mm is None else f'{mean_mm:.2f} mm'}" + ) + + return state, stage_reports + + +# ============================================================================= +# Serialization +# ============================================================================= + +def link_world_poses(link_infos: Dict[str, LinkInfo], state: Dict[str, Any]) -> Dict[str, Any]: + poses: Dict[str, Any] = {} + cache: Dict[Tuple[str, Tuple[Tuple[str, float], ...]], np.ndarray] = {} + for link_name in link_infos: + T = world_to_link_transform(link_name, link_infos, state, cache) + R = T[:3, :3] + t = T[:3, 3] + poses[link_name] = { + "translation_m": [float(x) for x in t], + "rotation_matrix": [[float(x) for x in row] for row in R], + } + return poses + + +def state_to_json( + state: Dict[str, Any], + link_infos: Dict[str, LinkInfo], + stage_reports: List[Dict[str, Any]], + observed_markers: Dict[int, Dict[str, Any]], +) -> Dict[str, Any]: + movements = summarize_state(state, link_infos)["movements"] + root_pose = summarize_state(state, link_infos)["root_pose"] + predicted = predict_marker_positions(link_infos, state) + + markers_out = [] + for marker_id, obs in sorted(observed_markers.items()): + entry = { + "marker_id": int(marker_id), + "link": obs.get("link", "unknown"), + "observed_position_m": [float(x) for x in np.asarray(obs["position_m"], dtype=np.float64)], + } + if marker_id in predicted: + entry["predicted_position_m"] = [float(x) for x in predicted[marker_id]] + err = predicted[marker_id] - np.asarray(obs["position_m"], dtype=np.float64) + entry["error_m"] = [float(x) for x in err] + entry["error_norm_m"] = float(np.linalg.norm(err)) + markers_out.append(entry) + + return { + "schema_version": "1.0", + "method": "deterministic_geometric_sequential_prefix", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "root_pose": root_pose, + "movements": movements, + "link_poses": link_world_poses(link_infos, state), + "stage_reports": stage_reports, + "markers": markers_out, + } + + +# ============================================================================= +# Main +# ============================================================================= + +def main() -> None: + parser = argparse.ArgumentParser( + description="Sequential deterministic geometric robot-state estimation from optimized ArUco marker positions" + ) + parser.add_argument("optimized_markers", help="aruco_positions_optimized*.json") + parser.add_argument("-robot", "--robot", required=True, help="robot.json") + parser.add_argument("-out", "--out", default=None, help="Output robot_state.json path") + parser.add_argument("--prefixStart", type=int, default=None, help="Override state_pose_params start") + parser.add_argument("--prefixFinal", type=int, default=None, help="Override state_pose_params final") + parser.add_argument("--passes", type=int, default=None, help="Override geometric passes per stage") + + args = parser.parse_args() + + print("[STEP 1] Load robot.json and optimized marker positions...") + robot_data = load_json(args.robot) + length_scale = get_length_scale(robot_data) + params = load_state_pose_params(robot_data) + + if args.prefixStart is not None: + params.numbers_of_elements_to_consider_start = max(1, int(args.prefixStart)) + if args.prefixFinal is not None: + params.numbers_of_elements_to_consider_final = max(1, int(args.prefixFinal)) + if args.passes is not None: + params.geometric_passes_per_stage = max(1, int(args.passes)) + + link_infos, marker_by_id, issues = parse_robot(robot_data, length_scale) + observed_map = load_observed_markers(args.optimized_markers) + + for issue in issues: + print(issue) + + print(f"[INFO] Links: {len(link_infos)}") + print(f"[INFO] Known robot markers: {len(marker_by_id)}") + print(f"[INFO] Observed optimized markers: {len(observed_map)}") + print( + f"[INFO] state_pose_params: start={params.numbers_of_elements_to_consider_start}, " + f"final={params.numbers_of_elements_to_consider_final}, " + f"passes={params.geometric_passes_per_stage}, " + f"normal_flip_tiebreak={params.use_marker_normals_flip_tiebreak}" + ) + + print("[STEP 2] Sequential geometric reconstruction...") + state, stage_reports = sequential_geometric_estimation(link_infos, observed_map, params) + + out_data = state_to_json(state, link_infos, stage_reports, observed_map) + + out_dir = os.path.dirname(resolve_path(args.out)) if args.out else os.path.dirname(resolve_path(args.optimized_markers)) + if not out_dir: + out_dir = "." + os.makedirs(out_dir, exist_ok=True) + + out_file = args.out or os.path.join(out_dir, "robot_state.json") + save_json(out_file, out_data) + + print(f"[INFO] Saved robot state to {out_file}") + + # Compact summary of the resulting movements + print("\n[INFO] Estimated movements:") + for key, info in out_data["movements"].items(): + if "value_mm" in info: + print(f" {key}: {info['value_mm']:.3f} mm ({info['link']}, {info['joint_type']})") + elif "value_deg" in info: + print(f" {key}: {info['value_deg']:.3f} deg ({info['link']}, {info['joint_type']})") + else: + print(f" {key}: {info.get('value', 0.0):.6f} ({info.get('link', 'unknown')}, {info.get('joint_type', 'unknown')})") + + +if __name__ == "__main__": + main() diff --git a/scripts/4b_revolute_angle.py b/scripts/4b_revolute_angle.py new file mode 100644 index 0000000..18fb439 --- /dev/null +++ b/scripts/4b_revolute_angle.py @@ -0,0 +1,360 @@ +#!/usr/bin/env python3 +""" +4b_revolute_angle.py +-------------------- +Generic revolute-joint angle estimator. + +For each movable link (Arm1, Ellbow, Arm2 …) whose joint type is 'revolute', +this script estimates the rotation angle using the pairwise-vector method: + + For every PAIR (m1, m2) of markers belonging to the target link: + v_model = local_pos_m2 - local_pos_m1 (in link's own frame) + v_obs = world_pos_m2 - world_pos_m1 (in world frame) + + Both vectors are projected perpendicular to the joint axis (in world frame), + and the signed angle from v_model_perp to v_obs_perp is measured. + + The joint axis in world frame is computed via FK using the already-known + joint values (from 4a, 4b-prev …), so it is ALWAYS correct — even for + deeply-nested joints whose world-frame axis differs from their local axis. + + Pair weights = baseline_model × baseline_obs (longer baselines → more reliable). + +How to use sequentially +----------------------- +Run 4b once per revolute joint, from root to tip: + + python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Arm1 --x-mm 180 + python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Ellbow --from-state state.json + python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Arm2 --from-state state.json + +The --from-state flag reads the accumulated joint state JSON so you don't have +to pass every preceding value on the command line. + +Output JSON +----------- +{ + "link": "Arm1", + "joint": "y", + "mean_angle_deg": 86.3, + "circular_std_deg": 0.7, + "num_pairs": 6, + "joint_origin_world_mm": [290, 108, 61], + "joint_axis_world": [-1, 0, 0], + ... +} + +The file also contains the full accumulated state so the next 4b invocation +can read it via --from-state. +""" + +from __future__ import annotations + +import argparse +import json +import math +from itertools import combinations +from pathlib import Path +from typing import Dict, List, Optional, Sequence, Tuple + +import numpy as np +from robot_fk import RobotFK + +STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e") + + +# ────────────────────────────────────────────────────────────── +# I/O +# ────────────────────────────────────────────────────────────── + +def _load_json(path: Path) -> dict: + with path.open("r", encoding="utf-8") as f: + return json.load(f) + + +def _load_observed_mm(aruco_json: dict) -> Dict[int, np.ndarray]: + result: Dict[int, np.ndarray] = {} + for m in aruco_json.get("markers", []): + mid = int(m.get("marker_id", m.get("id", -1))) + if mid < 0: + continue + if "position_mm" in m: + result[mid] = np.array(m["position_mm"], dtype=float) + elif "position_m" in m: + result[mid] = np.array(m["position_m"], dtype=float) * 1000.0 + return result + + +def _load_state(path: Path) -> Dict[str, float]: + """Load accumulated joint state from a previous 4b output JSON.""" + raw = _load_json(path) + state = raw.get("accumulated_state", raw.get("state", {})) + return {k: float(v) for k, v in state.items() if k in STATE_KEYS} + + +# ────────────────────────────────────────────────────────────── +# Angle maths +# ────────────────────────────────────────────────────────────── + +def _project_perp(v: np.ndarray, axis: np.ndarray) -> np.ndarray: + """Remove the component of v along axis.""" + a = axis / (np.linalg.norm(axis) + 1e-15) + return v - np.dot(v, a) * a + + +def _signed_angle_rad(v_from: np.ndarray, v_to: np.ndarray, + axis: np.ndarray) -> float: + """Signed angle rotating v_from onto v_to around axis (radians).""" + a = axis / (np.linalg.norm(axis) + 1e-15) + return math.atan2(float(np.dot(a, np.cross(v_from, v_to))), + float(np.dot(v_from, v_to))) + + +def _wrap(angle: float) -> float: + """Wrap to (−π, π].""" + return (angle + math.pi) % (2.0 * math.pi) - math.pi + + +def _circular_mean_deg(angles_rad: np.ndarray, + weights: np.ndarray) -> Tuple[float, float, float]: + """Returns mean_deg, circular_variance, circular_std_deg.""" + w = np.clip(weights, 0, None) + if w.sum() < 1e-15: + w = np.ones_like(w) + s, c = np.sum(w * np.sin(angles_rad)), np.sum(w * np.cos(angles_rad)) + mean = math.atan2(s, c) + R = math.sqrt(s*s + c*c) / w.sum() + R = float(np.clip(R, 0, 1)) + c_var = 1.0 - R + c_std = math.sqrt(max(0.0, -2.0 * math.log(max(R, 1e-15)))) + return math.degrees(mean), c_var, math.degrees(c_std) + + +# ────────────────────────────────────────────────────────────── +# Core estimator (generic — works for any revolute joint) +# ────────────────────────────────────────────────────────────── + +def estimate_revolute_angle( + fk: RobotFK, + observed_mm: Dict[int, np.ndarray], + link_name: str, + known_state: Dict[str, float], + min_baseline_mm: float = 15.0, + verbose: bool = True, +) -> dict: + """ + Estimate the revolute joint angle for `link_name`. + + Parameters + ---------- + fk : RobotFK + observed_mm : world marker positions from step 3 + link_name : e.g. "Arm1", "Ellbow", "Arm2" + known_state : already-estimated joint values (e.g. {"x": 180.0, "y": 86.0}) + The target joint variable should NOT be in this dict. + min_baseline_mm : skip pairs with model or observed baseline shorter than this + verbose : print report + + Returns + ------- + dict with estimation results + updated accumulated_state + """ + + # ── sanity checks ───────────────────────────────────────── + links = fk.links + if link_name not in links: + return {"status": "failed", + "reason": f"Link '{link_name}' not in robot.json"} + + ji = links[link_name].get("jointToParent", {}) or {} + jtype = str(ji.get("type", "")).lower() + if jtype != "revolute": + return {"status": "failed", + "reason": f"Joint of '{link_name}' is not revolute (type='{jtype}')"} + + var = str(ji.get("variable", "")).lower() + + # ── FK: joint origin and axis in world ─────────────────── + # Use known_state with the TARGET joint at 0 + zero_state = dict(known_state) + zero_state[var] = 0.0 + + origin_world = fk.joint_origin_world(link_name, zero_state) + axis_world = fk.joint_axis_world(link_name, zero_state) + + # ── collect matched markers ─────────────────────────────── + model_local: Dict[int, np.ndarray] = {} + for m in links[link_name].get("markers", []): + mid = int(m.get("id", -1)) + if mid >= 0 and "position" in m: + model_local[mid] = np.array(m["position"], dtype=float) + + matched = {mid: (model_local[mid], observed_mm[mid]) + for mid in model_local if mid in observed_mm} + + if len(matched) < 2: + return { + "status": "failed", + "reason": (f"Need ≥2 matched markers, found {len(matched)}: " + f"{list(matched.keys())}. " + f"Model marker IDs: {list(model_local.keys())}"), + } + + # ── pairwise estimation ─────────────────────────────────── + ids = sorted(matched.keys()) + angle_rad_list: List[float] = [] + weight_list: List[float] = [] + per_pair: List[dict] = [] + + for id1, id2 in combinations(ids, 2): + l1, o1 = matched[id1] + l2, o2 = matched[id2] + + v_model = l2 - l1 # local frame, both in same link + v_obs = o2 - o1 # world frame + + v_model_perp = _project_perp(v_model, axis_world) + v_obs_perp = _project_perp(v_obs, axis_world) + + bl_model = float(np.linalg.norm(v_model_perp)) + bl_obs = float(np.linalg.norm(v_obs_perp)) + + if bl_model < min_baseline_mm or bl_obs < min_baseline_mm: + per_pair.append({ + "marker_ids": [id1, id2], + "skipped": True, + "reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}", + }) + continue + + angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world)) + w = bl_model * bl_obs + + angle_rad_list.append(angle) + weight_list.append(w) + per_pair.append({ + "marker_ids": [id1, id2], + "skipped": False, + "angle_deg": math.degrees(angle), + "baseline_model_mm": bl_model, + "baseline_obs_mm": bl_obs, + "weight": w, + }) + + if not angle_rad_list: + return { + "status": "failed", + "reason": "All pairs below min_baseline_mm. " + "Try --min-baseline 5 or check step-3 output.", + } + + mean_deg, c_var, c_std_deg = _circular_mean_deg( + np.array(angle_rad_list), np.array(weight_list) + ) + + # ── verbose report ──────────────────────────────────────── + if verbose: + print(f"\n── 4b: '{link_name}' angle ({var}) ──────────────────────") + print(f" Joint origin (world): [{', '.join(f'{v:.1f}' for v in origin_world)}] mm") + print(f" Joint axis (world): [{', '.join(f'{v:.3f}' for v in axis_world)}]") + print(f" Matched markers: {list(matched.keys())}") + print(f" Pairs used: {len(angle_rad_list)} / {len(list(combinations(ids, 2)))}") + print(f" Angle: {mean_deg:+.2f} ° circular_σ {c_std_deg:.2f} °") + if c_std_deg > 5.0: + print(f" [WARN] high spread – step-3 errors or marker overlap") + print(f"\n Pair detail:") + for pp in per_pair: + if pp["skipped"]: + print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: SKIPPED – {pp['reason']}") + else: + print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: " + f"{pp['angle_deg']:+7.2f}° " + f"bl_model={pp['baseline_model_mm']:.1f} " + f"bl_obs={pp['baseline_obs_mm']:.1f}") + + # ── build accumulated state ─────────────────────────────── + accumulated = dict(known_state) + accumulated[var] = mean_deg + + return { + "status": "ok", + "link": link_name, + "joint": var, + "joint_origin_world_mm": origin_world.tolist(), + "joint_axis_world": axis_world.tolist(), + "mean_angle_deg": mean_deg, + "circular_variance": c_var, + "circular_std_deg": c_std_deg, + "num_pairs_used": len(angle_rad_list), + "num_markers_matched": len(matched), + "per_pair": per_pair, + "accumulated_state": accumulated, + } + + +# ────────────────────────────────────────────────────────────── +# CLI +# ────────────────────────────────────────────────────────────── + +def main() -> int: + p = argparse.ArgumentParser( + description="4b: estimate revolute joint angle for any link") + p.add_argument("--robot", type=Path, default=Path("robot.json")) + p.add_argument("--aruco", type=Path, + default=Path("aruco_positions_optimized.json")) + p.add_argument("--link", type=str, required=True, + help="Link name, e.g. Arm1, Ellbow, Arm2") + p.add_argument("--from-state", type=Path, default=None, + help="JSON from a previous 4b run (carries accumulated state)") + + # Manual joint-value overrides (for use without --from-state) + for k in STATE_KEYS: + p.add_argument(f"--{k}-mm" if k in ("x", "e") else f"--{k}-deg", + type=float, default=None, + help=f"Known value for joint '{k}'" + + (" (mm)" if k in ("x", "e") else " (deg)")) + + p.add_argument("--min-baseline", type=float, default=15.0, + help="Skip pairs with perpendicular baseline < this (mm)") + p.add_argument("--output", type=Path, default=None, + help="Save result JSON (readable by next 4b as --from-state)") + args = p.parse_args() + + # Assemble known state + if args.from_state: + known_state = _load_state(args.from_state) + print(f" Loaded state from {args.from_state}: {known_state}") + else: + known_state = {} + + # CLI overrides + for k in STATE_KEYS: + attr = f"{k}_mm" if k in ("x", "e") else f"{k}_deg" + v = getattr(args, attr, None) + if v is not None: + known_state[k] = float(v) + + fk = RobotFK.from_file(args.robot) + aruco_json = _load_json(args.aruco) + observed_mm = _load_observed_mm(aruco_json) + + result = estimate_revolute_angle( + fk = fk, + observed_mm = observed_mm, + link_name = args.link, + known_state = known_state, + min_baseline_mm = args.min_baseline, + verbose = True, + ) + + if args.output and result["status"] == "ok": + args.output.parent.mkdir(parents=True, exist_ok=True) + with args.output.open("w", encoding="utf-8") as f: + json.dump(result, f, indent=2) + print(f"\n Saved → {args.output}") + + return 0 if result["status"] == "ok" else 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/5_camera_z_refine.py b/scripts/5_camera_z_refine.py new file mode 100644 index 0000000..1dfe6a7 --- /dev/null +++ b/scripts/5_camera_z_refine.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python3 +""" +5_camera_z_refine.py +----------------------- +Refines camera z-positions using the FK-predicted z of the Ellbow link. + +Background +---------- +Camera poses from step 2 are estimated from board markers alone. Board +markers are roughly coplanar (z ≈ 0), so the camera's z-height above the +board is poorly constrained and can be 20–40 % off. + +Once we have the elbow angle (step 3b, from 4b_revolute_angle.py), +FK predicts where the Ellbow markers *should* be in world space. The +difference between FK-predicted z and triangulated z gives a global +z-offset for the cameras. + +Algorithm +--------- +1. Load elbow-angle result (v8_ellbow_angle.json) → joint state dict +2. Run FK with that state → predicted world positions for Ellbow markers (mm) +3. Load triangulated Ellbow marker positions from aruco_positions_optimized.json +4. delta_z_mm = median(fk_z - triangulated_z) over matched markers +5. For each camera pose file: + a. camera center world C = -R_wc.T @ t_wc + b. C_new = C + [0, 0, delta_z_mm / 1000] + c. t_wc_new = -R_wc @ C_new + d. Save as *_camera_pose_v8.json +6. Save v8_z_correction.json summary + +Usage +----- +python 5_camera_z_refine.py \\ + --angle path/to/v8_ellbow_angle.json \\ + --robot path/to/robot.json \\ + --aruco path/to/aruco_positions_optimized.json \\ + -pose path/to/render_a_camera_pose.json \\ + -pose path/to/render_b_camera_pose.json \\ + --outDir path/to/output_dir \\ + [--elbowLink Ellbow] +""" + +from __future__ import annotations + +import argparse +import copy +import json +import os +import sys +import time +from pathlib import Path +from typing import Dict, List, Optional + +import numpy as np + +# robot_fk lives in the same directory as this script +sys.path.insert(0, str(Path(__file__).parent)) +from robot_fk import RobotFK + + +# ────────────────────────────────────────────────────────────── +# I/O helpers +# ────────────────────────────────────────────────────────────── + +def load_json(path: str) -> dict: + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + +def save_json(path: str, data: dict) -> None: + os.makedirs(os.path.dirname(path) or ".", exist_ok=True) + with open(path, "w", encoding="utf-8") as f: + json.dump(data, f, indent=2) + + +def load_triangulated_mm(aruco_json: dict) -> Dict[int, np.ndarray]: + """Read marker world positions (mm) from aruco_positions_*.json.""" + result: Dict[int, np.ndarray] = {} + for m in aruco_json.get("markers", []): + mid = int(m.get("marker_id", m.get("id", -1))) + if mid < 0: + continue + if "position_mm" in m: + result[mid] = np.array(m["position_mm"], dtype=float) + elif "position_m" in m: + result[mid] = np.array(m["position_m"], dtype=float) * 1000.0 + return result + + +# ────────────────────────────────────────────────────────────── +# Main +# ────────────────────────────────────────────────────────────── + +def main() -> None: + parser = argparse.ArgumentParser( + description="5v8: refine camera z-positions using FK-predicted Ellbow marker z" + ) + parser.add_argument("--angle", required=True, + help="v8_ellbow_angle.json from 4b_revolute_angle.py") + parser.add_argument("--robot", required=True, help="robot.json") + parser.add_argument("--aruco", required=True, + help="aruco_positions_optimized.json (first-pass triangulation)") + parser.add_argument("-pose", "--poses", action="append", required=True, + help="*_camera_pose.json files (repeat for each camera)") + parser.add_argument("--outDir", required=True, help="Output directory") + parser.add_argument("--elbowLink", default="Ellbow", + help="Link name used for z estimation (default: Ellbow)") + args = parser.parse_args() + + os.makedirs(args.outDir, exist_ok=True) + + # ── 1. Load angle / state ──────────────────────────────── + angle_data = load_json(args.angle) + if angle_data.get("status") != "ok": + print(f"[ERROR] Angle estimation was not ok: {angle_data.get('reason', '?')}") + sys.exit(1) + + accumulated_state = angle_data.get("accumulated_state", {}) + elbow_link = angle_data.get("link", args.elbowLink) + + print(f"[INFO] Joint state from 4b: {accumulated_state}") + print(f"[INFO] Link used for z-ref: {elbow_link}") + + # ── 2. FK prediction ───────────────────────────────────── + fk = RobotFK.from_file(args.robot) + T = fk.compute(accumulated_state) + all_fk = fk.all_markers_world(T) # marker_id -> {world_mm, link, …} + + elbow_fk = {mid: v for mid, v in all_fk.items() if v["link"] == elbow_link} + + if not elbow_fk: + print(f"[ERROR] No FK markers found for link '{elbow_link}'") + sys.exit(1) + + print(f"[INFO] FK markers for '{elbow_link}': {sorted(elbow_fk.keys())}") + + # ── 3. Load triangulated positions ─────────────────────── + aruco_data = load_json(args.aruco) + triangulated = load_triangulated_mm(aruco_data) + + # ── 4. Compute z-deltas ────────────────────────────────── + z_deltas: List[float] = [] + print("\n[INFO] Per-marker z comparison (FK vs triangulated):") + for mid in sorted(elbow_fk.keys()): + fk_z = float(elbow_fk[mid]["world_mm"][2]) + if mid not in triangulated: + print(f" Marker {mid:4d}: FK z={fk_z:8.1f} mm [NOT triangulated – skip]") + continue + obs_z = float(triangulated[mid][2]) + delta = fk_z - obs_z + z_deltas.append(delta) + print(f" Marker {mid:4d}: FK z={fk_z:8.1f} mm obs z={obs_z:8.1f} mm Δz={delta:+7.1f} mm") + + if not z_deltas: + print(f"\n[ERROR] No matched markers for '{elbow_link}' — cannot compute z correction.") + sys.exit(1) + + delta_z_mm = float(np.median(z_deltas)) + print(f"\n[INFO] z-correction (median): {delta_z_mm:+.1f} mm " + f"(from {len(z_deltas)} markers)") + if abs(delta_z_mm) < 1.0: + print("[INFO] Correction < 1 mm — cameras already well-calibrated in z.") + + # ── 5. Apply correction to each camera pose ────────────── + corrected_files: List[str] = [] + + for pose_file in args.poses: + pose_data = load_json(pose_file) + + pose_section = pose_data.get("camera_pose", {}) or {} + w2c = pose_section.get("world_to_camera", {}) or {} + + R_wc = np.array(w2c.get("rotation_matrix", []), dtype=float).reshape(3, 3) + t_wc = np.array(w2c.get("translation_m", []), dtype=float).reshape(3) + + # Camera centre in world (metres) + C_world = -R_wc.T @ t_wc + C_world_new = C_world + np.array([0.0, 0.0, delta_z_mm / 1000.0]) + + t_wc_new = -R_wc @ C_world_new + + pose_new = copy.deepcopy(pose_data) + pose_new["camera_pose"]["world_to_camera"]["translation_m"] = \ + [float(v) for v in t_wc_new.tolist()] + pose_new["camera_pose"]["camera_in_world"]["position_m"] = \ + [float(v) for v in C_world_new.tolist()] + pose_new["camera_pose"]["camera_in_world"]["position_mm"] = \ + [float(v * 1000.0) for v in C_world_new.tolist()] + pose_new["v8_z_correction_mm"] = delta_z_mm + pose_new["created_utc"] = time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()) + + basename = os.path.basename(pose_file) + out_name = basename.replace("_camera_pose.json", "_camera_pose_v8.json") + if out_name == basename: + out_name = os.path.splitext(basename)[0] + "_v8.json" + + out_path = os.path.join(args.outDir, out_name) + save_json(out_path, pose_new) + print(f"[INFO] Saved: {out_path}") + corrected_files.append(out_path) + + # ── 6. Save correction summary ─────────────────────────── + summary = { + "schema_version": "1.0", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "elbow_link": elbow_link, + "z_correction_mm": delta_z_mm, + "n_markers_used": len(z_deltas), + "z_deltas_mm": z_deltas, + "corrected_pose_files": corrected_files, + } + summary_path = os.path.join(args.outDir, "v8_z_correction.json") + save_json(summary_path, summary) + print(f"\n[INFO] Correction summary → {summary_path}") + + +if __name__ == "__main__": + main() diff --git a/scripts/9_evaluateMarker.py b/scripts/9_evaluateMarker.py new file mode 100644 index 0000000..6e1c655 --- /dev/null +++ b/scripts/9_evaluateMarker.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +""" +9_evaluateMarker.py +=================== +Compare reconstructed markers against ground-truth (render_*.json). + +Reports, per link and overall: + * 3D position error (mm) + * orientation error (deg) between measured and GT normal — only meaningful + when the detected file carries a MEASURED normal (aruco_marker_poses.json). + +Backwards compatible: the original positional CLI + python 9_evaluateMarker.py detected.json original.json +still works and prints the familiar summary; --out adds a JSON report. +""" +from __future__ import annotations + +import argparse +import json +import math +from collections import defaultdict +from typing import Any, Dict, List, Optional + + +def dist3(a, b) -> float: + return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b))) + + +def angle_deg(a, b) -> Optional[float]: + na = math.sqrt(sum(x * x for x in a)) + nb = math.sqrt(sum(x * x for x in b)) + if na < 1e-9 or nb < 1e-9: + return None + c = sum(x * y for x, y in zip(a, b)) / (na * nb) + c = max(-1.0, min(1.0, c)) + ang = math.degrees(math.acos(c)) + return min(ang, 180.0 - ang) # flip-invariant + + +def _pct(values: List[float], q: float) -> float: + if not values: + return 0.0 + s = sorted(values) + idx = max(0, min(len(s) - 1, int(q * len(s)) - 1)) + return s[idx] + + +def analyze(detected_file: str, original_file: str) -> Dict[str, Any]: + detected = json.load(open(detected_file, "r", encoding="utf-8")) + original = json.load(open(original_file, "r", encoding="utf-8")) + + det_markers = detected.get("markers", detected if isinstance(detected, list) else []) + det_by_id = {int(m.get("marker_id", m.get("id", -1))): m for m in det_markers} + orig_by_id = {int(m["id"]): m for m in original} + + det_ids = set(det_by_id) - {-1} + orig_ids = set(orig_by_id) + recognized = det_ids & orig_ids + missing = orig_ids - det_ids + + per_link_pos: Dict[str, List[float]] = defaultdict(list) + per_link_ang: Dict[str, List[float]] = defaultdict(list) + rows = [] + for mid in sorted(recognized): + o = orig_by_id[mid] + d = det_by_id[mid] + link = o.get("link", d.get("link", "?")) + pos_err = dist3(o["position_m"], d["position_m"]) * 1000.0 # mm + per_link_pos[link].append(pos_err) + ang_err = None + if o.get("normal") is not None and d.get("normal") is not None: + ang_err = angle_deg(o["normal"], d["normal"]) + if ang_err is not None: + per_link_ang[link].append(ang_err) + rows.append({"marker_id": mid, "link": link, "pos_err_mm": pos_err, "normal_err_deg": ang_err}) + + all_pos = [r["pos_err_mm"] for r in rows] + all_ang = [r["normal_err_deg"] for r in rows if r["normal_err_deg"] is not None] + + return { + "n_original": len(orig_ids), + "n_recognized": len(recognized), + "n_missing": len(missing), + "recognition_rate": (len(recognized) / len(orig_ids) * 100.0) if orig_ids else 0.0, + "per_link": { + ln: { + "n": len(per_link_pos[ln]), + "pos_mean_mm": sum(per_link_pos[ln]) / len(per_link_pos[ln]), + "pos_max_mm": max(per_link_pos[ln]), + "normal_mean_deg": (sum(per_link_ang[ln]) / len(per_link_ang[ln])) if per_link_ang.get(ln) else None, + "normal_max_deg": max(per_link_ang[ln]) if per_link_ang.get(ln) else None, + } for ln in sorted(per_link_pos, key=lambda k: -len(per_link_pos[k])) + }, + "overall": { + "pos_mean_mm": (sum(all_pos) / len(all_pos)) if all_pos else 0.0, + "pos_p90_mm": _pct(all_pos, 0.9), + "pos_max_mm": max(all_pos) if all_pos else 0.0, + "normal_mean_deg": (sum(all_ang) / len(all_ang)) if all_ang else None, + "normal_p90_deg": _pct(all_ang, 0.9) if all_ang else None, + "normal_max_deg": max(all_ang) if all_ang else None, + }, + "rows": rows, + } + + +def print_report(r: Dict[str, Any]) -> None: + # familiar summary (backwards compatible wording) + print(f"Erkannte Marker: {r['n_recognized']}") + print(f"Nicht erkannte Marker: {r['n_missing']}") + print(f"Gesamtzahl der Original-Marker: {r['n_original']}") + print(f"Erkennungsrate: {r['recognition_rate']:.2f}%") + o = r["overall"] + print(f"Gemittelter 3D-Abstand: {o['pos_mean_mm']/1000.0:.4f}m") + print(f"90%-Radius: {o['pos_p90_mm']/1000.0:.4f}m") + print(f"Schlechtester Abstand: {o['pos_max_mm']/1000.0:.4f}m") + if o["normal_mean_deg"] is not None: + print(f"Normalen-Fehler: mean {o['normal_mean_deg']:.2f}deg / p90 {o['normal_p90_deg']:.2f}deg / max {o['normal_max_deg']:.2f}deg") + + print("\nPro Glied:") + print(f" {'link':>10} | {'n':>3} | {'pos mean/max [mm]':>18} | {'normal mean/max [deg]':>21}") + print(" " + "-" * 60) + for ln, s in r["per_link"].items(): + nd = f"{s['normal_mean_deg']:6.2f} /{s['normal_max_deg']:6.2f}" if s["normal_mean_deg"] is not None else " - " + print(f" {ln:>10} | {s['n']:>3} | {s['pos_mean_mm']:7.2f} /{s['pos_max_mm']:7.2f} | {nd:>21}") + + +def main() -> None: + ap = argparse.ArgumentParser(description="Analysiert die Markererkennung (Position + Orientierung).") + ap.add_argument("detected_file", help="aruco_marker_poses.json oder aruco_positions_*.json") + ap.add_argument("original_file", help="Ground-Truth render_*.json") + ap.add_argument("--out", default=None, help="optional JSON-Report") + args = ap.parse_args() + + r = analyze(args.detected_file, args.original_file) + if r["n_recognized"] == 0: + print("Keine gemeinsamen Marker gefunden, um die Genauigkeit zu bewerten.") + return + print_report(r) + if args.out: + json.dump(r, open(args.out, "w", encoding="utf-8"), indent=2) + print(f"\n[INFO] wrote {args.out}") + + +if __name__ == "__main__": + main()