import math from pathlib import Path import xml.etree.ElementTree as ET import numpy as np import pytest from scipy.spatial.transform import Rotation from linkerhand_calibration.extrinsics import ( camera_info_fingerprint, dump_three_camera_extrinsics, load_three_camera_extrinsics, ) from linkerhand_calibration.urdf_zero import ( AXIS_JOINTS, DIRECT_ZERO_JOINTS, INHERITED_ZERO_JOINTS, JointAxisMeasurement, PalmOrientationMeasurement, UrdfKinematicModel, _angles_from_state, _zero_sensitive_axis_error_rad, anchor_right_19_mechanical_endpoint_curves, baseline_hysteresis_by_cycle_rad, derive_right_19_mechanical_endpoint_offsets, fit_joint_axis_measurement, fit_partial_palm_orientation_measurement, fit_partial_palm_orientation_measurements, fit_rotation_joint_curve, select_cross_view_roll_direction_source, solve_urdf_zero_offsets, get_zero_calibration_profile, get_right_19_thumb_zero_profile, expand_right_19_thumb_zero_result_with_cad_fingers, merge_right_19_thumb_zero_result, write_zero_corrected_urdf, ) from linkerhand_calibration.full_hand import ( ACTIVE_JOINTS, JOINT_SPECS, MEASURED_JOINTS, PASSIVE_JOINTS, RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, JointCurveFit, get_hand_calibration_profile, ) from linkerhand_calibration.models.g20.urdf_input import ( build_g20_urdf_input_payload, load_g20_urdf_input, ) from linkerhand_calibration.storage import atomic_write_json PACKAGE_ROOT = Path(__file__).resolve().parents[1] SOURCE_URDF = PACKAGE_ROOT / ( "urdf/g20_left/linkerhand_g20_left.urdf" ) RIGHT_SOURCE_URDF = PACKAGE_ROOT / ( "urdf/g20_right/linkerhand_g20_right.urdf" ) def test_cross_view_roll_direction_is_selected_once_by_group_consensus() -> None: # Session 20260824_171302: the side residual in cycle 3 was 5.037 deg, # just outside the 5 deg cone gate, while the other three side cycles # passed and every front cycle was near 8 deg. Per-cycle selection mixed # three side axes with one front axis and fabricated a 3.24 deg spread. source = select_cross_view_roll_direction_source( [math.radians(value) for value in (8.0078, 8.1710, 8.0712, 8.0236)], [math.radians(value) for value in (4.9043, 4.9136, 5.0374, 4.9389)], math.radians(5.0), ) assert source == "secondary" def test_cross_view_roll_direction_keeps_primary_without_consensus() -> None: source = select_cross_view_roll_direction_source( [math.radians(value) for value in (5.2, 4.8, 5.1, 4.9)], [math.radians(value) for value in (4.7, 5.3, 4.8, 5.2)], math.radians(5.0), ) assert source == "primary" def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch(): parent = np.asarray([0.0, 0.0, 1.0]) predicted = np.asarray([1.0, 0.0, 0.0]) cone_mismatch = np.asarray( [math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))] ) zero_mismatch = np.asarray( [math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0] ) assert _zero_sensitive_axis_error_rad( predicted, cone_mismatch, parent ) == pytest.approx(0.0, abs=1.0e-12) assert math.degrees( _zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent) ) == pytest.approx(3.0, abs=1.0e-9) def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None: parent = np.asarray([0.0, 0.0, 1.0]) cone = math.radians(32.0) phase = math.radians(7.0) predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)]) observed = Rotation.from_rotvec(parent * phase).apply(predicted) error = _zero_sensitive_axis_error_rad(predicted, observed, parent) assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9) def _payload(transform: np.ndarray) -> dict[str, list[float]]: return { "translation_xyz_m": transform[:3, 3].tolist(), "quaternion_xyzw": Rotation.from_matrix( transform[:3, :3] ).as_quat().tolist(), } def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]: axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float) axis_parent /= np.linalg.norm(axis_parent) centre_parent = np.asarray([0.012, -0.008, 0.021]) radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2])) radial = 0.035 * radial / np.linalg.norm(radial) child_tag_mount = Rotation.from_euler( "xyz", [1.1, -0.7, 0.45] ) common_from_parent = np.eye(4) common_from_parent[:3, :3] = Rotation.from_euler( "xyz", [-0.8, 0.55, 1.3] ).as_matrix() common_from_parent[:3, 3] = [0.41, -0.12, 0.73] expected_axis = common_from_parent[:3, :3] @ axis_parent expected_point = ( common_from_parent[:3, :3] @ centre_parent + common_from_parent[:3, 3] ) commands = list(range(0, 256, 16)) + [255] records = [] for cycle in range(3): for direction in ("decreasing", "increasing"): for command in commands: angle = math.radians(62.0) * (255.0 - command) / 255.0 motion = Rotation.from_rotvec(axis_parent * angle) relative_rotation = motion * child_tag_mount relative_translation = centre_parent + motion.apply(radial) child_common = common_from_parent.copy() child_common[:3, :3] = ( common_from_parent[:3, :3] @ relative_rotation.as_matrix() ) child_common[:3, 3] = ( common_from_parent[:3, :3] @ relative_translation + common_from_parent[:3, 3] ) state = [255.0] * 20 state[5] = float(command) records.append( { "cycle": cycle, "direction": direction, "command_u8": command, "relative_translation_xyz_m": relative_translation.tolist(), "relative_quaternion_xyzw": relative_rotation.as_quat().tolist(), "parent_pose_common": _payload(common_from_parent), "child_pose_common": _payload(child_common), "state_u8": state, } ) return records, expected_axis, expected_point def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None: records, expected_axis, expected_point = _arbitrary_tag_records() curve = fit_rotation_joint_curve(records, zero_command_u8=255) measurement = fit_joint_axis_measurement( "thumb_cmc_roll", records, cycle=0, zero_command_u8=255 ) observed_axis = np.asarray(measurement.axis_common_xyz) observed_point = np.asarray(measurement.point_common_xyz_m) assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05)) assert np.linalg.norm( np.cross(observed_point - expected_point, expected_axis) ) < 1.0e-6 assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9) assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6) def test_baseline_hysteresis_uses_only_revolute_axis_component() -> None: off_axis_noise = Rotation.from_rotvec( np.radians([1.2, 0.0, 0.2]) ).as_quat().tolist() records = [] for cycle in range(3): records.extend( ( { "cycle": cycle, "direction": "decreasing", "command_u8": 255, "relative_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], }, { "cycle": cycle, "direction": "increasing", "command_u8": 255, "relative_quaternion_xyzw": off_axis_noise, }, ) ) full_pose = baseline_hysteresis_by_cycle_rad( records, zero_command_u8=255 ) joint_angle = baseline_hysteresis_by_cycle_rad( records, zero_command_u8=255, axis_xyz=[0.0, 0.0, 1.0] ) assert math.degrees(full_pose[0]) == pytest.approx( math.hypot(1.2, 0.2), abs=1.0e-9 ) assert math.degrees(joint_angle[0]) == pytest.approx(0.2, abs=1.0e-9) def test_canonical_zero_preserves_opposite_direction_baseline_offset() -> None: records = [] commands = sorted({0, 127, 255, *range(0, 256, 16)}) branch_offset = math.radians(1.0) for cycle in range(3): for direction in ("decreasing", "increasing"): offset = branch_offset if direction == "increasing" else 0.0 for command in commands: angle = math.radians(50.0) * (127.0 - command) / 255.0 angle += offset rotation = Rotation.from_rotvec([0.0, 0.0, angle]) records.append( { "cycle": cycle, "direction": direction, "command_u8": command, "relative_quaternion_xyzw": rotation.as_quat().tolist(), } ) curve = fit_rotation_joint_curve( records, zero_command_u8=127, canonical_zero_direction="decreasing", ) assert curve.angle_rad == curve.decreasing_rad assert curve.decreasing_rad[127] == pytest.approx(0.0, abs=1.0e-9) assert curve.increasing_rad[127] == pytest.approx( branch_offset, abs=1.0e-6 ) assert curve.maximum_hysteresis_rad == pytest.approx( branch_offset, abs=1.0e-6 ) @pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"]) def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth( joint: str, ) -> None: records, expected_axis, expected_point = _arbitrary_tag_records() parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) axis_parent = parent_rotation.inv().apply(expected_axis) tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7])) tangent /= np.linalg.norm(tangent) # Reproduce monocular planar-PnP depth bias: the centre trajectory remains # precise in its dominant directions but receives a command-correlated # component that makes a free 3-D plane normal substantially wrong. biased_records = [] for record in records: biased = dict(record) point = np.asarray(record["relative_translation_xyz_m"], dtype=float) depth_bias = 0.30 * float(point @ tangent) biased["relative_translation_xyz_m"] = ( point + depth_bias * axis_parent ).tolist() biased_records.append(biased) measurement = fit_joint_axis_measurement( joint, biased_records, cycle=0, zero_command_u8=255 ) observed_axis = np.asarray(measurement.axis_common_xyz) observed_point = np.asarray(measurement.point_common_xyz_m) assert abs(float(observed_axis @ expected_axis)) > math.cos( math.radians(0.05) ) assert np.linalg.norm( np.cross(observed_point - expected_point, expected_axis) ) < 0.003 assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0) assert measurement.plane_rms_m < 0.003 assert measurement.radial_rms_m < 0.003 def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None: records, expected_axis, expected_point = _arbitrary_tag_records() common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) axis_parent = common_from_parent.inv().apply(expected_axis) # Exact end-on depth is a gauge along the physical axis and therefore # cannot alter the observable axis line. An oblique camera has a small # irreducible coupling between monocular depth and radial position; that # case must be bounded by the residual/holdout gates, not asserted to be # exactly recoverable from one view. view_normal_parent = axis_parent view_normal_common = common_from_parent.apply(view_normal_parent) biased = [] for record in records: changed = dict(record) fraction = (255.0 - float(record["command_u8"])) / 255.0 depth_bias = 0.03 * (fraction - 0.5) changed["relative_translation_xyz_m"] = ( np.asarray(record["relative_translation_xyz_m"], dtype=float) + depth_bias * view_normal_parent ).tolist() biased.append(changed) measurement = fit_joint_axis_measurement( "thumb_cmc_pitch", biased, cycle=0, zero_command_u8=255, view_normal_common_xyz=view_normal_common, ) observed_point = np.asarray(measurement.point_common_xyz_m) assert measurement.axis_point_source == "pose_trajectory_image_plane" assert measurement.pose_axis_line_rms_m < 1.0e-6 assert np.linalg.norm( np.cross(observed_point - expected_point, expected_axis) ) < 1.0e-6 def test_side_roll_axis_point_ignores_real_screw_translation() -> None: records, expected_axis, expected_point = _arbitrary_tag_records() common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) axis_parent = common_from_parent.inv().apply(expected_axis) screw_records = [] for record in records: changed = dict(record) fraction = (255.0 - float(record["command_u8"])) / 255.0 changed["relative_translation_xyz_m"] = ( np.asarray(record["relative_translation_xyz_m"], dtype=float) + 0.008 * fraction * axis_parent ).tolist() screw_records.append(changed) measurement = fit_joint_axis_measurement( "middle_mcp_roll_side", screw_records, cycle=0, zero_command_u8=255, constrained_circle_joints=frozenset({"middle_mcp_roll_side"}), ) observed_point = np.asarray(measurement.point_common_xyz_m) assert measurement.pose_axis_line_rms_m < 1.0e-6 assert np.linalg.norm( np.cross(observed_point - expected_point, expected_axis) ) < 1.0e-6 def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None: records, expected_axis, _ = _arbitrary_tag_records() assert not any(record["command_u8"] == 127 for record in records) curve = fit_rotation_joint_curve(records, zero_command_u8=127) measurement = fit_joint_axis_measurement( "index_mcp_roll", records, cycle=0, zero_command_u8=127 ) observed_axis = np.asarray(measurement.axis_common_xyz) assert abs(float(observed_axis @ expected_axis)) > math.cos( math.radians(0.05) ) assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9) def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None: records, expected_axis, expected_point = _arbitrary_tag_records() common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) physical_axis_parent = common_from_parent.inv().apply(expected_axis) wrong_axis_parent = np.cross( physical_axis_parent, np.asarray([0.2, 0.8, -0.1]) ) wrong_axis_parent /= np.linalg.norm(wrong_axis_parent) mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"]) contradictory = [] for record in records: changed = dict(record) angle = math.radians(62.0) * ( 255.0 - float(record["command_u8"]) ) / 255.0 changed["relative_quaternion_xyzw"] = ( Rotation.from_rotvec(wrong_axis_parent * angle) * mount ).as_quat().tolist() contradictory.append(changed) measurement = fit_joint_axis_measurement( "index_dip", contradictory, cycle=0, zero_command_u8=255, axis_common_constraint=expected_axis, ) observed_axis = np.asarray(measurement.axis_common_xyz) observed_point = np.asarray(measurement.point_common_xyz_m) assert abs(float(observed_axis @ expected_axis)) > math.cos( math.radians(0.05) ) assert np.linalg.norm( np.cross(observed_point - expected_point, expected_axis) ) < 1.0e-6 def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None: cameras = { view: { "serial_number": f"SERIAL_{view}", "width": 1624, "height": 1240, "intrinsics_sha256": camera_info_fingerprint( width=1624, height=1240, camera_matrix=np.asarray( [[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]] ), ), } for view in ("front", "side", "top") } transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)} transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix() transforms["side"][:3, 3] = [0.2, 0.0, 0.1] transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix() transforms["top"][:3, 3] = [-0.1, 0.3, 0.2] destination = tmp_path / "extrinsics.yaml" dump_three_camera_extrinsics( destination, cameras=cameras, front_from_view=transforms, quality={ "passed": True, "reprojection_rms_px": 0.3, "maximum_rotation_repeatability_deg": 0.2, "maximum_translation_repeatability_m": 0.001, "front_side_captures": 15, "front_top_captures": 15, }, ) loaded = load_three_camera_extrinsics(destination) assert loaded.cameras["front"].serial_number == "SERIAL_front" assert np.allclose(loaded.transform("side"), transforms["side"]) assert np.allclose(loaded.transform("top"), transforms["top"]) assert loaded.camera_matches( "front", serial_number="SERIAL_front", width=1624, height=1240, intrinsics_sha256=cameras["front"]["intrinsics_sha256"], ) assert not loaded.camera_matches( "front", serial_number="WRONG_SERIAL", width=1624, height=1240, intrinsics_sha256=cameras["front"]["intrinsics_sha256"], ) relaxed_destination = tmp_path / "relaxed_extrinsics.yaml" relaxed_quality_limits = { "reprojection_rms_px": 1.5, "maximum_rotation_repeatability_deg": 0.3, "maximum_translation_repeatability_m": 0.0015, } dump_three_camera_extrinsics( relaxed_destination, cameras=cameras, front_from_view=transforms, quality={ "passed": True, "reprojection_rms_px": 1.4, "maximum_rotation_repeatability_deg": 0.2, "maximum_translation_repeatability_m": 0.001, "front_side_captures": 15, "front_top_captures": 15, }, quality_limits=relaxed_quality_limits, ) with pytest.raises(ValueError, match="exceeds 1.2"): load_three_camera_extrinsics(relaxed_destination) relaxed = load_three_camera_extrinsics( relaxed_destination, quality_limits=relaxed_quality_limits, ) assert relaxed.quality["reprojection_rms_px"] == pytest.approx(1.4) def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]: joint = next( element for element in ET.parse(path).getroot().findall("joint") if element.get("name") == name ) origin = joint.find("origin") axis = joint.find("axis") xyz = np.asarray([float(value) for value in origin.get("xyz").split()]) rpy = np.asarray([float(value) for value in origin.get("rpy").split()]) axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()]) return np.block( [ [Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]], [np.asarray([[0.0, 0.0, 0.0, 1.0]])], ] ), axis_xyz / np.linalg.norm(axis_xyz) def _joint_limit(path: Path, name: str) -> tuple[float, float]: joint = next( element for element in ET.parse(path).getroot().findall("joint") if element.get("name") == name ) limit = joint.find("limit") return float(limit.get("lower")), float(limit.get("upper")) def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None: offset = math.radians(7.3) destination = write_zero_corrected_urdf( source_urdf=SOURCE_URDF, output_directory=tmp_path, serial_number="G20_LEFT_001", offsets_rad={"thumb_cmc_yaw": offset}, timestamp="20260806_120000", ) original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw") corrected, _ = _joint_origin(destination, "thumb_cmc_yaw") expected = original.copy() expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec( axis * offset ).as_matrix() assert destination != SOURCE_URDF assert np.allclose(corrected, expected, atol=1.0e-12) with pytest.raises(ValueError, match="refusing to overwrite"): write_zero_corrected_urdf( source_urdf=SOURCE_URDF, output_directory=tmp_path, serial_number="G20_LEFT_001", offsets_rad={"thumb_cmc_yaw": offset}, timestamp="20260806_120000", ) with pytest.raises(ValueError, match="original CAD URDF"): write_zero_corrected_urdf( source_urdf=destination, output_directory=tmp_path, serial_number="G20_LEFT_001", offsets_rad={"thumb_cmc_yaw": offset}, timestamp="20260806_120001", ) with pytest.raises(ValueError, match="finite and within"): write_zero_corrected_urdf( source_urdf=SOURCE_URDF, output_directory=tmp_path, serial_number="G20_LEFT_001", offsets_rad={"thumb_cmc_yaw": math.nan}, timestamp="20260806_120002", ) def test_g20_json_handoff_preserves_existing_urdf_bytes(tmp_path: Path) -> None: offsets = {"thumb_cmc_yaw": math.radians(7.3)} direct = write_zero_corrected_urdf( source_urdf=SOURCE_URDF, output_directory=tmp_path / "direct", serial_number="G20_LEFT_JSON_HANDOFF", offsets_rad=offsets, timestamp="20260902_140001", ) input_path = tmp_path / "g20_urdf_correction_input.json" atomic_write_json( input_path, build_g20_urdf_input_payload( side="left", layout_id="legacy_11", serial_number="G20_LEFT_JSON_HANDOFF", source_urdf=SOURCE_URDF, offsets_rad=offsets, endpoint_anchored_offsets_rad={}, ), ) loaded_offsets, loaded_endpoints = load_g20_urdf_input( input_path, source_urdf=SOURCE_URDF, side="left", layout_id="legacy_11", serial_number="G20_LEFT_JSON_HANDOFF", ) via_json = write_zero_corrected_urdf( source_urdf=SOURCE_URDF, output_directory=tmp_path / "via_json", serial_number="G20_LEFT_JSON_HANDOFF", offsets_rad=loaded_offsets, endpoint_anchored_offsets_rad=loaded_endpoints, timestamp="20260902_140001", ) assert direct.read_bytes() == via_json.read_bytes() def test_urdf_writer_changes_only_the_16_active_zero_origins( tmp_path: Path, ) -> None: before = SOURCE_URDF.read_bytes() offsets = { name: math.radians(0.25 * (index + 1)) for index, name in enumerate(ACTIVE_JOINTS) } destination = write_zero_corrected_urdf( source_urdf=SOURCE_URDF, output_directory=tmp_path, serial_number="G20_LEFT_001", offsets_rad=offsets, timestamp="20260807_180000", ) assert len(offsets) == 16 assert SOURCE_URDF.read_bytes() == before for name in ACTIVE_JOINTS: original, axis = _joint_origin(SOURCE_URDF, name) corrected, corrected_axis = _joint_origin(destination, name) expected = original.copy() expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec( axis * offsets[name] ).as_matrix() assert np.allclose(corrected, expected, atol=1.0e-12) assert np.allclose(corrected_axis, axis, atol=1.0e-12) for name in PASSIVE_JOINTS: original, axis = _joint_origin(SOURCE_URDF, name) corrected, corrected_axis = _joint_origin(destination, name) assert np.allclose(corrected, original, atol=1.0e-12) assert np.allclose(corrected_axis, axis, atol=1.0e-12) # A zero calibration must not silently expand mechanical/CAD safety # limits. Dynamic measured ranges remain in the calibration JSON. for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS): assert _joint_limit(destination, name) == pytest.approx( _joint_limit(SOURCE_URDF, name) ) def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit: values = np.asarray( [travel * (255.0 - command) / 255.0 for command in range(256)] ) values -= values[zero_command] data = tuple(float(value) for value in values) return JointCurveFit( angle_rad=data, decreasing_rad=data, increasing_rad=data, circle={}, maximum_monotonic_correction_rad=0.0, maximum_hysteresis_rad=0.0, quality={}, ) def _solve_synthetic_offsets( side: str, offset_degrees: list[float], *, layout_id: str = "legacy_11", inject_oblique_optical_depth_bias: bool = False, inject_secondary_root_axis_bias_degrees: float = 0.0, inject_secondary_root_point_bias_m: float = 0.0, inject_observer_cone_bias_degrees: float = 0.0, palm_orientation_frame_bias_degrees: float = 0.0, maximum_systematic_axis_cone_bias_degrees: float | None = None, pose_axis_line_rms_by_joint_m: dict[str, float] | None = None, joint_maximum_offset_degrees: dict[str, float] | None = None, validation_offset_bias_degrees: dict[str, float] | None = None, static_output_offsets_degrees: dict[str, float] | None = None, fixed_direct_offsets_degrees: dict[str, float] | None = None, base_euler_xyz_rad: tuple[float, float, float] = (0.5, -0.4, 0.8), base_translation_xyz_m: tuple[float, float, float] = (0.31, -0.19, 0.72), solve_thumb_only: bool = False, ): hand = get_hand_calibration_profile(side, layout_id) zero = get_zero_calibration_profile(side, layout_id) source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF baseline = [255.0] * 20 baseline[6:10] = [127.0] * 4 curves = { name: _synthetic_curve( int(baseline[hand.joint_specs[name].motor_index]), math.radians(50.0), ) for name in hand.measured_joints } if inject_secondary_root_axis_bias_degrees: # Make the thumb root the higher-travel, directly observed direction, # matching the real right-hand data where the short pinky splay arc is # the less reliable root-axis orientation estimate. curves["thumb_cmc_roll"] = _synthetic_curve( int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]), math.radians(70.0), ) motor_by_joint = { name: spec.motor_index for name, spec in hand.joint_specs.items() } offsets = { name: math.radians(value) for name, value in zip(zero.direct_zero_joints, offset_degrees) } model = UrdfKinematicModel(source) base_rotation = Rotation.from_euler("xyz", base_euler_xyz_rad) base_translation = np.asarray(base_translation_xyz_m) measurements: list[JointAxisMeasurement] = [] palm_orientation_measurements: list[PalmOrientationMeasurement] = [] validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2 training_cycles = tuple(range(validation_cycle)) for cycle in range(validation_cycle + 1): cycle_offsets = dict(offsets) if cycle == validation_cycle: cycle_offsets.update( { name: cycle_offsets[name] + math.radians(value) for name, value in ( validation_offset_bias_degrees or {} ).items() } ) for joint in zero.axis_joints: state = list(baseline) if joint == "thumb_cmc_yaw": state[5] = 145.0 angles = _angles_from_state( state, curves=curves, motor_by_joint=motor_by_joint, inherited_zero_joints=zero.inherited_zero_joints, ) axis, point = model.axis_line( joint, zero_offsets=cycle_offsets, joint_angles=angles ) point_common = base_rotation.apply(point) + base_translation if ( inject_secondary_root_point_bias_m and joint == f"{zero.reference_finger}_mcp_roll" ): # A repeatable monocular depth error on the second parallel # root line must affect translation only, never palm rotation # or the inferred thumb-roll zero. point_common = point_common + base_rotation.apply( np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m]) ) view_normal_common = None if ( inject_oblique_optical_depth_bias and joint in zero.phase_parent_joint ): parent_axis = model.axis_line( zero.phase_parent_joint[joint], zero_offsets=offsets, joint_angles=angles, )[0] helper = ( np.asarray([1.0, 0.0, 0.0]) if abs(float(parent_axis[0])) < 0.8 else np.asarray([0.0, 1.0, 0.0]) ) tilt_axis = np.cross(parent_axis, helper) tilt_axis /= np.linalg.norm(tilt_axis) view_normal = Rotation.from_rotvec( math.radians(15.0) * tilt_axis ).apply(parent_axis) view_normal_common = tuple(base_rotation.apply(view_normal)) # Simulate an independent planar-PnP depth error on the child # Tag. It is large enough to drive the old 3-D phase solve to # a configured offset bound. point_common = point_common + 0.03 * np.asarray( view_normal_common ) axis_common = base_rotation.apply(axis) if ( inject_observer_cone_bias_degrees and joint == "thumb_cmc_pitch" ): parent_axis = model.axis_line( zero.axis_parent_joint[joint], zero_offsets=offsets, joint_angles=angles, )[0] cone_normal = np.cross(axis, parent_axis) cone_normal /= np.linalg.norm(cone_normal) axis_common = base_rotation.apply( Rotation.from_rotvec( math.radians(inject_observer_cone_bias_degrees) * cone_normal ).apply(axis) ) if ( inject_secondary_root_axis_bias_degrees and joint == f"{zero.reference_finger}_mcp_roll" ): helper = np.asarray([0.0, 0.0, 1.0]) if abs(float(axis_common @ helper)) > 0.8: helper = np.asarray([0.0, 1.0, 0.0]) bias_axis = np.cross(axis_common, helper) bias_axis /= np.linalg.norm(bias_axis) axis_common = Rotation.from_rotvec( math.radians(inject_secondary_root_axis_bias_degrees) * bias_axis ).apply(axis_common) measurements.append( JointAxisMeasurement( joint=joint, cycle=cycle, axis_common_xyz=tuple(axis_common), point_common_xyz_m=tuple(point_common), condition_state_u8=tuple(state), plane_rms_m=0.0002, radial_rms_m=0.0002, rotation_circle_axis_difference_rad=math.radians(0.1), view_normal_common_xyz=view_normal_common, pose_axis_line_rms_m=( pose_axis_line_rms_by_joint_m or {} ).get(joint, 0.0), ) ) for source_joint, model_joint in ( hand.palm_orientation_sources or {} ).items(): state = list(baseline) angles = _angles_from_state( state, curves=curves, motor_by_joint=motor_by_joint, inherited_zero_joints=zero.inherited_zero_joints, ) axis, _ = model.axis_line( model_joint, zero_offsets=cycle_offsets, joint_angles=angles, ) axis_common = base_rotation.apply(axis) if palm_orientation_frame_bias_degrees: bias_axis = np.asarray([0.3, -0.2, 0.4], dtype=float) bias_axis /= np.linalg.norm(bias_axis) axis_common = Rotation.from_rotvec( math.radians(palm_orientation_frame_bias_degrees) * bias_axis ).apply(axis_common) palm_orientation_measurements.append( PalmOrientationMeasurement( source_joint=source_joint, model_joint=model_joint, cycle=cycle, axis_common_xyz=tuple(axis_common), condition_state_u8=tuple(state), observed_arc_rad=math.radians(45.0), rotation_orthogonal_rms_rad=math.radians(0.1), ) ) solve_profile = ( get_right_19_thumb_zero_profile() if solve_thumb_only else zero ) result = solve_urdf_zero_offsets( source_urdf=source, measurements=[ item for item in measurements if item.joint in solve_profile.axis_joints ], palm_orientation_measurements=palm_orientation_measurements, curves=curves, motor_by_joint=motor_by_joint, hand_type=side, tag_layout=layout_id, joint_maximum_offset_rad={ name: math.radians(value) for name, value in (joint_maximum_offset_degrees or {}).items() }, training_cycles=training_cycles, validation_cycle=validation_cycle, maximum_systematic_axis_cone_bias_rad=( None if maximum_systematic_axis_cone_bias_degrees is None else math.radians( maximum_systematic_axis_cone_bias_degrees ) ), static_output_zero_offsets_rad={ name: math.radians(value) for name, value in (static_output_offsets_degrees or {}).items() }, fixed_direct_zero_offsets_rad=( None if fixed_direct_offsets_degrees is None else { name: math.radians(value) for name, value in fixed_direct_offsets_degrees.items() if name in solve_profile.direct_zero_joints } ), zero_profile=solve_profile, ) return zero, result def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supplied() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] zero, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19" ) assert len(zero.direct_zero_joints) == 16 assert len(zero.axis_joints) == 21 assert result.passed is True finger_rolls = tuple( name for name in zero.direct_zero_joints if name.endswith("_mcp_roll") and not name.startswith("thumb_") ) roll_common = float( np.median( [ injected[zero.direct_zero_joints.index(name)] for name in finger_rolls ] ) ) for name, expected in zip(zero.direct_zero_joints, injected): if name in zero.fixed_direct_zero_offsets_rad: expected = zero.fixed_direct_zero_offsets_rad[name] elif name in finger_rolls: expected -= roll_common assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( expected, abs=0.05 ) assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx( -1.5, abs=0.05 ) assert zero.fixed_direct_zero_offsets_rad == {} assert result.training_cycles == (0, 1, 2) assert result.validation_cycle == 3 assert all( value == pytest.approx(0.0, abs=1.0e-12) for value in result.offset_confidence_half_width_rad.values() ) assert result.observability_parameter_count == 22 assert result.observability_rank == 22 assert math.isfinite(result.observability_condition_number) assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints) def test_right_19_thumb_kernel_is_independent_and_matches_full_hand() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] _, full = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, static_output_offsets_degrees={"thumb_cmc_roll": 2.0}, ) _, thumb = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", solve_thumb_only=True, fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, static_output_offsets_degrees={"thumb_cmc_roll": 2.0}, ) assert thumb.passed is True assert set(thumb.direct_offsets_rad) == { "thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp", } for name in thumb.direct_offsets_rad: assert thumb.direct_offsets_rad[name] == pytest.approx( full.direct_offsets_rad[name], abs=math.radians(0.05) ) finger_offsets = { name: full.direct_offsets_rad[name] for name in get_zero_calibration_profile( "right", "g20_right_19" ).direct_zero_joints if not name.startswith("thumb_") } thumb_only = merge_right_19_thumb_zero_result( thumb_result=thumb, preserved_offsets_rad=finger_offsets, ) full_hand = merge_right_19_thumb_zero_result( thumb_result=thumb, companion_result=full, ) for name in thumb.direct_offsets_rad: assert thumb_only.direct_offsets_rad[name] == pytest.approx( full_hand.direct_offsets_rad[name], abs=1.0e-12 ) standalone = expand_right_19_thumb_zero_result_with_cad_fingers(thumb) assert all( standalone.direct_offsets_rad[name] == 0.0 for name in standalone.direct_offsets_rad if not name.startswith("thumb_") ) def test_thumb_endpoint_output_uses_published_offset_safety_limit() -> None: # The visual roll scalar may carry a root-frame gauge beyond the global # limit. A separately validated mechanical endpoint is the value written # to the URDF and therefore the value the configured safety gate guards. injected = [ 21.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] _, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", solve_thumb_only=True, fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, static_output_offsets_degrees={"thumb_cmc_roll": 3.45}, ) assert result.passed is True assert "thumb_cmc_roll" not in result.failure_reasons assert math.degrees( result.direct_offsets_rad["thumb_cmc_roll"] ) == pytest.approx(3.45, abs=0.01) def test_right_19_thumb_kernel_is_invariant_to_rigid_hand_movement() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] arguments = { "layout_id": "g20_right_19", "solve_thumb_only": True, "fixed_direct_offsets_degrees": {"thumb_mcp": -1.5}, "static_output_offsets_degrees": {"thumb_cmc_roll": 2.0}, } _, first = _solve_synthetic_offsets("right", injected, **arguments) _, moved = _solve_synthetic_offsets( "right", injected, base_euler_xyz_rad=(-0.2, 0.7, -0.35), base_translation_xyz_m=(-0.15, 0.42, 0.55), **arguments, ) assert first.passed is True assert moved.passed is True for name in first.direct_offsets_rad: assert moved.direct_offsets_rad[name] == pytest.approx( first.direct_offsets_rad[name], abs=math.radians(0.01) ) def test_right_19_independent_roll_output_cannot_change_yaw_solution() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] _, visual = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19" ) _, independent = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", static_output_offsets_degrees={"thumb_cmc_roll": 3.4}, ) assert math.degrees( independent.direct_offsets_rad["thumb_cmc_roll"] ) == pytest.approx(3.4) assert independent.direct_offsets_rad["thumb_cmc_yaw"] == pytest.approx( visual.direct_offsets_rad["thumb_cmc_yaw"], abs=1.0e-10 ) assert independent.direct_offsets_rad["thumb_cmc_pitch"] == pytest.approx( visual.direct_offsets_rad["thumb_cmc_pitch"], abs=1.0e-10 ) def _partial_orientation_records( *, tag_mount: Rotation, common_rotation: Rotation, maximum_angle_deg: float = 30.0, tag_offset_xyz_m: tuple[float, float, float] = (0.01, 0.02, -0.015), include_child_pose_common: bool = False, ) -> list[dict[str, object]]: axis_parent = np.asarray([0.0, 1.0, 0.0]) parent_pose = { "translation_xyz_m": [0.2, -0.1, 0.7], "quaternion_xyzw": list(common_rotation.as_quat()), } records: list[dict[str, object]] = [] commands = tuple(range(255, 174, -4)) for direction, ordered in ( ("decreasing", commands), ("increasing", tuple(reversed(commands))), ): for command in ordered: fraction = (255 - command) / (255 - commands[-1]) angle = math.radians(maximum_angle_deg) * fraction relative = ( Rotation.from_rotvec(axis_parent * angle) * tag_mount ) relative_translation = Rotation.from_rotvec( axis_parent * angle ).apply(tag_offset_xyz_m) state = [255.0] * 20 state[1] = float(command) record: dict[str, object] = { "cycle": 0, "direction": direction, "command_u8": command, "relative_quaternion_xyzw": list(relative.as_quat()), "relative_translation_xyz_m": list( relative_translation ), "parent_pose_common": parent_pose, "state_u8": state, } if include_child_pose_common: child_common = common_rotation * relative record["child_pose_common"] = { "translation_xyz_m": list( np.asarray(parent_pose["translation_xyz_m"]) + common_rotation.apply(relative_translation) ), "quaternion_xyzw": list(child_common.as_quat()), } records.append(record) return records def test_partial_palm_direction_ignores_fixed_tag_mount_pose() -> None: common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) first = fit_partial_palm_orientation_measurement( "index_mcp_pitch_front_axis", "index_mcp_pitch", _partial_orientation_records( tag_mount=Rotation.from_euler("xyz", [0.1, 0.2, -0.4]), common_rotation=common_rotation, ), cycle=0, zero_command_u8=255, ) second = fit_partial_palm_orientation_measurement( "index_mcp_pitch_front_axis", "index_mcp_pitch", _partial_orientation_records( tag_mount=Rotation.from_euler("xyz", [-0.7, 0.45, 0.9]), common_rotation=common_rotation, tag_offset_xyz_m=(-0.035, 0.008, 0.041), ), cycle=0, zero_command_u8=255, ) expected = common_rotation.apply([0.0, 1.0, 0.0]) assert abs(np.dot(first.axis_common_xyz, expected)) == pytest.approx( 1.0, abs=1.0e-8 ) assert abs( np.dot(first.axis_common_xyz, second.axis_common_xyz) ) == pytest.approx(1.0, abs=1.0e-8) def test_partial_palm_direction_is_not_weighted_by_dwell_frame_count() -> None: records = _partial_orientation_records( tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), common_rotation=Rotation.identity(), ) baseline = fit_partial_palm_orientation_measurement( "index_mcp_pitch_front_axis", "index_mcp_pitch", records, cycle=0, zero_command_u8=255, ) source = next( record for record in records if record["direction"] == "decreasing" and record["command_u8"] == 223 ) biased = dict(source) biased["relative_quaternion_xyzw"] = list( ( Rotation.from_rotvec([math.radians(2.0), 0.0, 0.0]) * Rotation.from_quat(source["relative_quaternion_xyzw"]) ).as_quat() ) fitted = fit_partial_palm_orientation_measurement( "index_mcp_pitch_front_axis", "index_mcp_pitch", [*records, *([biased] * 200)], cycle=0, zero_command_u8=255, ) difference = math.acos( abs( float( np.clip( np.dot(baseline.axis_common_xyz, fitted.axis_common_xyz), -1.0, 1.0, ) ) ) ) assert difference < math.radians(0.25) def test_partial_palm_direction_ignores_far_stroke_pnp_bias() -> None: records = _partial_orientation_records( tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), common_rotation=Rotation.identity(), maximum_angle_deg=30.0, ) expected = fit_partial_palm_orientation_measurement( "thumb_cmc_roll_top_axis", "thumb_cmc_roll", records, cycle=0, zero_command_u8=255, minimum_arc_rad=math.radians(5.0), maximum_command_distance_u8=64, ) biased: list[dict[str, object]] = [] for source in records: record = dict(source) if abs(int(record["command_u8"]) - 255) > 64: rotation = Rotation.from_quat( record["relative_quaternion_xyzw"] ) record["relative_quaternion_xyzw"] = list( ( Rotation.from_rotvec([math.radians(12.0), 0.0, 0.0]) * rotation ).as_quat() ) biased.append(record) fitted = fit_partial_palm_orientation_measurement( "thumb_cmc_roll_top_axis", "thumb_cmc_roll", biased, cycle=0, zero_command_u8=255, minimum_arc_rad=math.radians(5.0), maximum_command_distance_u8=64, ) assert abs( float(np.dot(expected.axis_common_xyz, fitted.axis_common_xyz)) ) == pytest.approx(1.0, abs=1.0e-10) def test_partial_palm_direction_uses_robust_full_stroke_local_motion() -> None: common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) records = _partial_orientation_records( tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), common_rotation=common_rotation, maximum_angle_deg=48.0, include_child_pose_common=True, ) expected = fit_partial_palm_orientation_measurement( "thumb_cmc_roll_top_axis", "thumb_cmc_roll", records, cycle=0, zero_command_u8=255, ) biased: list[dict[str, object]] = [] for source in records: record = dict(source) pose = dict(record["child_pose_common"]) # Simulate a command-correlated planar-PnP branch curvature affecting # fewer than one quarter of the full-stroke local increments. The # zero-adjacent quality trajectory remains unchanged. if int(record["command_u8"]) <= 187: rotation = Rotation.from_quat(pose["quaternion_xyzw"]) amount = math.radians(8.0) * ( (187 - int(record["command_u8"])) / 12.0 ) pose["quaternion_xyzw"] = list( ( Rotation.from_rotvec([amount, 0.0, 0.0]) * rotation ).as_quat() ) record["child_pose_common"] = pose biased.append(record) fitted = fit_partial_palm_orientation_measurement( "thumb_cmc_roll_top_axis", "thumb_cmc_roll", biased, cycle=0, zero_command_u8=255, ) difference = math.acos( abs( float( np.clip( np.dot(expected.axis_common_xyz, fitted.axis_common_xyz), -1.0, 1.0, ) ) ) ) assert expected.axis_estimator == "robust_full_stroke_local_so3_v1" assert expected.incremental_pair_count >= 24 assert fitted.axis_estimator == expected.axis_estimator assert difference < math.radians(0.25) def test_full_stroke_palm_axis_tracks_reestablished_common_frame() -> None: tag_mount = Rotation.from_euler("xyz", [0.2, -0.1, 0.3]) first_common = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) repositioning = Rotation.from_euler("xyz", [-0.18, 0.27, -0.41]) second_common = repositioning * first_common first = fit_partial_palm_orientation_measurement( "thumb_cmc_roll_top_axis", "thumb_cmc_roll", _partial_orientation_records( tag_mount=tag_mount, common_rotation=first_common, maximum_angle_deg=48.0, include_child_pose_common=True, ), cycle=0, zero_command_u8=255, ) second = fit_partial_palm_orientation_measurement( "thumb_cmc_roll_top_axis", "thumb_cmc_roll", _partial_orientation_records( tag_mount=tag_mount, common_rotation=second_common, maximum_angle_deg=48.0, include_child_pose_common=True, ), cycle=0, zero_command_u8=255, ) expected_second = repositioning.apply(first.axis_common_xyz) assert first.axis_estimator == "robust_full_stroke_local_so3_v1" assert second.axis_estimator == first.axis_estimator assert abs(float(np.dot(expected_second, second.axis_common_xyz))) == ( pytest.approx(1.0, abs=1.0e-8) ) def test_right_19_top_axis_pair_holdout_ignores_shared_frame_bias() -> None: injected = [0.0] * 16 _, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", palm_orientation_frame_bias_degrees=8.0, ) assert result.passed is True assert math.degrees( result.direct_offsets_rad["thumb_cmc_yaw"] ) == pytest.approx(0.0, abs=0.05) def test_partial_palm_direction_rejects_too_short_visible_arc() -> None: records = _partial_orientation_records( tag_mount=Rotation.identity(), common_rotation=Rotation.identity(), maximum_angle_deg=5.0, ) with pytest.raises(ValueError, match="visible rotation arc"): fit_partial_palm_orientation_measurement( "index_mcp_pitch_front_axis", "index_mcp_pitch", records, cycle=0, zero_command_u8=255, ) def test_partial_palm_direction_uses_three_of_four_visible_sources() -> None: sources = { f"{finger}_mcp_pitch_front_axis": f"{finger}_mcp_pitch" for finger in ("index", "middle", "ring", "pinky") } records = { source: _partial_orientation_records( tag_mount=Rotation.from_euler( "xyz", [0.1 * index, -0.2, 0.3] ), common_rotation=Rotation.identity(), maximum_angle_deg=(5.0 if index == 3 else 30.0), ) for index, source in enumerate(sources) } fitted, rejected = fit_partial_palm_orientation_measurements( sources=sources, records_by_joint=records, motor_by_source={source: index + 1 for index, source in enumerate(sources)}, baseline_command_u8=(255,) * 20, cycles=(0,), minimum_sources=3, ) assert len(fitted) == 3 assert len(rejected) == 1 assert next(iter(rejected)).startswith("pinky_mcp_pitch_front_axis") def test_right_19_offsets_are_invariant_to_rigid_hand_repositioning() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] _, first = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19" ) _, moved = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", base_euler_xyz_rad=(-0.25, 0.55, -0.35), base_translation_xyz_m=(-0.12, 0.28, 0.91), ) assert moved.direct_offsets_rad == pytest.approx( first.direct_offsets_rad, abs=1.0e-8 ) def test_right_19_roll_and_verified_contact_zeros_use_independent_endpoints() -> None: hand = get_hand_calibration_profile("right", "g20_right_19") curves = { name: _synthetic_curve(255, math.radians(50.0)) for name in hand.measured_joints } for finger in ("index", "middle", "ring", "pinky"): curves[f"{finger}_mcp_pitch"] = _synthetic_curve( 255, math.radians(71.0) ) curves[f"{finger}_pip"] = _synthetic_curve( 255, math.radians(103.0) ) curves["thumb_mcp"] = _synthetic_curve(255, math.radians(70.0)) curves["thumb_cmc_roll"] = _synthetic_curve( 255, math.radians(76.0) ) curves["thumb_cmc_yaw"] = _synthetic_curve( 255, math.radians(91.0) ) curves["thumb_cmc_pitch"] = _synthetic_curve( 255, math.radians(47.0) ) offsets = derive_right_19_mechanical_endpoint_offsets( RIGHT_SOURCE_URDF, curves ) assert set(offsets) == { "thumb_cmc_roll", "thumb_mcp", *( f"{finger}_{suffix}" for finger in ("index", "middle", "ring", "pinky") for suffix in ("mcp_pitch", "pip") ), } assert math.degrees(offsets["thumb_mcp"]) == pytest.approx( math.degrees(1.25) - 70.0 ) assert math.degrees(offsets["thumb_cmc_roll"]) == pytest.approx( math.degrees(1.39) - 76.0 ) assert not {"thumb_cmc_yaw", "thumb_cmc_pitch"}.intersection(offsets) assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx( math.degrees(1.22) - 71.0 ) assert math.degrees(offsets["index_pip"]) == pytest.approx( math.degrees(1.75) - 103.0 ) def _settled_endpoint_records( travel_rad: float, *, increasing_travel_rad: float | None = None, ) -> list[dict[str, object]]: fixed_mounting = Rotation.from_euler("xyz", [0.7, -0.4, 1.1]) mounted_axis = Rotation.from_euler("xyz", [-0.3, 0.8, 0.2]).apply( [0.0, 0.0, 1.0] ) result: list[dict[str, object]] = [] for direction, branch_travel in ( ("decreasing", travel_rad), ( "increasing", travel_rad if increasing_travel_rad is None else increasing_travel_rad, ), ): endpoint = fixed_mounting * Rotation.from_rotvec( mounted_axis * branch_travel ) for command, rotation in ((0, endpoint), (255, fixed_mounting)): result.append( { "cycle": 0, "direction": direction, "requested_command_u8": command, "feedback_u8": float(command), "relative_quaternion_xyzw": rotation.as_quat().tolist(), } ) return result def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None: hand = get_hand_calibration_profile("right", "g20_right_19") names = { "thumb_cmc_roll", "thumb_mcp", *( f"{finger}_{suffix}" for finger in ("index", "middle", "ring", "pinky") for suffix in ("mcp_pitch", "pip") ), } projected_travel = math.radians(105.832) direct_travel = math.radians(103.820) curves = { name: _synthetic_curve(255, projected_travel) for name in hand.measured_joints } records = { name: _settled_endpoint_records(direct_travel) for name in names } anchored = anchor_right_19_mechanical_endpoint_curves( curves, records, feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}), ) assert curves["middle_pip"].angle_rad[0] == pytest.approx( projected_travel ) for name in names: assert anchored[name].angle_rad[0] == pytest.approx(direct_travel) assert anchored[name].angle_rad[255] == pytest.approx(0.0) assert anchored[name].circle[ "mechanical_endpoint_direct_travel_rad" ] == pytest.approx(direct_travel) assert anchored["thumb_cmc_pitch"] is curves["thumb_cmc_pitch"] def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None: hand = get_hand_calibration_profile("right", "g20_right_19") names = { "thumb_cmc_roll", "thumb_mcp", *( f"{finger}_{suffix}" for finger in ("index", "middle", "ring", "pinky") for suffix in ("mcp_pitch", "pip") ), } curves = { name: _synthetic_curve(255, math.radians(103.0)) for name in hand.measured_joints } records = { name: _settled_endpoint_records(math.radians(103.0)) for name in names } records["middle_pip"] = _settled_endpoint_records( math.radians(103.0), increasing_travel_rad=math.radians(104.5), ) with pytest.raises(ValueError, match="endpoint directions disagree"): anchor_right_19_mechanical_endpoint_curves( curves, records, feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}), ) def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None: # A shared electrical centre belongs to the four-motor common datum; the # strict 3 deg assembly guard applies to finger-to-finger deviations. injected = [ 2.0, -3.0, 4.0, -1.5, 6.0, -1.0, 0.7, 6.4, -0.7, -0.8, 5.7, 0.6, 0.9, 6.2, -1.0, -0.6, ] zero, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19" ) assert result.passed is True finger_rolls = tuple( name for name in zero.direct_zero_joints if name.endswith("_mcp_roll") and not name.startswith("thumb_") ) roll_common = float( np.median( [ injected[zero.direct_zero_joints.index(name)] for name in finger_rolls ] ) ) for name, expected in zip(zero.direct_zero_joints, injected): if name in zero.fixed_direct_zero_offsets_rad: expected = zero.fixed_direct_zero_offsets_rad[name] elif name in finger_rolls: expected -= roll_common assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( expected, abs=0.05 ) assert np.median( [result.direct_offsets_rad[name] for name in finger_rolls] ) == pytest.approx(0.0, abs=1.0e-10) def test_right_19_roll_gauge_at_search_bound_uses_relative_deviation() -> None: # One raw roll reaches the diagnostic search edge, as in session 160326, # while all four physical deviations from their shared datum remain safe. injected = [ 2.0, -3.0, 4.0, -1.5, 9.3, -1.0, 0.0, 8.4, -0.7, 0.0, 8.7, 0.6, 0.0, 8.8, -1.0, 0.0, ] zero, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19" ) assert result.passed is True finger_rolls = tuple( name for name in zero.direct_zero_joints if name.endswith("_mcp_roll") and not name.startswith("thumb_") ) assert max( abs(math.degrees(result.direct_offsets_rad[name])) for name in finger_rolls ) < 3.0 assert not result.failure_reasons def test_right_19_holdout_never_changes_frozen_training_offsets() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] zero, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", validation_offset_bias_degrees={"thumb_cmc_roll": 2.0}, ) finger_rolls = tuple( name for name in zero.direct_zero_joints if name.endswith("_mcp_roll") and not name.startswith("thumb_") ) roll_common = float( np.median( [ injected[zero.direct_zero_joints.index(name)] for name in finger_rolls ] ) ) assert math.degrees( result.direct_offsets_rad["thumb_cmc_roll"] ) == pytest.approx(injected[0], abs=0.05) assert result.validation_cycle not in result.training_cycles def test_failed_thumb_holdout_does_not_blame_fixed_partial_scope_zeros() -> None: zero = get_zero_calibration_profile("right", "g20_right_19") injected = [ 2.0, -3.0, 4.0, -1.5, 1.0, -1.0, 0.7, 0.8, -0.7, -0.8, -0.5, 0.6, 0.9, 1.1, -1.0, -0.6, ] frozen = { name: injected[index] for index, name in enumerate(zero.direct_zero_joints) if not name.startswith("thumb_") } _, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", validation_offset_bias_degrees={"thumb_cmc_yaw": 8.0}, fixed_direct_offsets_degrees=frozen, ) assert result.passed is False assert result.failure_reasons == { "palm_orientation": "palm_orientation_holdout_too_large" } assert set(frozen).isdisjoint(result.failure_reasons) def test_right_19_palm_pose_and_16_zero_observation_jacobian_is_full_rank() -> None: """Guard the reviewed 6-palm-DOF plus 16-static-zero observability.""" zero = get_zero_calibration_profile("right", "g20_right_19") model = UrdfKinematicModel(RIGHT_SOURCE_URDF) parameter_count = 6 + len(zero.direct_zero_joints) def line_observations(parameters: np.ndarray) -> np.ndarray: palm_rotation = Rotation.from_rotvec(parameters[:3]) palm_translation = parameters[3:6] offsets = { name: float(value) for name, value in zip( zero.direct_zero_joints, parameters[6:] ) } result: list[float] = [] for joint in zero.axis_joints: axis, point = model.axis_line( joint, zero_offsets=offsets, joint_angles={} ) axis = palm_rotation.apply(axis) point = palm_rotation.apply(point) + palm_translation # An oriented 3-D line is represented by its direction and # Pluecker moment. The moment is invariant to choosing a different # point along the same axis, so no unobservable along-axis Tag # placement is accidentally counted as information. result.extend(float(value) for value in axis) result.extend(float(value) for value in np.cross(point, axis)) return np.asarray(result, dtype=float) origin = np.zeros(parameter_count, dtype=float) step = 1.0e-6 jacobian = np.column_stack( [ ( line_observations( origin + np.eye(parameter_count, dtype=float)[index] * step ) - line_observations( origin - np.eye(parameter_count, dtype=float)[index] * step ) ) / (2.0 * step) for index in range(parameter_count) ] ) assert parameter_count == 22 assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count def test_right_19_urdf_writer_changes_only_the_16_static_targets( tmp_path: Path, ) -> None: zero = get_zero_calibration_profile("right", "g20_right_19") offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints} destination = write_zero_corrected_urdf( source_urdf=RIGHT_SOURCE_URDF, output_directory=tmp_path, serial_number="G20_RIGHT_019", offsets_rad=offsets, timestamp="20260818_120000", ) original = { str(joint.get("name")): joint for joint in ET.parse(RIGHT_SOURCE_URDF).getroot().findall("joint") } corrected = { str(joint.get("name")): joint for joint in ET.parse(destination).getroot().findall("joint") } for mesh in ET.parse(destination).getroot().findall(".//mesh"): relative = Path(mesh.get("filename")) copied = destination.parent / relative source = RIGHT_SOURCE_URDF.parent / relative assert copied.is_file() assert copied.stat().st_size == source.stat().st_size changed = set() for name in original: original_origin = original[name].find("origin") corrected_origin = corrected[name].find("origin") if original_origin is None or corrected_origin is None: continue if original_origin.get("rpy") != corrected_origin.get("rpy"): changed.add(name) assert original_origin.get("xyz") == corrected_origin.get("xyz") assert changed == set(zero.direct_zero_joints) assert "thumb_mcp" in changed assert not set(get_hand_calibration_profile( "right", "g20_right_19" ).passive_joints) & changed def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None: zero, result = _solve_synthetic_offsets("right", [0.1] * 7) assert result.passed is True static_policy = { **zero.fixed_direct_zero_offsets_rad, **zero.static_output_zero_offsets_rad, } for name, value in result.direct_offsets_rad.items(): assert value == pytest.approx(static_policy.get(name, 0.0)) def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None: right = get_zero_calibration_profile("right") right_19 = get_zero_calibration_profile("right", "g20_right_19") left = get_zero_calibration_profile("left") assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad assert "thumb_mcp" not in right_19.fixed_direct_zero_offsets_rad def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None: zero, result = _solve_synthetic_offsets( "right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0] ) reference_roll = f"{zero.reference_finger}_mcp_roll" assert result.passed is True assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx( 0.0, abs=1.0e-12 ) def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None: zero, result = _solve_synthetic_offsets( "right", [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], inject_secondary_root_axis_bias_degrees=15.0, ) assert result.passed is True expected = dict( zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]) ) expected.update( { name: math.degrees(value) for name, value in { **zero.fixed_direct_zero_offsets_rad, **zero.static_output_zero_offsets_rad, }.items() } ) for name, value in expected.items(): assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( value, abs=0.05 ) def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None: zero, result = _solve_synthetic_offsets( "right", [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], inject_secondary_root_point_bias_m=0.02, ) assert result.passed is True assert math.degrees( result.direct_offsets_rad["thumb_cmc_roll"] ) == pytest.approx(2.0, abs=0.05) assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0 def test_thumb_mcp_is_not_hard_coded_to_original_cad_zero() -> None: offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0, *([0.0] * 9)] _, result = _solve_synthetic_offsets( "right", offsets, layout_id="g20_right_19", joint_maximum_offset_degrees={"thumb_mcp": 45.0}, ) assert result.passed is True assert math.degrees(result.direct_offsets_rad["thumb_mcp"]) == pytest.approx( -40.0, abs=0.05 ) assert all( math.degrees(value) == pytest.approx(-40.0, abs=0.05) for value in result.cycle_offsets_rad["thumb_mcp"] ) assert result.validation_error_by_joint_rad["thumb_ip"] < math.radians(0.05) for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"): assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0 for name in ("pinky_mcp_pitch", "pinky_pip"): assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0 assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0) def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None: zero, result = _solve_synthetic_offsets( "right", [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], inject_oblique_optical_depth_bias=True, ) assert result.passed is True expected = dict( zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]) ) expected.update( { name: math.degrees(value) for name, value in { **zero.fixed_direct_zero_offsets_rad, **zero.static_output_zero_offsets_rad, }.items() } ) for name, value in expected.items(): assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( value, abs=0.05 ) def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None: _, result = _solve_synthetic_offsets( "right", [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], inject_observer_cone_bias_degrees=8.0, ) assert result.passed is False assert result.failure_reasons["thumb_cmc_yaw"] == ( "zero_axis_cone_mismatch_too_large" ) def test_right_19_audits_stable_cross_view_cone_bias_without_retry() -> None: injected = [0.0] * 16 _, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", inject_observer_cone_bias_degrees=8.0, maximum_systematic_axis_cone_bias_degrees=15.0, ) assert result.passed is True assert math.degrees( result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"] ) == pytest.approx(8.0) assert result.axis_cone_bias_classification_by_joint[ "thumb_cmc_yaw" ] == "stable_cross_view_or_planar_pnp_bias" def test_right_19_thumb_yaw_uses_top_axis_pair_not_front_cone_bias() -> None: zero = get_zero_calibration_profile("right", "g20_right_19") injected = [0.0] * len(zero.direct_zero_joints) injected[zero.direct_zero_joints.index("thumb_cmc_roll")] = 3.611 injected[zero.direct_zero_joints.index("thumb_cmc_yaw")] = -1.319 zero, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", inject_observer_cone_bias_degrees=3.803, maximum_systematic_axis_cone_bias_degrees=15.0, ) assert result.passed is True assert math.degrees( result.direct_offsets_rad["thumb_cmc_roll"] ) == pytest.approx(3.611, abs=0.05) assert math.degrees( result.direct_offsets_rad["thumb_cmc_yaw"] ) == pytest.approx(-1.319, abs=0.05) assert math.degrees( result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"] ) == pytest.approx(3.803, abs=0.01) def test_right_19_still_rejects_gross_cross_view_cone_mismatch() -> None: injected = [0.0] * 16 _, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19", inject_observer_cone_bias_degrees=16.0, maximum_systematic_axis_cone_bias_degrees=15.0, ) assert result.passed is False assert result.failure_reasons["thumb_cmc_yaw"] == ( "zero_axis_cone_mismatch_too_large" ) def test_right_19_finger_roll_common_gauge_is_removed_before_limits() -> None: zero = get_zero_calibration_profile("right", "g20_right_19") injected = [0.0] * len(zero.direct_zero_joints) deviations = { "index_mcp_roll": -0.4, "middle_mcp_roll": 0.7, "ring_mcp_roll": 0.1, "pinky_mcp_roll": -0.1, } for name, deviation in deviations.items(): injected[zero.direct_zero_joints.index(name)] = 18.0 + deviation _, result = _solve_synthetic_offsets( "right", injected, layout_id="g20_right_19" ) assert result.passed is True common = float(np.median(list(deviations.values()))) for name, deviation in deviations.items(): expected = deviation - common assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( expected, abs=0.05 ) def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None: _, result = _solve_synthetic_offsets( "right", [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002}, ) assert result.passed is False assert result.failure_reasons["thumb_cmc_pitch"] == ( "zero_phase_axis_line_residual_too_large" ) def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None: zero = get_zero_calibration_profile("left") baseline = [255.0] * 20 baseline[6:10] = [127.0] * 4 curves = { name: _synthetic_curve( int(baseline[JOINT_SPECS[name].motor_index]), math.radians(50.0), ) for name in MEASURED_JOINTS } motor_by_joint = { name: spec.motor_index for name, spec in JOINT_SPECS.items() } true_offsets = { name: math.radians(value) for name, value in zip( DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0] ) } model = UrdfKinematicModel(SOURCE_URDF) base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8]) base_translation = np.asarray([0.31, -0.19, 0.72]) measurements = [] yaw_axis_without_clearance = None yaw_axis_with_clearance = None for cycle in range(3): for joint in AXIS_JOINTS: state = list(baseline) if joint == "thumb_cmc_yaw": state[5] = 145.0 angles = _angles_from_state( state, curves=curves, motor_by_joint=motor_by_joint ) axis, point = model.axis_line( joint, zero_offsets=true_offsets, joint_angles=angles, ) if joint == "thumb_cmc_yaw": yaw_axis_with_clearance = axis.copy() baseline_angles = _angles_from_state( baseline, curves=curves, motor_by_joint=motor_by_joint ) yaw_axis_without_clearance = model.axis_line( joint, zero_offsets=true_offsets, joint_angles=baseline_angles, )[0] measurements.append( JointAxisMeasurement( joint=joint, cycle=cycle, axis_common_xyz=tuple(base_rotation.apply(axis)), point_common_xyz_m=tuple( base_rotation.apply(point) + base_translation ), condition_state_u8=tuple(state), plane_rms_m=0.0002, radial_rms_m=0.0002, rotation_circle_axis_difference_rad=math.radians(0.1), ) ) result = solve_urdf_zero_offsets( source_urdf=SOURCE_URDF, measurements=measurements, curves=curves, motor_by_joint=motor_by_joint, ) assert math.degrees( math.acos( np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0) ) ) > 1.0 assert result.passed is True static_policy = { **zero.fixed_direct_zero_offsets_rad, **zero.static_output_zero_offsets_rad, } for name, expected in true_offsets.items(): if name in static_policy: expected = static_policy[name] assert result.direct_offsets_rad[name] == pytest.approx( expected, abs=math.radians(0.05) ) assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS) assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx( 0.0, abs=math.radians(0.05) ) for target in INHERITED_ZERO_JOINTS: assert result.all_active_offsets_rad[target] == pytest.approx( 0.0, abs=1.0e-12 ) assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys() def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None: hand = get_hand_calibration_profile("right") zero = get_zero_calibration_profile("right") baseline = [255.0] * 20 baseline[6:10] = [127.0] * 4 curves = { name: _synthetic_curve( int(baseline[hand.joint_specs[name].motor_index]), math.radians(50.0), ) for name in hand.measured_joints } motor_by_joint = { name: spec.motor_index for name, spec in hand.joint_specs.items() } true_offsets = { name: math.radians(value) for name, value in zip( zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], ) } model = UrdfKinematicModel(RIGHT_SOURCE_URDF) base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8]) base_translation = np.asarray([0.31, -0.19, 0.72]) measurements: list[JointAxisMeasurement] = [] for cycle in range(3): states: dict[str, list[float]] = {} lines: dict[str, tuple[np.ndarray, np.ndarray]] = {} for joint in zero.axis_joints: state = list(baseline) if joint == "thumb_cmc_yaw": state[5] = 145.0 states[joint] = state angles = _angles_from_state( state, curves=curves, motor_by_joint=motor_by_joint, inherited_zero_joints=zero.inherited_zero_joints, ) lines[joint] = model.axis_line( joint, zero_offsets=true_offsets, joint_angles=angles, ) original_lines = { name: (axis.copy(), point.copy()) for name, (axis, point) in lines.items() } for observer, parent in zero.phase_parent_joint.items(): parent_axis, parent_point = lines[parent] original_parent_axis, original_parent_point = original_lines[parent] child_axis, original_child_point = original_lines[observer] radial = original_child_point - original_parent_point radial -= original_parent_axis * float( radial @ original_parent_axis ) # Preserve angular phase while deliberately corrupting link radius # and along-axis depth. These components must not move a zero. lines[observer] = ( child_axis, parent_point + 1.25 * radial + 0.02 * parent_axis, ) for joint in zero.axis_joints: axis, point = lines[joint] measurements.append( JointAxisMeasurement( joint=joint, cycle=cycle, axis_common_xyz=tuple(base_rotation.apply(axis)), point_common_xyz_m=tuple( base_rotation.apply(point) + base_translation ), condition_state_u8=tuple(states[joint]), plane_rms_m=0.0002, radial_rms_m=0.0002, rotation_circle_axis_difference_rad=math.radians(0.1), ) ) result = solve_urdf_zero_offsets( source_urdf=RIGHT_SOURCE_URDF, measurements=measurements, curves=curves, motor_by_joint=motor_by_joint, hand_type="right", ) assert result.passed is True static_policy = { **zero.fixed_direct_zero_offsets_rad, **zero.static_output_zero_offsets_rad, } for name, expected in true_offsets.items(): if name in static_policy: expected = static_policy[name] assert result.direct_offsets_rad[name] == pytest.approx( expected, abs=math.radians(0.05) ) for target in zero.inherited_zero_joints: assert result.all_active_offsets_rad[target] == pytest.approx( 0.0, abs=1.0e-12 ) assert set(result.all_active_offsets_rad) == set(hand.active_joints)