2244 lines
78 KiB
Python
2244 lines
78 KiB
Python
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,
|
|
)
|
|
|
|
|
|
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"],
|
|
)
|
|
|
|
|
|
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_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)
|