refactor: dispatch formal calibration through model profiles
This commit is contained in:
@@ -34,6 +34,16 @@ Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改;v
|
|||||||
`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明,通用层
|
`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明,通用层
|
||||||
不要求 `front/side/top`,也不假设固定 20 个命令。
|
不要求 `front/side/top`,也不假设固定 20 个命令。
|
||||||
|
|
||||||
|
仓库中存在已审定硬件会话时,可执行只读金标准检查(不会覆盖任何产物):
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run linkerhand_calibration validate_g20_goldens \
|
||||||
|
calibration_output/G20_RIGHT_001
|
||||||
|
```
|
||||||
|
|
||||||
|
它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话;完整
|
||||||
|
整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。
|
||||||
|
|
||||||
完全独立地只标定大拇指4项任务时,使用:
|
完全独立地只标定大拇指4项任务时,使用:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
|
|||||||
@@ -88,7 +88,7 @@ def _launch_stack(context):
|
|||||||
expected_source_hash = LaunchConfiguration(
|
expected_source_hash = LaunchConfiguration(
|
||||||
"source_urdf_expected_sha256"
|
"source_urdf_expected_sha256"
|
||||||
).perform(context).strip().lower()
|
).perform(context).strip().lower()
|
||||||
if contract.profile.supports("urdf_zero_publication"):
|
if contract.typed_profile.artifacts.publish_corrected_urdf:
|
||||||
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
||||||
@@ -276,6 +276,10 @@ def _launch_stack(context):
|
|||||||
name="g20_calibration",
|
name="g20_calibration",
|
||||||
output="screen",
|
output="screen",
|
||||||
emulate_tty=True,
|
emulate_tty=True,
|
||||||
|
arguments=[
|
||||||
|
"--profile-id",
|
||||||
|
contract.typed_profile.key.profile_id,
|
||||||
|
],
|
||||||
parameters=[
|
parameters=[
|
||||||
LaunchConfiguration("calibration_config"),
|
LaunchConfiguration("calibration_config"),
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -1,11 +1,17 @@
|
|||||||
"""Compatibility adapters for one-release calibration migrations."""
|
"""Compatibility adapters for one-release calibration migrations."""
|
||||||
|
|
||||||
from .config_v1 import product_profile_key
|
from .config_v1 import (
|
||||||
|
legacy_default_profile_key,
|
||||||
|
product_profile_key,
|
||||||
|
resolve_legacy_profile_alias,
|
||||||
|
)
|
||||||
from .defaults import default_product_config_path
|
from .defaults import default_product_config_path
|
||||||
from .paths import resolve_renamed_package_path
|
from .paths import resolve_renamed_package_path
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
"default_product_config_path",
|
"default_product_config_path",
|
||||||
|
"legacy_default_profile_key",
|
||||||
"product_profile_key",
|
"product_profile_key",
|
||||||
|
"resolve_legacy_profile_alias",
|
||||||
"resolve_renamed_package_path",
|
"resolve_renamed_package_path",
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -28,3 +28,20 @@ def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
|
|||||||
if not layout and (model, side) == ("G20", "right"):
|
if not layout and (model, side) == ("G20", "right"):
|
||||||
layout = "g20_right_19"
|
layout = "g20_right_19"
|
||||||
return ProfileKey(model, side, layout, 1)
|
return ProfileKey(model, side, layout, 1)
|
||||||
|
|
||||||
|
|
||||||
|
def legacy_default_profile_key() -> ProfileKey:
|
||||||
|
"""Preserve the former no-argument executable for one release."""
|
||||||
|
return ProfileKey("G20", "right", "g20_right_19", 1)
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey:
|
||||||
|
"""Map retired layout identifiers to their reviewed physical profile."""
|
||||||
|
if (
|
||||||
|
key.model == "G20"
|
||||||
|
and key.side == "right"
|
||||||
|
and key.layout == "g20_right_15"
|
||||||
|
and key.revision == 1
|
||||||
|
):
|
||||||
|
return ProfileKey("G20", "right", "g20_right_19", 1)
|
||||||
|
return key
|
||||||
|
|||||||
@@ -1 +1,4 @@
|
|||||||
"""Legacy single-camera algorithms retained for one compatibility release."""
|
"""Legacy single-camera algorithms retained for one compatibility release."""
|
||||||
|
from .session_v1 import uses_coupled_full_hand_zero_solver
|
||||||
|
|
||||||
|
__all__ = ["uses_coupled_full_hand_zero_solver"]
|
||||||
|
|||||||
@@ -0,0 +1,24 @@
|
|||||||
|
"""Version selection for replaying durable pre-v3 hardware sessions."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from typing import Any, Mapping
|
||||||
|
|
||||||
|
|
||||||
|
def uses_coupled_full_hand_zero_solver(
|
||||||
|
session_start: Mapping[str, Any],
|
||||||
|
) -> bool:
|
||||||
|
"""Return the solver contract recorded by the legacy session header.
|
||||||
|
|
||||||
|
Capabilities are not consulted by the live runtime. This adapter reads
|
||||||
|
the durable v1 header only so offline replay can reproduce an artifact
|
||||||
|
created before the independent thumb solver was introduced.
|
||||||
|
"""
|
||||||
|
capabilities = {
|
||||||
|
str(value) for value in session_start.get("capabilities", ())
|
||||||
|
}
|
||||||
|
return (
|
||||||
|
int(session_start.get("sample_schema_version", 1)) == 1
|
||||||
|
and "palm_axis_side_channel_v2" in capabilities
|
||||||
|
and "palm_axis_relative_motion_v3" not in capabilities
|
||||||
|
)
|
||||||
@@ -23,6 +23,8 @@ class ProfileKey:
|
|||||||
object.__setattr__(self, "revision", int(self.revision))
|
object.__setattr__(self, "revision", int(self.revision))
|
||||||
if not self.model or not self.side or not self.layout:
|
if not self.model or not self.side or not self.layout:
|
||||||
raise ValueError("profile identity fields must be non-empty")
|
raise ValueError("profile identity fields must be non-empty")
|
||||||
|
if self.side not in {"left", "right"}:
|
||||||
|
raise ValueError("profile side must be left or right")
|
||||||
if self.revision < 1:
|
if self.revision < 1:
|
||||||
raise ValueError("profile revision must be positive")
|
raise ValueError("profile revision must be positive")
|
||||||
|
|
||||||
@@ -74,6 +76,10 @@ class VisionRigSpec:
|
|||||||
views: tuple[ViewSpec, ...]
|
views: tuple[ViewSpec, ...]
|
||||||
common_frame: str
|
common_frame: str
|
||||||
extrinsic_reference_view: str
|
extrinsic_reference_view: str
|
||||||
|
extrinsics_quality_limits: Mapping[str, float] = field(
|
||||||
|
default_factory=dict
|
||||||
|
)
|
||||||
|
minimum_capture_counts: Mapping[str, int] = field(default_factory=dict)
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def view_names(self) -> tuple[str, ...]:
|
def view_names(self) -> tuple[str, ...]:
|
||||||
@@ -168,6 +174,7 @@ class ArtifactPolicy:
|
|||||||
protected_input_fields: frozenset[str]
|
protected_input_fields: frozenset[str]
|
||||||
publication_pointer: str = "latest_passed"
|
publication_pointer: str = "latest_passed"
|
||||||
session_compatibility_tokens: frozenset[str] = frozenset()
|
session_compatibility_tokens: frozenset[str] = frozenset()
|
||||||
|
publish_corrected_urdf: bool = False
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
|
|||||||
@@ -1,5 +1,14 @@
|
|||||||
"""Pure geometry used by online and offline calibration."""
|
"""Pure geometry used by online and offline calibration."""
|
||||||
|
|
||||||
|
from .extrinsics import (
|
||||||
|
CameraCalibrationIdentity,
|
||||||
|
CameraExtrinsics,
|
||||||
|
camera_info_fingerprint,
|
||||||
|
load_camera_extrinsics,
|
||||||
|
matrix_payload,
|
||||||
|
transform_matrix,
|
||||||
|
validate_camera_extrinsics_payload,
|
||||||
|
)
|
||||||
from .rotation import (
|
from .rotation import (
|
||||||
delta_rotation_vector,
|
delta_rotation_vector,
|
||||||
fit_rotation_axis,
|
fit_rotation_axis,
|
||||||
@@ -13,13 +22,20 @@ from .rotation import (
|
|||||||
)
|
)
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
|
"CameraCalibrationIdentity",
|
||||||
|
"CameraExtrinsics",
|
||||||
|
"camera_info_fingerprint",
|
||||||
"delta_rotation_vector",
|
"delta_rotation_vector",
|
||||||
"fit_rotation_axis",
|
"fit_rotation_axis",
|
||||||
"image_plane_tag_quaternion_xyzw",
|
"image_plane_tag_quaternion_xyzw",
|
||||||
|
"load_camera_extrinsics",
|
||||||
|
"matrix_payload",
|
||||||
"normalize_quaternion_xyzw",
|
"normalize_quaternion_xyzw",
|
||||||
"relative_quaternion_xyzw",
|
"relative_quaternion_xyzw",
|
||||||
"robust_rotation_summary",
|
"robust_rotation_summary",
|
||||||
"rotation_inlier_fraction",
|
"rotation_inlier_fraction",
|
||||||
"rotation_rms_rad",
|
"rotation_rms_rad",
|
||||||
"rotation_spread_rad",
|
"rotation_spread_rad",
|
||||||
|
"transform_matrix",
|
||||||
|
"validate_camera_extrinsics_payload",
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -0,0 +1,212 @@
|
|||||||
|
"""Camera-extrinsic data model shared by calibration and runtime nodes."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any, Mapping, Sequence
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import yaml
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
|
||||||
|
def camera_info_fingerprint(
|
||||||
|
*,
|
||||||
|
width: int,
|
||||||
|
height: int,
|
||||||
|
camera_matrix: Sequence[Sequence[float]] | Sequence[float],
|
||||||
|
distortion: Sequence[float] = (),
|
||||||
|
rectification: Sequence[float] = (),
|
||||||
|
projection: Sequence[float] = (),
|
||||||
|
) -> str:
|
||||||
|
"""Return a stable fingerprint for rectified image geometry."""
|
||||||
|
matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
|
||||||
|
payload = {
|
||||||
|
"width": int(width),
|
||||||
|
"height": int(height),
|
||||||
|
"camera_matrix": [round(float(value), 12) for value in matrix.flat],
|
||||||
|
"distortion": [round(float(value), 12) for value in distortion],
|
||||||
|
"rectification": [round(float(value), 12) for value in rectification],
|
||||||
|
"projection": [round(float(value), 12) for value in projection],
|
||||||
|
}
|
||||||
|
encoded = json.dumps(
|
||||||
|
payload, sort_keys=True, separators=(",", ":")
|
||||||
|
).encode("utf-8")
|
||||||
|
return hashlib.sha256(encoded).hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
def transform_matrix(
|
||||||
|
translation_xyz_m: Sequence[float],
|
||||||
|
quaternion_xyzw: Sequence[float],
|
||||||
|
) -> np.ndarray:
|
||||||
|
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||||
|
quaternion = np.asarray(quaternion_xyzw, dtype=float)
|
||||||
|
if translation.shape != (3,) or not np.all(np.isfinite(translation)):
|
||||||
|
raise ValueError("translation_xyz_m must contain three finite values")
|
||||||
|
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
||||||
|
raise ValueError("quaternion_xyzw must contain four finite values")
|
||||||
|
norm = float(np.linalg.norm(quaternion))
|
||||||
|
if norm < 1.0e-12:
|
||||||
|
raise ValueError("quaternion_xyzw has zero norm")
|
||||||
|
result = np.eye(4, dtype=float)
|
||||||
|
result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
|
||||||
|
result[:3, 3] = translation
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
|
||||||
|
value = np.asarray(matrix, dtype=float)
|
||||||
|
if value.shape != (4, 4) or not np.all(np.isfinite(value)):
|
||||||
|
raise ValueError("transform must be a finite 4x4 matrix")
|
||||||
|
return {
|
||||||
|
"translation_xyz_m": [float(item) for item in value[:3, 3]],
|
||||||
|
"quaternion_xyzw": [
|
||||||
|
float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
|
||||||
|
],
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CameraCalibrationIdentity:
|
||||||
|
serial_number: str
|
||||||
|
width: int
|
||||||
|
height: int
|
||||||
|
intrinsics_sha256: str
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CameraExtrinsics:
|
||||||
|
"""Transforms every declared camera into one Profile-selected reference."""
|
||||||
|
|
||||||
|
cameras: Mapping[str, CameraCalibrationIdentity]
|
||||||
|
reference_view: str
|
||||||
|
reference_from_view: Mapping[str, np.ndarray]
|
||||||
|
quality: Mapping[str, float]
|
||||||
|
|
||||||
|
def transform(self, view: str) -> np.ndarray:
|
||||||
|
if view not in self.reference_from_view:
|
||||||
|
raise KeyError(f"extrinsics do not contain view {view}")
|
||||||
|
return np.asarray(self.reference_from_view[view], dtype=float).copy()
|
||||||
|
|
||||||
|
|
||||||
|
def validate_camera_extrinsics_payload(
|
||||||
|
payload: Mapping[str, Any],
|
||||||
|
*,
|
||||||
|
required_views: Sequence[str],
|
||||||
|
reference_view: str,
|
||||||
|
quality_limits: Mapping[str, float] | None = None,
|
||||||
|
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||||
|
) -> None:
|
||||||
|
if int(payload.get("schema_version", -1)) != 1:
|
||||||
|
raise ValueError("camera extrinsics schema_version must be 1")
|
||||||
|
reference = str(reference_view)
|
||||||
|
if payload.get("reference_view") != reference:
|
||||||
|
raise ValueError(
|
||||||
|
"camera extrinsics reference_view differs from the Profile"
|
||||||
|
)
|
||||||
|
cameras = payload.get("cameras")
|
||||||
|
transforms = payload.get(f"{reference}_from_view")
|
||||||
|
quality = payload.get("quality")
|
||||||
|
views = tuple(str(view) for view in required_views)
|
||||||
|
if not views or len(set(views)) != len(views):
|
||||||
|
raise ValueError("required extrinsic views must be non-empty and unique")
|
||||||
|
if reference not in views:
|
||||||
|
raise ValueError("extrinsic reference view is not required")
|
||||||
|
if not isinstance(cameras, Mapping) or set(cameras) != set(views):
|
||||||
|
raise ValueError("camera extrinsics differ from the Profile views")
|
||||||
|
if not isinstance(transforms, Mapping) or set(transforms) != set(views):
|
||||||
|
raise ValueError("camera transforms differ from the Profile views")
|
||||||
|
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
|
||||||
|
raise ValueError("camera extrinsics quality is not passed")
|
||||||
|
for key, limit in dict(quality_limits or {}).items():
|
||||||
|
value = float(quality.get(key, float("inf")))
|
||||||
|
if not np.isfinite(value) or value > limit:
|
||||||
|
raise ValueError(
|
||||||
|
f"camera extrinsics {key}={value} exceeds {limit}"
|
||||||
|
)
|
||||||
|
for key, minimum in dict(minimum_capture_counts or {}).items():
|
||||||
|
if int(quality.get(key, 0)) < int(minimum):
|
||||||
|
raise ValueError(
|
||||||
|
f"camera extrinsics {key} must be at least {minimum}"
|
||||||
|
)
|
||||||
|
for view in views:
|
||||||
|
identity = cameras[view]
|
||||||
|
if not isinstance(identity, Mapping):
|
||||||
|
raise ValueError(f"{view} camera identity must be an object")
|
||||||
|
if not str(identity.get("serial_number", "")):
|
||||||
|
raise ValueError(f"{view} camera serial_number is missing")
|
||||||
|
if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
|
||||||
|
raise ValueError(f"{view} camera image dimensions are invalid")
|
||||||
|
fingerprint = str(identity.get("intrinsics_sha256", ""))
|
||||||
|
if len(fingerprint) != 64:
|
||||||
|
raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
|
||||||
|
transform = transforms[view]
|
||||||
|
if not isinstance(transform, Mapping):
|
||||||
|
raise ValueError(f"{view} transform must be an object")
|
||||||
|
matrix = transform_matrix(
|
||||||
|
transform.get("translation_xyz_m", ()),
|
||||||
|
transform.get("quaternion_xyzw", ()),
|
||||||
|
)
|
||||||
|
if view == reference and not np.allclose(
|
||||||
|
matrix, np.eye(4), atol=1.0e-9
|
||||||
|
):
|
||||||
|
raise ValueError("reference-view transform must be identity")
|
||||||
|
serials = [str(cameras[view]["serial_number"]) for view in views]
|
||||||
|
if len(set(serials)) != len(views):
|
||||||
|
raise ValueError("camera extrinsics serial numbers must be unique")
|
||||||
|
|
||||||
|
|
||||||
|
def load_camera_extrinsics(
|
||||||
|
path: str | Path,
|
||||||
|
*,
|
||||||
|
required_views: Sequence[str],
|
||||||
|
reference_view: str,
|
||||||
|
quality_limits: Mapping[str, float] | None = None,
|
||||||
|
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||||
|
) -> CameraExtrinsics:
|
||||||
|
source = Path(path).expanduser().resolve()
|
||||||
|
if not source.is_file():
|
||||||
|
raise ValueError(f"camera extrinsics file does not exist: {source}")
|
||||||
|
with source.open("r", encoding="utf-8") as stream:
|
||||||
|
payload = yaml.safe_load(stream)
|
||||||
|
if not isinstance(payload, Mapping):
|
||||||
|
raise ValueError("camera extrinsics file must contain an object")
|
||||||
|
validate_camera_extrinsics_payload(
|
||||||
|
payload,
|
||||||
|
required_views=required_views,
|
||||||
|
reference_view=reference_view,
|
||||||
|
quality_limits=quality_limits,
|
||||||
|
minimum_capture_counts=minimum_capture_counts,
|
||||||
|
)
|
||||||
|
views = tuple(str(view) for view in required_views)
|
||||||
|
transform_key = f"{reference_view}_from_view"
|
||||||
|
cameras = {
|
||||||
|
view: CameraCalibrationIdentity(
|
||||||
|
serial_number=str(payload["cameras"][view]["serial_number"]),
|
||||||
|
width=int(payload["cameras"][view]["width"]),
|
||||||
|
height=int(payload["cameras"][view]["height"]),
|
||||||
|
intrinsics_sha256=str(
|
||||||
|
payload["cameras"][view]["intrinsics_sha256"]
|
||||||
|
),
|
||||||
|
)
|
||||||
|
for view in views
|
||||||
|
}
|
||||||
|
transforms = {
|
||||||
|
view: transform_matrix(
|
||||||
|
payload[transform_key][view]["translation_xyz_m"],
|
||||||
|
payload[transform_key][view]["quaternion_xyzw"],
|
||||||
|
)
|
||||||
|
for view in views
|
||||||
|
}
|
||||||
|
return CameraExtrinsics(
|
||||||
|
cameras=cameras,
|
||||||
|
reference_view=str(reference_view),
|
||||||
|
reference_from_view=transforms,
|
||||||
|
quality={
|
||||||
|
str(key): float(value) if isinstance(value, (int, float)) else value
|
||||||
|
for key, value in payload["quality"].items()
|
||||||
|
},
|
||||||
|
)
|
||||||
@@ -1,83 +1,35 @@
|
|||||||
"""Camera-extrinsic data model shared by calibration and runtime nodes."""
|
"""Three-view compatibility policy over generic camera extrinsics."""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import hashlib
|
|
||||||
import json
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any, Mapping, Sequence
|
from typing import Any, Mapping, Sequence
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import yaml
|
import yaml
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
from .core.geometry.extrinsics import (
|
||||||
|
CameraCalibrationIdentity,
|
||||||
|
CameraExtrinsics,
|
||||||
|
camera_info_fingerprint,
|
||||||
|
load_camera_extrinsics,
|
||||||
|
matrix_payload,
|
||||||
|
transform_matrix,
|
||||||
|
validate_camera_extrinsics_payload,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
||||||
|
_QUALITY_LIMITS = {
|
||||||
|
"reprojection_rms_px": 1.2,
|
||||||
def camera_info_fingerprint(
|
"maximum_rotation_repeatability_deg": 0.3,
|
||||||
*,
|
"maximum_translation_repeatability_m": 0.0015,
|
||||||
width: int,
|
}
|
||||||
height: int,
|
_MINIMUM_CAPTURE_COUNTS = {
|
||||||
camera_matrix: Sequence[Sequence[float]] | Sequence[float],
|
"front_side_captures": 15,
|
||||||
distortion: Sequence[float] = (),
|
"front_top_captures": 15,
|
||||||
rectification: Sequence[float] = (),
|
}
|
||||||
projection: Sequence[float] = (),
|
|
||||||
) -> str:
|
|
||||||
"""Return a stable fingerprint for rectified image geometry."""
|
|
||||||
matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
|
|
||||||
payload = {
|
|
||||||
"width": int(width),
|
|
||||||
"height": int(height),
|
|
||||||
"camera_matrix": [round(float(value), 12) for value in matrix.flat],
|
|
||||||
"distortion": [round(float(value), 12) for value in distortion],
|
|
||||||
"rectification": [round(float(value), 12) for value in rectification],
|
|
||||||
"projection": [round(float(value), 12) for value in projection],
|
|
||||||
}
|
|
||||||
encoded = json.dumps(
|
|
||||||
payload, sort_keys=True, separators=(",", ":")
|
|
||||||
).encode("utf-8")
|
|
||||||
return hashlib.sha256(encoded).hexdigest()
|
|
||||||
|
|
||||||
|
|
||||||
def transform_matrix(
|
|
||||||
translation_xyz_m: Sequence[float],
|
|
||||||
quaternion_xyzw: Sequence[float],
|
|
||||||
) -> np.ndarray:
|
|
||||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
|
||||||
quaternion = np.asarray(quaternion_xyzw, dtype=float)
|
|
||||||
if translation.shape != (3,) or not np.all(np.isfinite(translation)):
|
|
||||||
raise ValueError("translation_xyz_m must contain three finite values")
|
|
||||||
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
|
||||||
raise ValueError("quaternion_xyzw must contain four finite values")
|
|
||||||
norm = float(np.linalg.norm(quaternion))
|
|
||||||
if norm < 1.0e-12:
|
|
||||||
raise ValueError("quaternion_xyzw has zero norm")
|
|
||||||
result = np.eye(4, dtype=float)
|
|
||||||
result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
|
|
||||||
result[:3, 3] = translation
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
|
|
||||||
value = np.asarray(matrix, dtype=float)
|
|
||||||
if value.shape != (4, 4) or not np.all(np.isfinite(value)):
|
|
||||||
raise ValueError("transform must be a finite 4x4 matrix")
|
|
||||||
return {
|
|
||||||
"translation_xyz_m": [float(item) for item in value[:3, 3]],
|
|
||||||
"quaternion_xyzw": [
|
|
||||||
float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
|
|
||||||
],
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
|
||||||
class CameraCalibrationIdentity:
|
|
||||||
serial_number: str
|
|
||||||
width: int
|
|
||||||
height: int
|
|
||||||
intrinsics_sha256: str
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -113,92 +65,27 @@ class ThreeCameraExtrinsics:
|
|||||||
|
|
||||||
|
|
||||||
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
||||||
if int(payload.get("schema_version", -1)) != 1:
|
validate_camera_extrinsics_payload(
|
||||||
raise ValueError("camera extrinsics schema_version must be 1")
|
payload,
|
||||||
if payload.get("reference_view") != "front":
|
required_views=VIEWS,
|
||||||
raise ValueError("camera extrinsics reference_view must be front")
|
reference_view="front",
|
||||||
cameras = payload.get("cameras")
|
quality_limits=_QUALITY_LIMITS,
|
||||||
transforms = payload.get("front_from_view")
|
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
|
||||||
quality = payload.get("quality")
|
)
|
||||||
if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
|
|
||||||
raise ValueError("camera extrinsics must contain front/side/top cameras")
|
|
||||||
if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
|
|
||||||
raise ValueError("camera extrinsics must contain all three transforms")
|
|
||||||
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
|
|
||||||
raise ValueError("camera extrinsics quality is not passed")
|
|
||||||
quality_limits = {
|
|
||||||
"reprojection_rms_px": 1.2,
|
|
||||||
"maximum_rotation_repeatability_deg": 0.3,
|
|
||||||
"maximum_translation_repeatability_m": 0.0015,
|
|
||||||
}
|
|
||||||
for key, limit in quality_limits.items():
|
|
||||||
value = float(quality.get(key, float("inf")))
|
|
||||||
if not np.isfinite(value) or value > limit:
|
|
||||||
raise ValueError(
|
|
||||||
f"camera extrinsics {key}={value} exceeds {limit}"
|
|
||||||
)
|
|
||||||
for key in ("front_side_captures", "front_top_captures"):
|
|
||||||
if int(quality.get(key, 0)) < 15:
|
|
||||||
raise ValueError(f"camera extrinsics {key} must be at least 15")
|
|
||||||
for view in VIEWS:
|
|
||||||
identity = cameras[view]
|
|
||||||
if not isinstance(identity, Mapping):
|
|
||||||
raise ValueError(f"{view} camera identity must be an object")
|
|
||||||
if not str(identity.get("serial_number", "")):
|
|
||||||
raise ValueError(f"{view} camera serial_number is missing")
|
|
||||||
if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
|
|
||||||
raise ValueError(f"{view} camera image dimensions are invalid")
|
|
||||||
fingerprint = str(identity.get("intrinsics_sha256", ""))
|
|
||||||
if len(fingerprint) != 64:
|
|
||||||
raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
|
|
||||||
transform = transforms[view]
|
|
||||||
if not isinstance(transform, Mapping):
|
|
||||||
raise ValueError(f"{view} transform must be an object")
|
|
||||||
matrix = transform_matrix(
|
|
||||||
transform.get("translation_xyz_m", ()),
|
|
||||||
transform.get("quaternion_xyzw", ()),
|
|
||||||
)
|
|
||||||
if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
|
|
||||||
raise ValueError("front_from_view.front must be identity")
|
|
||||||
serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
|
|
||||||
if len(set(serials)) != len(VIEWS):
|
|
||||||
raise ValueError("camera extrinsics serial numbers must be unique")
|
|
||||||
|
|
||||||
|
|
||||||
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
|
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
|
||||||
source = Path(path).expanduser().resolve()
|
generic = load_camera_extrinsics(
|
||||||
if not source.is_file():
|
path,
|
||||||
raise ValueError(f"camera extrinsics file does not exist: {source}")
|
required_views=VIEWS,
|
||||||
with source.open("r", encoding="utf-8") as stream:
|
reference_view="front",
|
||||||
payload = yaml.safe_load(stream)
|
quality_limits=_QUALITY_LIMITS,
|
||||||
if not isinstance(payload, Mapping):
|
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
|
||||||
raise ValueError("camera extrinsics file must contain an object")
|
)
|
||||||
validate_extrinsics_payload(payload)
|
|
||||||
cameras = {
|
|
||||||
view: CameraCalibrationIdentity(
|
|
||||||
serial_number=str(payload["cameras"][view]["serial_number"]),
|
|
||||||
width=int(payload["cameras"][view]["width"]),
|
|
||||||
height=int(payload["cameras"][view]["height"]),
|
|
||||||
intrinsics_sha256=str(
|
|
||||||
payload["cameras"][view]["intrinsics_sha256"]
|
|
||||||
),
|
|
||||||
)
|
|
||||||
for view in VIEWS
|
|
||||||
}
|
|
||||||
transforms = {
|
|
||||||
view: transform_matrix(
|
|
||||||
payload["front_from_view"][view]["translation_xyz_m"],
|
|
||||||
payload["front_from_view"][view]["quaternion_xyzw"],
|
|
||||||
)
|
|
||||||
for view in VIEWS
|
|
||||||
}
|
|
||||||
return ThreeCameraExtrinsics(
|
return ThreeCameraExtrinsics(
|
||||||
cameras=cameras,
|
cameras=generic.cameras,
|
||||||
front_from_view=transforms,
|
front_from_view=generic.reference_from_view,
|
||||||
quality={
|
quality=generic.quality,
|
||||||
str(key): float(value) if isinstance(value, (int, float)) else value
|
|
||||||
for key, value in payload["quality"].items()
|
|
||||||
},
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -225,3 +112,19 @@ def dump_three_camera_extrinsics(
|
|||||||
with temporary.open("w", encoding="utf-8") as stream:
|
with temporary.open("w", encoding="utf-8") as stream:
|
||||||
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
|
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
|
||||||
temporary.replace(destination)
|
temporary.replace(destination)
|
||||||
|
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
"CameraCalibrationIdentity",
|
||||||
|
"CameraExtrinsics",
|
||||||
|
"ThreeCameraExtrinsics",
|
||||||
|
"VIEWS",
|
||||||
|
"camera_info_fingerprint",
|
||||||
|
"dump_three_camera_extrinsics",
|
||||||
|
"load_camera_extrinsics",
|
||||||
|
"load_three_camera_extrinsics",
|
||||||
|
"matrix_payload",
|
||||||
|
"transform_matrix",
|
||||||
|
"validate_camera_extrinsics_payload",
|
||||||
|
"validate_extrinsics_payload",
|
||||||
|
]
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -17,6 +17,7 @@ from ...core import (
|
|||||||
VisionRigSpec,
|
VisionRigSpec,
|
||||||
ZeroSolvePolicy,
|
ZeroSolvePolicy,
|
||||||
)
|
)
|
||||||
|
from .profile import MIMIC_DERIVED_FINGER_DIPS
|
||||||
from ..registry import EngineBindings, RegisteredProfile
|
from ..registry import EngineBindings, RegisteredProfile
|
||||||
from .artifacts import build_artifact_policy
|
from .artifacts import build_artifact_policy
|
||||||
from .motion import (
|
from .motion import (
|
||||||
@@ -24,7 +25,6 @@ from .motion import (
|
|||||||
build_calibration_preparation_waypoints,
|
build_calibration_preparation_waypoints,
|
||||||
build_calibration_return_waypoints,
|
build_calibration_return_waypoints,
|
||||||
)
|
)
|
||||||
from .zero_policy import MIMIC_DERIVED_FINGER_DIPS
|
|
||||||
|
|
||||||
|
|
||||||
_HARD_THRESHOLD_KEYS = frozenset(
|
_HARD_THRESHOLD_KEYS = frozenset(
|
||||||
@@ -45,6 +45,12 @@ def _run_cli(args: list[str] | None = None) -> None:
|
|||||||
main(args)
|
main(args)
|
||||||
|
|
||||||
|
|
||||||
|
def _run_node(args: list[str] | None = None) -> None:
|
||||||
|
from .node import main
|
||||||
|
|
||||||
|
main(args)
|
||||||
|
|
||||||
|
|
||||||
def adapt_profile(
|
def adapt_profile(
|
||||||
*,
|
*,
|
||||||
key: ProfileKey,
|
key: ProfileKey,
|
||||||
@@ -127,6 +133,15 @@ def adapt_profile(
|
|||||||
views=views,
|
views=views,
|
||||||
common_frame="calibration_common",
|
common_frame="calibration_common",
|
||||||
extrinsic_reference_view=views[0].name,
|
extrinsic_reference_view=views[0].name,
|
||||||
|
extrinsics_quality_limits={
|
||||||
|
"reprojection_rms_px": 1.2,
|
||||||
|
"maximum_rotation_repeatability_deg": 0.3,
|
||||||
|
"maximum_translation_repeatability_m": 0.0015,
|
||||||
|
},
|
||||||
|
minimum_capture_counts={
|
||||||
|
"front_side_captures": 15,
|
||||||
|
"front_top_captures": 15,
|
||||||
|
},
|
||||||
),
|
),
|
||||||
motion=MotionPolicy(
|
motion=MotionPolicy(
|
||||||
tasks=tasks,
|
tasks=tasks,
|
||||||
@@ -196,5 +211,6 @@ def adapt_profile(
|
|||||||
preparation_waypoints=build_calibration_preparation_waypoints,
|
preparation_waypoints=build_calibration_preparation_waypoints,
|
||||||
return_waypoints=build_calibration_return_waypoints,
|
return_waypoints=build_calibration_return_waypoints,
|
||||||
cli_main=_run_cli,
|
cli_main=_run_cli,
|
||||||
|
node_main=_run_node,
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -21,6 +21,9 @@ def build_artifact_policy(
|
|||||||
}
|
}
|
||||||
),
|
),
|
||||||
session_compatibility_tokens=frozenset(compatibility_tokens),
|
session_compatibility_tokens=frozenset(compatibility_tokens),
|
||||||
|
publish_corrected_urdf=(
|
||||||
|
"urdf_zero_publication" in compatibility_tokens
|
||||||
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,144 @@
|
|||||||
|
"""Read-only regression checks for the reviewed hardware sessions."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
_PASS_SESSIONS = {
|
||||||
|
"20260830_181154": "full",
|
||||||
|
"20260831_141123": "thumb",
|
||||||
|
"20260831_163843": "thumb",
|
||||||
|
}
|
||||||
|
_FAIL_SESSIONS = {
|
||||||
|
"20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"),
|
||||||
|
"20260831_142322": ("VAL-QUALITY-501", "zero_model_validation_failed"),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _sha256_file(path: Path) -> str:
|
||||||
|
digest = hashlib.sha256()
|
||||||
|
with path.open("rb") as stream:
|
||||||
|
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||||
|
digest.update(chunk)
|
||||||
|
return digest.hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
def _summary(session: Path) -> dict[str, Any]:
|
||||||
|
value = json.loads(
|
||||||
|
(session / "calibration_summary_zh.json").read_text(encoding="utf-8")
|
||||||
|
)
|
||||||
|
if not isinstance(value, dict):
|
||||||
|
raise ValueError(f"invalid golden summary: {session}")
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def validate_golden_sessions(
|
||||||
|
serial_root: str | Path,
|
||||||
|
*,
|
||||||
|
replay_full_session: bool = True,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Prove PASS/FAIL, hashes, partial freezes, and failure non-publication."""
|
||||||
|
root = Path(serial_root).expanduser().resolve()
|
||||||
|
results: dict[str, Any] = {}
|
||||||
|
for session_id, scope in _PASS_SESSIONS.items():
|
||||||
|
session = root / session_id
|
||||||
|
summary = _summary(session)
|
||||||
|
if summary.get("result") != "PASS":
|
||||||
|
raise ValueError(f"golden PASS changed: {session_id}")
|
||||||
|
if summary.get("calibration_scope") != scope:
|
||||||
|
raise ValueError(f"golden scope changed: {session_id}")
|
||||||
|
artifacts = summary["artifacts"]
|
||||||
|
hashes = summary["hashes"]
|
||||||
|
calibration_json = session / artifacts["json"]
|
||||||
|
corrected_urdf = session / artifacts["urdf"]
|
||||||
|
actual_json_hash = _sha256_file(calibration_json)
|
||||||
|
actual_urdf_hash = _sha256_file(corrected_urdf)
|
||||||
|
if actual_json_hash != hashes["calibration_json_sha256"]:
|
||||||
|
raise ValueError(f"golden JSON hash changed: {session_id}")
|
||||||
|
if actual_urdf_hash != hashes["corrected_urdf_sha256"]:
|
||||||
|
raise ValueError(f"golden URDF hash changed: {session_id}")
|
||||||
|
if session_id == "20260831_141123" and len(
|
||||||
|
summary.get("preserved_certified_zero_joints", ())
|
||||||
|
) != 12:
|
||||||
|
raise ValueError("merged thumb no longer freezes all finger zeros")
|
||||||
|
if session_id == "20260831_163843":
|
||||||
|
payload = json.loads(calibration_json.read_text(encoding="utf-8"))
|
||||||
|
if payload.get("artifact_type") != (
|
||||||
|
"g20_right_standalone_thumb_calibration"
|
||||||
|
):
|
||||||
|
raise ValueError("standalone thumb artifact type changed")
|
||||||
|
results[session_id] = {
|
||||||
|
"result": "PASS",
|
||||||
|
"json_sha256": actual_json_hash,
|
||||||
|
"urdf_sha256": actual_urdf_hash,
|
||||||
|
}
|
||||||
|
|
||||||
|
for session_id, (error_code, reason) in _FAIL_SESSIONS.items():
|
||||||
|
session = root / session_id
|
||||||
|
summary = _summary(session)
|
||||||
|
if (
|
||||||
|
summary.get("result") != "FAIL"
|
||||||
|
or summary.get("error_code") != error_code
|
||||||
|
or summary.get("reason") != reason
|
||||||
|
):
|
||||||
|
raise ValueError(f"golden failure decision changed: {session_id}")
|
||||||
|
results[session_id] = {
|
||||||
|
"result": "FAIL",
|
||||||
|
"error_code": error_code,
|
||||||
|
"reason": reason,
|
||||||
|
}
|
||||||
|
|
||||||
|
failure_ids = set(_FAIL_SESSIONS)
|
||||||
|
for pointer_name in ("latest_passed", "latest_thumb_passed"):
|
||||||
|
pointer = root / pointer_name
|
||||||
|
if pointer.exists() and pointer.resolve().name in failure_ids:
|
||||||
|
raise ValueError(f"failure session was published through {pointer_name}")
|
||||||
|
|
||||||
|
if replay_full_session:
|
||||||
|
from .offline_replay import replay_session
|
||||||
|
|
||||||
|
session_id = "20260830_181154"
|
||||||
|
replay = replay_session(root / session_id, write_outputs=False)
|
||||||
|
expected = results[session_id]
|
||||||
|
if replay["computed_final_json_sha256"] != expected["json_sha256"]:
|
||||||
|
raise ValueError(
|
||||||
|
"full-session replay JSON is not byte-identical: "
|
||||||
|
f"computed={replay['computed_final_json_sha256']} "
|
||||||
|
f"expected={expected['json_sha256']}"
|
||||||
|
)
|
||||||
|
if replay["corrected_urdf_sha256"] != expected["urdf_sha256"]:
|
||||||
|
raise ValueError(
|
||||||
|
"full-session replay URDF is not byte-identical: "
|
||||||
|
f"computed={replay['corrected_urdf_sha256']} "
|
||||||
|
f"expected={expected['urdf_sha256']}"
|
||||||
|
)
|
||||||
|
results[session_id]["offline_replay"] = "byte_identical"
|
||||||
|
return results
|
||||||
|
|
||||||
|
|
||||||
|
def main(args: list[str] | None = None) -> None:
|
||||||
|
parser = argparse.ArgumentParser(
|
||||||
|
description="Validate the five reviewed calibration sessions"
|
||||||
|
)
|
||||||
|
parser.add_argument("serial_root")
|
||||||
|
parser.add_argument("--no-replay", action="store_true")
|
||||||
|
selected = parser.parse_args(args)
|
||||||
|
print(
|
||||||
|
json.dumps(
|
||||||
|
validate_golden_sessions(
|
||||||
|
selected.serial_root,
|
||||||
|
replay_full_session=not selected.no_replay,
|
||||||
|
),
|
||||||
|
ensure_ascii=False,
|
||||||
|
indent=2,
|
||||||
|
sort_keys=True,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -1,7 +1,7 @@
|
|||||||
"""One-release typed wrappers for the legacy 11-Tag layouts."""
|
"""One-release typed wrappers for the legacy 11-Tag layouts."""
|
||||||
|
|
||||||
from ...core import ProfileKey
|
from ...core import ProfileKey
|
||||||
from ...full_hand import get_hand_calibration_profile
|
from .profile import get_hand_calibration_profile
|
||||||
from ...urdf_zero import get_zero_calibration_profile
|
from ...urdf_zero import get_zero_calibration_profile
|
||||||
from ..registry import RegisteredProfile
|
from ..registry import RegisteredProfile
|
||||||
from ._adapter import adapt_profile
|
from ._adapter import adapt_profile
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
"""Reviewed motion and safe-waypoint strategy exports."""
|
"""Reviewed motion and safe-waypoint strategy exports."""
|
||||||
|
|
||||||
from ...full_hand import (
|
from .profile import (
|
||||||
build_calibration_motion_command,
|
build_calibration_motion_command,
|
||||||
build_calibration_preparation_waypoints,
|
build_calibration_preparation_waypoints,
|
||||||
build_calibration_return_waypoints,
|
build_calibration_return_waypoints,
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,914 @@
|
|||||||
|
"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from datetime import datetime, timezone
|
||||||
|
import copy
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
import os
|
||||||
|
from pathlib import Path
|
||||||
|
import re
|
||||||
|
from typing import Any, Mapping, Sequence
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
from .profile import (
|
||||||
|
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
||||||
|
G20_RIGHT_19_LAYOUT,
|
||||||
|
get_hand_calibration_profile,
|
||||||
|
validate_compact_payload,
|
||||||
|
)
|
||||||
|
from ...product import ProductConfig, sha256_file
|
||||||
|
from ...storage import atomic_write_json
|
||||||
|
from ...core.urdf import UrdfCorrectionPlan, build_correction_plan
|
||||||
|
from .zero_solver import (
|
||||||
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
|
get_zero_calibration_profile,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
PASSIVE_JOINTS = frozenset(
|
||||||
|
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
||||||
|
)
|
||||||
|
VISUALLY_MEASURED_FINGER_DIPS = frozenset(
|
||||||
|
{"index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
||||||
|
)
|
||||||
|
ACTIVE_ZERO_JOINTS = frozenset(
|
||||||
|
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
|
||||||
|
)
|
||||||
|
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
||||||
|
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
||||||
|
) - ACTIVE_ZERO_JOINTS
|
||||||
|
SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9
|
||||||
|
|
||||||
|
|
||||||
|
def _load_json(path: Path) -> dict[str, Any]:
|
||||||
|
with path.open("r", encoding="utf-8") as stream:
|
||||||
|
payload = json.load(stream)
|
||||||
|
if not isinstance(payload, dict):
|
||||||
|
raise ValueError(f"{path} must contain a JSON object")
|
||||||
|
return payload
|
||||||
|
|
||||||
|
|
||||||
|
def _joint_blocks(text: str) -> dict[str, str]:
|
||||||
|
pattern = re.compile(
|
||||||
|
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||||
|
re.DOTALL,
|
||||||
|
)
|
||||||
|
return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
|
||||||
|
|
||||||
|
|
||||||
|
def _mask_origin_rpy(block: str) -> str:
|
||||||
|
return re.sub(
|
||||||
|
r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
|
||||||
|
r"\1\2__CALIBRATED_RPY__\2",
|
||||||
|
block,
|
||||||
|
count=1,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _mask_active_origin_rpy_fields(text: str) -> str:
|
||||||
|
pattern = re.compile(
|
||||||
|
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||||
|
re.DOTALL,
|
||||||
|
)
|
||||||
|
|
||||||
|
def replace(match: re.Match[str]) -> str:
|
||||||
|
block = match.group(0)
|
||||||
|
if match.group("name") not in ACTIVE_ZERO_JOINTS:
|
||||||
|
return block
|
||||||
|
masked = _mask_origin_rpy(block)
|
||||||
|
if masked == block:
|
||||||
|
raise ValueError(
|
||||||
|
f"active joint {match.group('name')} has no origin.rpy"
|
||||||
|
)
|
||||||
|
return masked
|
||||||
|
|
||||||
|
return pattern.sub(replace, text)
|
||||||
|
|
||||||
|
|
||||||
|
def _mask_endpoint_coordinate_fields(text: str) -> str:
|
||||||
|
"""Mask only limit/mimic fields induced by endpoint zero coordinates."""
|
||||||
|
pattern = re.compile(
|
||||||
|
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||||
|
re.DOTALL,
|
||||||
|
)
|
||||||
|
|
||||||
|
def replace(match: re.Match[str]) -> str:
|
||||||
|
block = match.group(0)
|
||||||
|
name = match.group("name")
|
||||||
|
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS:
|
||||||
|
return re.sub(
|
||||||
|
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
|
||||||
|
r"\1\2__CALIBRATED_UPPER__\2",
|
||||||
|
block,
|
||||||
|
count=1,
|
||||||
|
)
|
||||||
|
mimic = re.search(
|
||||||
|
r"<mimic\b[^>]*\bjoint\s*=\s*([\"'])(?P<source>[^\"']+)\1[^>]*>",
|
||||||
|
block,
|
||||||
|
re.DOTALL,
|
||||||
|
)
|
||||||
|
if (
|
||||||
|
mimic is not None
|
||||||
|
and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||||
|
):
|
||||||
|
return re.sub(
|
||||||
|
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
|
||||||
|
r"\1\2__CALIBRATED_MIMIC_OFFSET__\2",
|
||||||
|
block,
|
||||||
|
count=1,
|
||||||
|
)
|
||||||
|
return block
|
||||||
|
|
||||||
|
return pattern.sub(replace, text)
|
||||||
|
|
||||||
|
|
||||||
|
def _triplet(value: str) -> np.ndarray:
|
||||||
|
result = np.asarray([float(item) for item in value.split()], dtype=float)
|
||||||
|
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
||||||
|
raise ValueError(f"invalid URDF triplet: {value}")
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
|
||||||
|
return {
|
||||||
|
str(joint.get("name")): joint
|
||||||
|
for joint in ET.parse(Path(path)).getroot().findall("joint")
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _verify_expected_origin_offsets(
|
||||||
|
source: str | Path,
|
||||||
|
corrected: str | Path,
|
||||||
|
expected_offsets_rad: Mapping[str, float],
|
||||||
|
) -> None:
|
||||||
|
offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
|
||||||
|
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
|
||||||
|
not math.isfinite(value) for value in offsets.values()
|
||||||
|
):
|
||||||
|
raise ValueError("expected offsets must contain all finite active static-zero values")
|
||||||
|
before = _joint_elements(source)
|
||||||
|
after = _joint_elements(corrected)
|
||||||
|
maximum_rotation_error = 0.0
|
||||||
|
for name, original_joint in before.items():
|
||||||
|
corrected_joint = after[name]
|
||||||
|
original_origin = original_joint.find("origin")
|
||||||
|
corrected_origin = corrected_joint.find("origin")
|
||||||
|
if original_origin is None or corrected_origin is None:
|
||||||
|
if original_origin is not corrected_origin:
|
||||||
|
raise ValueError(f"corrected URDF changed origin presence for {name}")
|
||||||
|
continue
|
||||||
|
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
|
||||||
|
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
|
||||||
|
if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
|
||||||
|
raise ValueError(f"corrected URDF changed origin.xyz for {name}")
|
||||||
|
original_rotation = Rotation.from_euler(
|
||||||
|
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
|
||||||
|
)
|
||||||
|
corrected_rotation = Rotation.from_euler(
|
||||||
|
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
|
||||||
|
)
|
||||||
|
expected_rotation = original_rotation
|
||||||
|
if name in offsets:
|
||||||
|
axis_node = original_joint.find("axis")
|
||||||
|
axis = _triplet(
|
||||||
|
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
|
||||||
|
)
|
||||||
|
norm = float(np.linalg.norm(axis))
|
||||||
|
if norm <= 1.0e-12:
|
||||||
|
raise ValueError(f"source URDF joint {name} has a degenerate axis")
|
||||||
|
expected_rotation = expected_rotation * Rotation.from_rotvec(
|
||||||
|
axis / norm * offsets[name]
|
||||||
|
)
|
||||||
|
error = float((expected_rotation.inv() * corrected_rotation).magnitude())
|
||||||
|
maximum_rotation_error = max(maximum_rotation_error, error)
|
||||||
|
# Schema v4 intentionally stores zero offsets at eight decimal places.
|
||||||
|
# Older sessions wrote the corrected URDF from the full-precision solve,
|
||||||
|
# so comparing that file with the published JSON necessarily permits half
|
||||||
|
# of one last-place unit. This is about 2.9e-7 degrees and is far below
|
||||||
|
# any calibration or URDF numerical significance.
|
||||||
|
if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD:
|
||||||
|
raise ValueError(
|
||||||
|
"corrected URDF origin.rpy does not match the published zero offsets"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def verify_corrected_urdf(
|
||||||
|
source: str | Path,
|
||||||
|
corrected: str | Path,
|
||||||
|
*,
|
||||||
|
expected_offsets_rad: Mapping[str, float] | None = None,
|
||||||
|
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||||
|
correction_plan: UrdfCorrectionPlan | None = None,
|
||||||
|
) -> tuple[str, ...]:
|
||||||
|
"""Prove that only active-joint origin.rpy attributes changed.
|
||||||
|
|
||||||
|
Passive joint blocks, including their mimic elements, are compared as raw
|
||||||
|
UTF-8 text so formatting and numeric spelling are protected as well.
|
||||||
|
"""
|
||||||
|
source_text = Path(source).read_text(encoding="utf-8")
|
||||||
|
corrected_text = Path(corrected).read_text(encoding="utf-8")
|
||||||
|
if correction_plan is not None:
|
||||||
|
correction_plan.verify_source(source)
|
||||||
|
if expected_offsets_rad is None:
|
||||||
|
raise ValueError(
|
||||||
|
"correction-plan validation requires expected offsets"
|
||||||
|
)
|
||||||
|
correction_plan.authorize_offsets(expected_offsets_rad)
|
||||||
|
before = _joint_blocks(source_text)
|
||||||
|
after = _joint_blocks(corrected_text)
|
||||||
|
if set(before) != set(after):
|
||||||
|
raise ValueError("corrected URDF changed the joint set")
|
||||||
|
# Text patchers may normalize only the final newline. It has no URDF/XML
|
||||||
|
# semantics; every joint block and every non-rpy byte remains guarded
|
||||||
|
# below.
|
||||||
|
masked_source = _mask_endpoint_coordinate_fields(
|
||||||
|
_mask_active_origin_rpy_fields(source_text)
|
||||||
|
).rstrip("\r\n")
|
||||||
|
masked_corrected = _mask_endpoint_coordinate_fields(
|
||||||
|
_mask_active_origin_rpy_fields(corrected_text)
|
||||||
|
).rstrip("\r\n")
|
||||||
|
if masked_source != masked_corrected:
|
||||||
|
raise ValueError(
|
||||||
|
"corrected URDF changed content outside active origin.rpy or "
|
||||||
|
"endpoint-calibrated fields"
|
||||||
|
)
|
||||||
|
changed: list[str] = []
|
||||||
|
for name in before:
|
||||||
|
if before[name] == after[name]:
|
||||||
|
continue
|
||||||
|
if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS:
|
||||||
|
raise ValueError(f"corrected URDF changed protected joint {name}")
|
||||||
|
if name in ACTIVE_ZERO_JOINTS:
|
||||||
|
changed.append(name)
|
||||||
|
if expected_offsets_rad is not None:
|
||||||
|
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
|
||||||
|
endpoint_offsets = {
|
||||||
|
str(name): float(value)
|
||||||
|
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
||||||
|
}
|
||||||
|
if endpoint_offsets:
|
||||||
|
if correction_plan is not None and not set(endpoint_offsets).issubset(
|
||||||
|
correction_plan.endpoint_limit_joints
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"endpoint offsets are not authorized by the correction plan"
|
||||||
|
)
|
||||||
|
source_joints = _joint_elements(source)
|
||||||
|
corrected_joints = _joint_elements(corrected)
|
||||||
|
for name, offset in endpoint_offsets.items():
|
||||||
|
original_limit = source_joints[name].find("limit")
|
||||||
|
corrected_limit = corrected_joints[name].find("limit")
|
||||||
|
expected_upper = float(original_limit.get("upper")) - offset
|
||||||
|
# Schema v4 stores the corresponding zero at eight decimal
|
||||||
|
# places, while the URDF is written from the full-precision solve.
|
||||||
|
# Match the half-last-place tolerance used for origin rotations.
|
||||||
|
if (
|
||||||
|
abs(float(corrected_limit.get("upper")) - expected_upper)
|
||||||
|
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||||
|
):
|
||||||
|
raise ValueError(f"corrected URDF has invalid {name} upper limit")
|
||||||
|
for name in PASSIVE_JOINTS:
|
||||||
|
original_mimic = source_joints[name].find("mimic")
|
||||||
|
corrected_mimic = corrected_joints[name].find("mimic")
|
||||||
|
if original_mimic is None or corrected_mimic is None:
|
||||||
|
continue
|
||||||
|
source_name = str(original_mimic.get("joint"))
|
||||||
|
multiplier = float(original_mimic.get("multiplier", "1"))
|
||||||
|
expected = float(original_mimic.get("offset", "0"))
|
||||||
|
expected += multiplier * endpoint_offsets.get(source_name, 0.0)
|
||||||
|
# The active endpoint offset comes from schema-v4 JSON rounded to
|
||||||
|
# eight decimal places, while the URDF mimic was written from the
|
||||||
|
# full-precision solve. Propagate exactly the same accepted
|
||||||
|
# quantization through the mimic multiplier; retain a much
|
||||||
|
# smaller allowance for XML decimal formatting itself.
|
||||||
|
tolerance = (
|
||||||
|
abs(multiplier)
|
||||||
|
* SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||||
|
+ 1.0e-12
|
||||||
|
)
|
||||||
|
if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance:
|
||||||
|
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
|
||||||
|
return tuple(sorted(changed))
|
||||||
|
|
||||||
|
|
||||||
|
def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
|
||||||
|
"""Return every session-local mesh after proving it resolves safely."""
|
||||||
|
path = Path(urdf).expanduser().resolve()
|
||||||
|
root = ET.parse(path).getroot()
|
||||||
|
resources: dict[str, Path] = {}
|
||||||
|
for mesh in root.findall(".//mesh"):
|
||||||
|
filename = str(mesh.get("filename", "")).strip()
|
||||||
|
if not filename or "://" in filename or filename.startswith("package:"):
|
||||||
|
continue
|
||||||
|
relative = Path(filename)
|
||||||
|
if relative.is_absolute() or ".." in relative.parts:
|
||||||
|
raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
|
||||||
|
resolved = (path.parent / relative).resolve()
|
||||||
|
try:
|
||||||
|
resolved.relative_to(path.parent)
|
||||||
|
except ValueError as error:
|
||||||
|
raise ValueError(f"URDF mesh escapes the session: {filename}") from error
|
||||||
|
if not resolved.is_file():
|
||||||
|
raise ValueError(f"URDF mesh resource is missing: {filename}")
|
||||||
|
resources[relative.as_posix()] = resolved
|
||||||
|
return dict(sorted(resources.items()))
|
||||||
|
|
||||||
|
|
||||||
|
def validate_runtime_curves_against_urdf_limits(
|
||||||
|
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||||
|
) -> None:
|
||||||
|
"""Reject a curve whose q leaves its runtime URDF coordinate limits.
|
||||||
|
|
||||||
|
Curves in schema v4 are expressed in the corrected URDF joint coordinate,
|
||||||
|
not in the source-CAD coordinate. Endpoint zero calibration can therefore
|
||||||
|
move a corrected coordinate limit while preserving the same physical CAD
|
||||||
|
endpoint; callers publishing a calibrated pair must pass that corrected
|
||||||
|
URDF here.
|
||||||
|
"""
|
||||||
|
validate_compact_payload(payload)
|
||||||
|
runtime_joints = _joint_elements(runtime_urdf)
|
||||||
|
for name, calibration in payload["joints"].items():
|
||||||
|
joint = runtime_joints.get(str(name))
|
||||||
|
if joint is None:
|
||||||
|
raise ValueError(f"runtime URDF is missing joint {name}")
|
||||||
|
limit = joint.find("limit")
|
||||||
|
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
|
||||||
|
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||||
|
lower = float(limit.get("lower"))
|
||||||
|
upper = float(limit.get("upper"))
|
||||||
|
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||||
|
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||||
|
curve = np.asarray(calibration["angle_rad"], dtype=float)
|
||||||
|
minimum = float(np.min(curve))
|
||||||
|
maximum = float(np.max(curve))
|
||||||
|
tolerance = 1.0e-7
|
||||||
|
if minimum < lower - tolerance or maximum > upper + tolerance:
|
||||||
|
raise ValueError(
|
||||||
|
f"runtime curve exceeds runtime URDF limit for {name}: "
|
||||||
|
f"[{minimum:.9g}, {maximum:.9g}] not within "
|
||||||
|
f"[{lower:.9g}, {upper:.9g}]"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def clamp_compact_payload_to_urdf_limits(
|
||||||
|
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||||
|
) -> tuple[dict[str, Any], dict[str, int]]:
|
||||||
|
"""Return a schema-preserving payload bounded in its runtime coordinates."""
|
||||||
|
result = copy.deepcopy(dict(payload))
|
||||||
|
validate_compact_payload(result)
|
||||||
|
runtime_joints = _joint_elements(runtime_urdf)
|
||||||
|
clipped_by_joint: dict[str, int] = {}
|
||||||
|
for name, calibration in result["joints"].items():
|
||||||
|
joint = runtime_joints.get(str(name))
|
||||||
|
limit = None if joint is None else joint.find("limit")
|
||||||
|
if (
|
||||||
|
limit is None
|
||||||
|
or limit.get("lower") is None
|
||||||
|
or limit.get("upper") is None
|
||||||
|
):
|
||||||
|
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||||
|
lower = float(limit.get("lower"))
|
||||||
|
upper = float(limit.get("upper"))
|
||||||
|
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||||
|
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||||
|
source = np.asarray(calibration["angle_rad"], dtype=float)
|
||||||
|
bounded = np.clip(source, lower, upper)
|
||||||
|
count = int(np.count_nonzero(bounded != source))
|
||||||
|
if count:
|
||||||
|
clipped_by_joint[str(name)] = count
|
||||||
|
calibration["angle_rad"] = [
|
||||||
|
round(float(value), 8) for value in bounded
|
||||||
|
]
|
||||||
|
validate_compact_payload(result)
|
||||||
|
return result, clipped_by_joint
|
||||||
|
|
||||||
|
|
||||||
|
def build_mujoco_validation_commands(
|
||||||
|
baseline: Sequence[int],
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
if len(baseline) != 20:
|
||||||
|
raise ValueError("baseline must contain exactly 20 commands")
|
||||||
|
base = [int(value) for value in baseline]
|
||||||
|
if any(not 0 <= value <= 255 for value in base):
|
||||||
|
raise ValueError("baseline commands must be in [0, 255]")
|
||||||
|
|
||||||
|
def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
|
||||||
|
values = list(base)
|
||||||
|
for index, value in changes.items():
|
||||||
|
values[int(index)] = int(value)
|
||||||
|
return {"name": name, "label_zh": label, "command_u8": values}
|
||||||
|
|
||||||
|
return {
|
||||||
|
"schema_version": 1,
|
||||||
|
"topic": "/g20/cb_right_hand_control_cmd",
|
||||||
|
"serial_number": "G20_RIGHT_001",
|
||||||
|
"poses": [
|
||||||
|
pose("all_open", "全开", {}),
|
||||||
|
pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
|
||||||
|
pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
|
||||||
|
pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
|
||||||
|
pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
|
||||||
|
pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
|
||||||
|
pose(
|
||||||
|
"half_grip",
|
||||||
|
"四指半握",
|
||||||
|
{1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
|
||||||
|
),
|
||||||
|
pose(
|
||||||
|
"light_pinch",
|
||||||
|
"轻捏",
|
||||||
|
{0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
|
||||||
|
),
|
||||||
|
],
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
|
||||||
|
parent = Path(root).resolve()
|
||||||
|
target = Path(session).resolve()
|
||||||
|
if target.parent != parent:
|
||||||
|
raise ValueError("session pointer target must be a direct child of the serial root")
|
||||||
|
if not target.is_dir():
|
||||||
|
raise ValueError(f"session directory does not exist: {target}")
|
||||||
|
if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}:
|
||||||
|
raise ValueError("unsupported session pointer name")
|
||||||
|
parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
destination = parent / name
|
||||||
|
temporary = parent / f".{name}.{os.getpid()}.tmp"
|
||||||
|
if temporary.is_symlink():
|
||||||
|
temporary.unlink()
|
||||||
|
elif temporary.exists():
|
||||||
|
raise ValueError(f"temporary pointer path is occupied: {temporary}")
|
||||||
|
os.symlink(target.name, temporary, target_is_directory=True)
|
||||||
|
os.replace(temporary, destination)
|
||||||
|
return destination
|
||||||
|
|
||||||
|
|
||||||
|
def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||||
|
validate_compact_payload(payload)
|
||||||
|
result = {
|
||||||
|
name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||||
|
for name in ACTIVE_ZERO_JOINTS
|
||||||
|
}
|
||||||
|
if set(result) != ACTIVE_ZERO_JOINTS or any(
|
||||||
|
not math.isfinite(value) for value in result.values()
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"payload does not contain all finite observable active zero offsets"
|
||||||
|
)
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||||
|
"""Validate a standalone thumb artifact and expand CAD finger zeros."""
|
||||||
|
if (
|
||||||
|
payload.get("schema_version") != 1
|
||||||
|
or payload.get("artifact_type")
|
||||||
|
!= "g20_right_standalone_thumb_calibration"
|
||||||
|
or payload.get("side") != "right"
|
||||||
|
or payload.get("non_thumb_zero_policy") != "source_cad_unchanged"
|
||||||
|
or not bool(payload.get("quality", {}).get("passed"))
|
||||||
|
):
|
||||||
|
raise ValueError("standalone thumb JSON contract is invalid")
|
||||||
|
joints = payload.get("joints")
|
||||||
|
expected_joint_names = {
|
||||||
|
"thumb_cmc_roll",
|
||||||
|
"thumb_cmc_yaw",
|
||||||
|
"thumb_cmc_pitch",
|
||||||
|
"thumb_mcp",
|
||||||
|
"thumb_ip",
|
||||||
|
}
|
||||||
|
if not isinstance(joints, Mapping) or set(joints) != expected_joint_names:
|
||||||
|
raise ValueError("standalone thumb JSON has the wrong joint set")
|
||||||
|
thumb_names = expected_joint_names - {"thumb_ip"}
|
||||||
|
result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS}
|
||||||
|
for name in thumb_names:
|
||||||
|
try:
|
||||||
|
value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||||
|
except (KeyError, TypeError, ValueError) as error:
|
||||||
|
raise ValueError(
|
||||||
|
f"standalone thumb JSON is missing zero for {name}"
|
||||||
|
) from error
|
||||||
|
curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float)
|
||||||
|
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||||
|
raise ValueError(f"standalone thumb JSON curve is invalid: {name}")
|
||||||
|
result[name] = value
|
||||||
|
passive_curve = np.asarray(
|
||||||
|
joints["thumb_ip"].get("angle_rad", ()), dtype=float
|
||||||
|
)
|
||||||
|
if (
|
||||||
|
passive_curve.shape != (256,)
|
||||||
|
or not np.all(np.isfinite(passive_curve))
|
||||||
|
or not bool(joints["thumb_ip"].get("passive"))
|
||||||
|
or any(not math.isfinite(value) for value in result.values())
|
||||||
|
):
|
||||||
|
raise ValueError("standalone thumb JSON passive curve is invalid")
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def verify_partial_scope_preserves_certified_zeros(
|
||||||
|
*,
|
||||||
|
scope: str,
|
||||||
|
source_session: str | Path,
|
||||||
|
serial_root: str | Path | None = None,
|
||||||
|
serial_number: str,
|
||||||
|
current_offsets: Mapping[str, float],
|
||||||
|
) -> None:
|
||||||
|
"""Reject a partial artifact that changes any certified non-target zero."""
|
||||||
|
selected = str(scope).strip().lower()
|
||||||
|
if selected not in {"thumb", "fingers"}:
|
||||||
|
return
|
||||||
|
source = Path(source_session).expanduser()
|
||||||
|
if not source.is_absolute():
|
||||||
|
if serial_root is None:
|
||||||
|
raise ValueError(
|
||||||
|
"relative partial base session requires its serial root"
|
||||||
|
)
|
||||||
|
source = Path(serial_root).expanduser() / source
|
||||||
|
source = source.resolve()
|
||||||
|
base_payload = _load_json(
|
||||||
|
source / f"g20_right_{serial_number}_calibration.json"
|
||||||
|
)
|
||||||
|
base_offsets = (
|
||||||
|
standalone_thumb_offsets(base_payload)
|
||||||
|
if base_payload.get("artifact_type")
|
||||||
|
== "g20_right_standalone_thumb_calibration"
|
||||||
|
else active_offsets(base_payload)
|
||||||
|
)
|
||||||
|
preserve_thumb = selected == "fingers"
|
||||||
|
changed = {
|
||||||
|
name: abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||||
|
for name in ACTIVE_ZERO_JOINTS
|
||||||
|
if name.startswith("thumb_") == preserve_thumb
|
||||||
|
and abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||||
|
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||||
|
}
|
||||||
|
if changed:
|
||||||
|
raise ValueError(
|
||||||
|
f"partial {selected} calibration changed certified non-target "
|
||||||
|
"zeros: "
|
||||||
|
+ ",".join(sorted(changed))
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def compare_session_offsets(
|
||||||
|
first: Mapping[str, Any],
|
||||||
|
second: Mapping[str, Any],
|
||||||
|
*,
|
||||||
|
maximum_difference_rad: float,
|
||||||
|
) -> dict[str, float]:
|
||||||
|
first_standalone = (
|
||||||
|
first.get("artifact_type")
|
||||||
|
== "g20_right_standalone_thumb_calibration"
|
||||||
|
)
|
||||||
|
second_standalone = (
|
||||||
|
second.get("artifact_type")
|
||||||
|
== "g20_right_standalone_thumb_calibration"
|
||||||
|
)
|
||||||
|
if first_standalone != second_standalone:
|
||||||
|
raise ValueError("cannot compare standalone thumb and full-hand artifacts")
|
||||||
|
left = (
|
||||||
|
standalone_thumb_offsets(first)
|
||||||
|
if first_standalone
|
||||||
|
else active_offsets(first)
|
||||||
|
)
|
||||||
|
right = (
|
||||||
|
standalone_thumb_offsets(second)
|
||||||
|
if second_standalone
|
||||||
|
else active_offsets(second)
|
||||||
|
)
|
||||||
|
differences = {
|
||||||
|
name: abs(left[name] - right[name])
|
||||||
|
for name in sorted(left)
|
||||||
|
}
|
||||||
|
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
|
||||||
|
if failed:
|
||||||
|
details = ", ".join(
|
||||||
|
f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
|
||||||
|
)
|
||||||
|
raise ValueError(f"independent-session static-zero mismatch: {details}")
|
||||||
|
return differences
|
||||||
|
|
||||||
|
|
||||||
|
def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
|
||||||
|
directory = Path(session).resolve()
|
||||||
|
urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
|
||||||
|
if len(urdfs) != 1:
|
||||||
|
raise ValueError("session must contain exactly one corrected URDF")
|
||||||
|
return {
|
||||||
|
"json": directory / f"g20_right_{serial_number}_calibration.json",
|
||||||
|
"urdf": urdfs[0],
|
||||||
|
"summary": directory / "calibration_summary_zh.json",
|
||||||
|
"commands": directory / "mujoco_validation_commands.json",
|
||||||
|
"raw": directory / "raw_samples.jsonl",
|
||||||
|
"log": directory / "calibration.log",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def find_compatible_prior_session(
|
||||||
|
config: ProductConfig,
|
||||||
|
current: str | Path,
|
||||||
|
current_payload: Mapping[str, Any],
|
||||||
|
) -> tuple[Path | None, dict[str, float]]:
|
||||||
|
current_path = Path(current).resolve()
|
||||||
|
for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
|
||||||
|
if candidate.resolve() == current_path:
|
||||||
|
continue
|
||||||
|
json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
|
||||||
|
summary_path = candidate / "calibration_summary_zh.json"
|
||||||
|
if not json_path.is_file() or not summary_path.is_file():
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
summary = _load_json(summary_path)
|
||||||
|
if not bool(summary.get("quality", {}).get("passed")):
|
||||||
|
continue
|
||||||
|
payload = _load_json(json_path)
|
||||||
|
differences = compare_session_offsets(
|
||||||
|
payload,
|
||||||
|
current_payload,
|
||||||
|
maximum_difference_rad=config.static_repeatability_rad,
|
||||||
|
)
|
||||||
|
except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
|
||||||
|
continue
|
||||||
|
return candidate, differences
|
||||||
|
return None, {}
|
||||||
|
|
||||||
|
|
||||||
|
def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
|
||||||
|
# The formal product uses the independently held-out fourth sweep cycle.
|
||||||
|
# The optional eight-pose check remains a developer diagnostic because the
|
||||||
|
# axis-line zero solve does not establish an absolute Cartesian hand-base
|
||||||
|
# transform. When explicitly enabled, retain its strict coverage and
|
||||||
|
# error checks.
|
||||||
|
if not bool(combination.get("enabled")):
|
||||||
|
return
|
||||||
|
position_p95 = float(combination.get("position_p95_m", float("inf")))
|
||||||
|
orientation_p95 = float(
|
||||||
|
combination.get("orientation_p95_rad", float("inf"))
|
||||||
|
)
|
||||||
|
required = {str(value) for value in combination.get("required_targets", ())}
|
||||||
|
expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
|
||||||
|
observations = combination.get("observation_counts")
|
||||||
|
validations = combination.get("validation_counts")
|
||||||
|
if required != expected:
|
||||||
|
raise ValueError("combination validation has the wrong required-target set")
|
||||||
|
if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
|
||||||
|
raise ValueError("combination validation is missing per-target coverage")
|
||||||
|
missing_observations = sorted(
|
||||||
|
key for key in expected if int(observations.get(key, 0)) < 2
|
||||||
|
)
|
||||||
|
missing_validations = sorted(
|
||||||
|
key for key in expected if int(validations.get(key, 0)) < 1
|
||||||
|
)
|
||||||
|
if missing_observations or missing_validations:
|
||||||
|
raise ValueError(
|
||||||
|
"combination validation target coverage is incomplete: "
|
||||||
|
f"observations={','.join(missing_observations) or '-'}; "
|
||||||
|
f"validations={','.join(missing_validations) or '-'}"
|
||||||
|
)
|
||||||
|
if (
|
||||||
|
not bool(combination.get("completed"))
|
||||||
|
or int(combination.get("completed_poses", 0)) != 8
|
||||||
|
or position_p95 > 0.003
|
||||||
|
or orientation_p95 > math.radians(2.0)
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"eight-pose four-finger combination validation is incomplete or failed"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def finalize_session_artifacts(
|
||||||
|
config: ProductConfig,
|
||||||
|
session: str | Path,
|
||||||
|
*,
|
||||||
|
node_status: Mapping[str, Any],
|
||||||
|
) -> tuple[dict[str, Any], bool]:
|
||||||
|
"""Create diagnostics and update latest_passed only after every guard."""
|
||||||
|
directory = Path(session).resolve()
|
||||||
|
paths = session_artifact_paths(directory, config.serial_number)
|
||||||
|
for name in ("json", "urdf", "raw", "log"):
|
||||||
|
if not paths[name].is_file():
|
||||||
|
raise ValueError(f"session is missing {name}: {paths[name]}")
|
||||||
|
payload = _load_json(paths["json"])
|
||||||
|
standalone_thumb = bool(
|
||||||
|
payload.get("artifact_type")
|
||||||
|
== "g20_right_standalone_thumb_calibration"
|
||||||
|
)
|
||||||
|
if standalone_thumb:
|
||||||
|
standalone_thumb_offsets(payload)
|
||||||
|
else:
|
||||||
|
validate_compact_payload(payload)
|
||||||
|
if payload.get("schema_version") != 4 or payload.get("side") != "right":
|
||||||
|
raise ValueError("runtime JSON is not the compact right-hand schema v4")
|
||||||
|
if not bool(payload.get("quality", {}).get("passed")):
|
||||||
|
raise ValueError("runtime JSON quality is not passed")
|
||||||
|
combination = node_status.get("combination_validation")
|
||||||
|
if not isinstance(combination, Mapping):
|
||||||
|
raise ValueError("node status is missing combination validation")
|
||||||
|
_verify_combination_validation(combination)
|
||||||
|
resume = node_status.get("resume", {})
|
||||||
|
if not isinstance(resume, Mapping):
|
||||||
|
raise ValueError("node status has invalid resume provenance")
|
||||||
|
calibration_scope = str(
|
||||||
|
resume.get("recalibration_scope", "full")
|
||||||
|
).strip().lower()
|
||||||
|
if calibration_scope not in {"full", "thumb", "fingers"}:
|
||||||
|
raise ValueError("node status has an unsupported recalibration scope")
|
||||||
|
recalibration_tasks = tuple(
|
||||||
|
str(value) for value in resume.get("recalibration_task_keys", ())
|
||||||
|
)
|
||||||
|
if calibration_scope == "thumb" and not standalone_thumb and (
|
||||||
|
not bool(resume.get("used"))
|
||||||
|
or not str(resume.get("source_session", ""))
|
||||||
|
or len(recalibration_tasks) != 4
|
||||||
|
or any("thumb_" not in name for name in recalibration_tasks)
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"thumb recalibration is missing its passed base-session provenance"
|
||||||
|
)
|
||||||
|
if calibration_scope == "fingers" and (
|
||||||
|
not bool(resume.get("used"))
|
||||||
|
or not str(resume.get("source_session", ""))
|
||||||
|
or len(recalibration_tasks) != 12
|
||||||
|
or any("thumb_" in name for name in recalibration_tasks)
|
||||||
|
):
|
||||||
|
raise ValueError(
|
||||||
|
"finger recalibration is missing its certified thumb-session "
|
||||||
|
"provenance"
|
||||||
|
)
|
||||||
|
if standalone_thumb and calibration_scope != "thumb":
|
||||||
|
raise ValueError("standalone thumb artifact has the wrong node scope")
|
||||||
|
offsets = (
|
||||||
|
standalone_thumb_offsets(payload)
|
||||||
|
if standalone_thumb
|
||||||
|
else active_offsets(payload)
|
||||||
|
)
|
||||||
|
if calibration_scope != "full" and not standalone_thumb:
|
||||||
|
verify_partial_scope_preserves_certified_zeros(
|
||||||
|
scope=calibration_scope,
|
||||||
|
source_session=str(resume.get("source_session", "")),
|
||||||
|
serial_root=directory.parent,
|
||||||
|
serial_number=config.serial_number,
|
||||||
|
current_offsets=offsets,
|
||||||
|
)
|
||||||
|
endpoint_offsets = {
|
||||||
|
name: offsets[name]
|
||||||
|
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||||
|
}
|
||||||
|
typed_profile = config.calibration_contract.typed_profile
|
||||||
|
frozen_names = typed_profile.scope.frozen_joints[calibration_scope]
|
||||||
|
correction_plan = build_correction_plan(
|
||||||
|
typed_profile,
|
||||||
|
source_sha256=config.source_urdf_sha256,
|
||||||
|
scope=calibration_scope,
|
||||||
|
frozen_offsets_rad={name: offsets[name] for name in frozen_names},
|
||||||
|
)
|
||||||
|
changed_joints = verify_corrected_urdf(
|
||||||
|
config.source_urdf,
|
||||||
|
paths["urdf"],
|
||||||
|
expected_offsets_rad=offsets,
|
||||||
|
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||||
|
correction_plan=correction_plan,
|
||||||
|
)
|
||||||
|
if standalone_thumb:
|
||||||
|
clipped_runtime_joints = {}
|
||||||
|
else:
|
||||||
|
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
||||||
|
payload, paths["urdf"]
|
||||||
|
)
|
||||||
|
if clipped_runtime_joints:
|
||||||
|
atomic_write_json(paths["json"], payload)
|
||||||
|
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||||
|
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
|
||||||
|
mesh_hashes = {
|
||||||
|
name: sha256_file(path) for name, path in mesh_resources.items()
|
||||||
|
}
|
||||||
|
commands = (
|
||||||
|
{
|
||||||
|
"artifact_type": "standalone_thumb_no_full_hand_validation",
|
||||||
|
"poses": [],
|
||||||
|
}
|
||||||
|
if standalone_thumb
|
||||||
|
else build_mujoco_validation_commands(payload["baseline_command_u8"])
|
||||||
|
)
|
||||||
|
atomic_write_json(paths["commands"], commands)
|
||||||
|
|
||||||
|
prior, differences = find_compatible_prior_session(config, directory, payload)
|
||||||
|
release_ready = config.required_independent_passes == 1 or prior is not None
|
||||||
|
quality = dict(payload["quality"])
|
||||||
|
preserved_certified_zeros = sorted(
|
||||||
|
name
|
||||||
|
for name in ACTIVE_ZERO_JOINTS
|
||||||
|
if (
|
||||||
|
calibration_scope == "thumb"
|
||||||
|
and not name.startswith("thumb_")
|
||||||
|
)
|
||||||
|
or (
|
||||||
|
calibration_scope == "fingers"
|
||||||
|
and name.startswith("thumb_")
|
||||||
|
)
|
||||||
|
)
|
||||||
|
if standalone_thumb:
|
||||||
|
preserved_certified_zeros = []
|
||||||
|
calibrated_zero_joints = (
|
||||||
|
sorted(
|
||||||
|
name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_")
|
||||||
|
)
|
||||||
|
if standalone_thumb
|
||||||
|
else sorted(ACTIVE_ZERO_JOINTS)
|
||||||
|
)
|
||||||
|
summary: dict[str, Any] = {
|
||||||
|
"schema_version": 1,
|
||||||
|
"serial_number": config.serial_number,
|
||||||
|
"session_id": f"{config.serial_number}_{directory.name}",
|
||||||
|
"calibration_scope": calibration_scope,
|
||||||
|
"inherited_base_session": (
|
||||||
|
None
|
||||||
|
if calibration_scope == "full" or standalone_thumb
|
||||||
|
else str(resume.get("source_session"))
|
||||||
|
),
|
||||||
|
"freshly_calibrated_task_keys": list(recalibration_tasks),
|
||||||
|
"preserved_certified_zero_joints": preserved_certified_zeros,
|
||||||
|
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
|
||||||
|
"quality": quality,
|
||||||
|
"runtime_limit_clipped_bins": clipped_runtime_joints,
|
||||||
|
"runtime_curve_domain": (
|
||||||
|
"thumb_diagnostic_only"
|
||||||
|
if standalone_thumb
|
||||||
|
else "requested_command_u8"
|
||||||
|
),
|
||||||
|
"static_zero_calibrated_joints": calibrated_zero_joints,
|
||||||
|
"non_thumb_zero_policy": (
|
||||||
|
"source_cad_unchanged" if standalone_thumb else None
|
||||||
|
),
|
||||||
|
"retained_active_urdf_zero_joints": sorted(
|
||||||
|
RETAINED_ACTIVE_ZERO_JOINTS
|
||||||
|
),
|
||||||
|
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
|
||||||
|
"mimic_derived_dynamic_joints": [],
|
||||||
|
"visually_measured_passive_joints": sorted(
|
||||||
|
VISUALLY_MEASURED_FINGER_DIPS
|
||||||
|
),
|
||||||
|
"changed_urdf_joint_origins": list(changed_joints),
|
||||||
|
"hashes": {
|
||||||
|
"source_urdf_sha256": config.source_urdf_sha256,
|
||||||
|
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||||
|
"calibration_config_sha256": config.calibration_config_sha256,
|
||||||
|
"corrected_urdf_sha256": sha256_file(paths["urdf"]),
|
||||||
|
"mesh_resources_sha256": mesh_hashes,
|
||||||
|
"calibration_json_sha256": sha256_file(paths["json"]),
|
||||||
|
"raw_samples_sha256": sha256_file(paths["raw"]),
|
||||||
|
},
|
||||||
|
"holdout": {
|
||||||
|
"training_cycles": [0, 1, 2],
|
||||||
|
"validation_cycle": 3,
|
||||||
|
"isolated": True,
|
||||||
|
},
|
||||||
|
"combination_validation": dict(combination),
|
||||||
|
"formal_release": {
|
||||||
|
"passed": release_ready,
|
||||||
|
"required_independent_passes": config.required_independent_passes,
|
||||||
|
"comparison_session": None if prior is None else prior.name,
|
||||||
|
"maximum_static_difference_deg": (
|
||||||
|
None if not differences else math.degrees(max(differences.values()))
|
||||||
|
),
|
||||||
|
},
|
||||||
|
"node_status": dict(node_status),
|
||||||
|
"created_at_utc": datetime.now(timezone.utc).isoformat(),
|
||||||
|
"artifacts": {name: path.name for name, path in paths.items()},
|
||||||
|
}
|
||||||
|
atomic_write_json(paths["summary"], summary)
|
||||||
|
# Recompute hashes after all files exist and validate the pair once more
|
||||||
|
# immediately before the one atomic publication operation.
|
||||||
|
if sha256_file(config.source_urdf) != config.source_urdf_sha256:
|
||||||
|
raise ValueError("source URDF changed during calibration")
|
||||||
|
verify_corrected_urdf(
|
||||||
|
config.source_urdf,
|
||||||
|
paths["urdf"],
|
||||||
|
expected_offsets_rad=offsets,
|
||||||
|
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||||
|
)
|
||||||
|
# Revalidate the same coordinate contract immediately before publication:
|
||||||
|
# verify_corrected_urdf proves that corrected endpoint limits map back to
|
||||||
|
# the original physical CAD endpoints, while the runtime curves must stay
|
||||||
|
# inside those corrected-coordinate limits.
|
||||||
|
if not standalone_thumb:
|
||||||
|
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||||
|
final_mesh_hashes = {
|
||||||
|
name: sha256_file(path)
|
||||||
|
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
||||||
|
}
|
||||||
|
if final_mesh_hashes != mesh_hashes:
|
||||||
|
raise ValueError("URDF mesh resources changed during publication")
|
||||||
|
if release_ready:
|
||||||
|
atomic_session_pointer(
|
||||||
|
config.session_root,
|
||||||
|
"latest_thumb_passed" if standalone_thumb else "latest_passed",
|
||||||
|
directory,
|
||||||
|
)
|
||||||
|
return summary, release_ready
|
||||||
@@ -1,7 +1,7 @@
|
|||||||
"""Independent reviewed profile for the right 19-Tag product layout."""
|
"""Independent reviewed profile for the right 19-Tag product layout."""
|
||||||
|
|
||||||
from ...core import ProfileKey
|
from ...core import ProfileKey
|
||||||
from ...full_hand import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile
|
from .profile import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile
|
||||||
from .zero_policy import (
|
from .zero_policy import (
|
||||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
||||||
|
|||||||
@@ -27,7 +27,7 @@ from ...operator_report import (
|
|||||||
render_progress_zh,
|
render_progress_zh,
|
||||||
)
|
)
|
||||||
from ...product import ProductConfig, load_product_config, sha256_file
|
from ...product import ProductConfig, load_product_config, sha256_file
|
||||||
from ...publication import atomic_session_pointer, finalize_session_artifacts
|
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||||
from ...storage import atomic_write_json
|
from ...storage import atomic_write_json
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
"""Reviewed static-zero, endpoint, and mimic topology exports."""
|
"""Reviewed static-zero, endpoint, and mimic topology exports."""
|
||||||
|
|
||||||
from ...full_hand import MIMIC_DERIVED_FINGER_DIPS
|
from .profile import MIMIC_DERIVED_FINGER_DIPS
|
||||||
from ...urdf_zero import (
|
from .zero_solver import (
|
||||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
||||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -18,6 +18,7 @@ class EngineBindings:
|
|||||||
preparation_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
|
preparation_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
|
||||||
return_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
|
return_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
|
||||||
cli_main: Callable[[list[str] | None], None]
|
cli_main: Callable[[list[str] | None], None]
|
||||||
|
node_main: Callable[[list[str] | None], None]
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -17,9 +17,13 @@ import xml.etree.ElementTree as ET
|
|||||||
|
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
from .compat import product_profile_key, resolve_renamed_package_path
|
from .compat import (
|
||||||
|
product_profile_key,
|
||||||
|
resolve_legacy_profile_alias,
|
||||||
|
resolve_renamed_package_path,
|
||||||
|
)
|
||||||
from .core import CalibrationProfile, ProfileKey
|
from .core import CalibrationProfile, ProfileKey
|
||||||
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
from .core.geometry import camera_info_fingerprint, load_camera_extrinsics
|
||||||
from .models import RegisteredProfile, get_default_registry
|
from .models import RegisteredProfile, get_default_registry
|
||||||
|
|
||||||
|
|
||||||
@@ -65,8 +69,11 @@ class ProductCalibrationContract:
|
|||||||
def get_product_calibration_contract(
|
def get_product_calibration_contract(
|
||||||
model: str, side: str, layout_id: str, revision: int = 1
|
model: str, side: str, layout_id: str, revision: int = 1
|
||||||
) -> ProductCalibrationContract:
|
) -> ProductCalibrationContract:
|
||||||
|
key = resolve_legacy_profile_alias(
|
||||||
|
ProfileKey(model, side, layout_id, revision)
|
||||||
|
)
|
||||||
return ProductCalibrationContract(
|
return ProductCalibrationContract(
|
||||||
get_default_registry().resolve(model, side, layout_id, revision)
|
get_default_registry().get(key)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -133,12 +140,18 @@ def _custom_pnp_tag_sizes_m_by_id(
|
|||||||
) -> dict[int, float]:
|
) -> dict[int, float]:
|
||||||
with path.open("r", encoding="utf-8") as stream:
|
with path.open("r", encoding="utf-8") as stream:
|
||||||
payload = _mapping(yaml.safe_load(stream), str(path))
|
payload = _mapping(yaml.safe_load(stream), str(path))
|
||||||
parameters = _mapping(
|
candidates = [
|
||||||
_mapping(payload.get("g20_calibration"), "g20_calibration").get(
|
_mapping(node, str(name)).get("ros__parameters")
|
||||||
"ros__parameters"
|
for name, node in payload.items()
|
||||||
),
|
if isinstance(node, Mapping)
|
||||||
"ros__parameters",
|
and isinstance(node.get("ros__parameters"), Mapping)
|
||||||
)
|
and "tag_size_m" in node["ros__parameters"]
|
||||||
|
]
|
||||||
|
if len(candidates) != 1:
|
||||||
|
raise ValueError(
|
||||||
|
"calibration config must contain exactly one Tag-size parameter set"
|
||||||
|
)
|
||||||
|
parameters = _mapping(candidates[0], "ros__parameters")
|
||||||
default = float(parameters.get("tag_size_m", 0.0))
|
default = float(parameters.get("tag_size_m", 0.0))
|
||||||
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
|
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
|
||||||
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
|
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
|
||||||
@@ -181,6 +194,7 @@ class ProductConfig:
|
|||||||
workspace: Path
|
workspace: Path
|
||||||
schema_version: int
|
schema_version: int
|
||||||
profile_key: ProfileKey
|
profile_key: ProfileKey
|
||||||
|
namespace: str
|
||||||
model: str
|
model: str
|
||||||
side: str
|
side: str
|
||||||
tag_layout: str
|
tag_layout: str
|
||||||
@@ -228,6 +242,11 @@ def load_product_config(
|
|||||||
contract = get_product_calibration_contract(
|
contract = get_product_calibration_contract(
|
||||||
model, side, layout, profile_key.revision
|
model, side, layout, profile_key.revision
|
||||||
)
|
)
|
||||||
|
namespace = str(
|
||||||
|
raw.get("namespace", contract.typed_profile.namespace)
|
||||||
|
).strip()
|
||||||
|
if not namespace.startswith("/") or namespace.endswith("/"):
|
||||||
|
raise ValueError("namespace must be an absolute ROS namespace")
|
||||||
can_interface = str(raw.get("can_interface", "")).strip()
|
can_interface = str(raw.get("can_interface", "")).strip()
|
||||||
if not can_interface:
|
if not can_interface:
|
||||||
raise ValueError("can_interface is required")
|
raise ValueError("can_interface is required")
|
||||||
@@ -301,7 +320,14 @@ def load_product_config(
|
|||||||
required_views = tuple(contract.views)
|
required_views = tuple(contract.views)
|
||||||
if set(camera_raw) != set(required_views):
|
if set(camera_raw) != set(required_views):
|
||||||
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
||||||
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
vision = contract.typed_profile.vision
|
||||||
|
loaded_extrinsics = load_camera_extrinsics(
|
||||||
|
extrinsics,
|
||||||
|
required_views=required_views,
|
||||||
|
reference_view=vision.extrinsic_reference_view,
|
||||||
|
quality_limits=vision.extrinsics_quality_limits,
|
||||||
|
minimum_capture_counts=vision.minimum_capture_counts,
|
||||||
|
)
|
||||||
cameras: dict[str, dict[str, str]] = {}
|
cameras: dict[str, dict[str, str]] = {}
|
||||||
serials: set[str] = set()
|
serials: set[str] = set()
|
||||||
for view in required_views:
|
for view in required_views:
|
||||||
@@ -344,6 +370,7 @@ def load_product_config(
|
|||||||
workspace=root,
|
workspace=root,
|
||||||
schema_version=int(raw["schema_version"]),
|
schema_version=int(raw["schema_version"]),
|
||||||
profile_key=profile_key,
|
profile_key=profile_key,
|
||||||
|
namespace=namespace,
|
||||||
model=model,
|
model=model,
|
||||||
side=side,
|
side=side,
|
||||||
tag_layout=layout,
|
tag_layout=layout,
|
||||||
|
|||||||
@@ -1,914 +1,7 @@
|
|||||||
"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
|
"""One-release module alias for the relocated model publication policy."""
|
||||||
|
|
||||||
from __future__ import annotations
|
import sys
|
||||||
|
|
||||||
from datetime import datetime, timezone
|
from .models.g20 import publication as _implementation
|
||||||
import copy
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
import os
|
|
||||||
from pathlib import Path
|
|
||||||
import re
|
|
||||||
from typing import Any, Mapping, Sequence
|
|
||||||
import xml.etree.ElementTree as ET
|
|
||||||
|
|
||||||
import numpy as np
|
sys.modules[__name__] = _implementation
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
from .full_hand import (
|
|
||||||
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
|
||||||
G20_RIGHT_19_LAYOUT,
|
|
||||||
get_hand_calibration_profile,
|
|
||||||
validate_compact_payload,
|
|
||||||
)
|
|
||||||
from .product import ProductConfig, sha256_file
|
|
||||||
from .storage import atomic_write_json
|
|
||||||
from .core.urdf import UrdfCorrectionPlan, build_correction_plan
|
|
||||||
from .urdf_zero import (
|
|
||||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
|
||||||
get_zero_calibration_profile,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
PASSIVE_JOINTS = frozenset(
|
|
||||||
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
|
||||||
)
|
|
||||||
VISUALLY_MEASURED_FINGER_DIPS = frozenset(
|
|
||||||
{"index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
|
||||||
)
|
|
||||||
ACTIVE_ZERO_JOINTS = frozenset(
|
|
||||||
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
|
|
||||||
)
|
|
||||||
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
|
||||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
|
||||||
) - ACTIVE_ZERO_JOINTS
|
|
||||||
SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9
|
|
||||||
|
|
||||||
|
|
||||||
def _load_json(path: Path) -> dict[str, Any]:
|
|
||||||
with path.open("r", encoding="utf-8") as stream:
|
|
||||||
payload = json.load(stream)
|
|
||||||
if not isinstance(payload, dict):
|
|
||||||
raise ValueError(f"{path} must contain a JSON object")
|
|
||||||
return payload
|
|
||||||
|
|
||||||
|
|
||||||
def _joint_blocks(text: str) -> dict[str, str]:
|
|
||||||
pattern = re.compile(
|
|
||||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
|
||||||
re.DOTALL,
|
|
||||||
)
|
|
||||||
return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
|
|
||||||
|
|
||||||
|
|
||||||
def _mask_origin_rpy(block: str) -> str:
|
|
||||||
return re.sub(
|
|
||||||
r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
|
|
||||||
r"\1\2__CALIBRATED_RPY__\2",
|
|
||||||
block,
|
|
||||||
count=1,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _mask_active_origin_rpy_fields(text: str) -> str:
|
|
||||||
pattern = re.compile(
|
|
||||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
|
||||||
re.DOTALL,
|
|
||||||
)
|
|
||||||
|
|
||||||
def replace(match: re.Match[str]) -> str:
|
|
||||||
block = match.group(0)
|
|
||||||
if match.group("name") not in ACTIVE_ZERO_JOINTS:
|
|
||||||
return block
|
|
||||||
masked = _mask_origin_rpy(block)
|
|
||||||
if masked == block:
|
|
||||||
raise ValueError(
|
|
||||||
f"active joint {match.group('name')} has no origin.rpy"
|
|
||||||
)
|
|
||||||
return masked
|
|
||||||
|
|
||||||
return pattern.sub(replace, text)
|
|
||||||
|
|
||||||
|
|
||||||
def _mask_endpoint_coordinate_fields(text: str) -> str:
|
|
||||||
"""Mask only limit/mimic fields induced by endpoint zero coordinates."""
|
|
||||||
pattern = re.compile(
|
|
||||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
|
||||||
re.DOTALL,
|
|
||||||
)
|
|
||||||
|
|
||||||
def replace(match: re.Match[str]) -> str:
|
|
||||||
block = match.group(0)
|
|
||||||
name = match.group("name")
|
|
||||||
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS:
|
|
||||||
return re.sub(
|
|
||||||
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
|
|
||||||
r"\1\2__CALIBRATED_UPPER__\2",
|
|
||||||
block,
|
|
||||||
count=1,
|
|
||||||
)
|
|
||||||
mimic = re.search(
|
|
||||||
r"<mimic\b[^>]*\bjoint\s*=\s*([\"'])(?P<source>[^\"']+)\1[^>]*>",
|
|
||||||
block,
|
|
||||||
re.DOTALL,
|
|
||||||
)
|
|
||||||
if (
|
|
||||||
mimic is not None
|
|
||||||
and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
|
||||||
):
|
|
||||||
return re.sub(
|
|
||||||
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
|
|
||||||
r"\1\2__CALIBRATED_MIMIC_OFFSET__\2",
|
|
||||||
block,
|
|
||||||
count=1,
|
|
||||||
)
|
|
||||||
return block
|
|
||||||
|
|
||||||
return pattern.sub(replace, text)
|
|
||||||
|
|
||||||
|
|
||||||
def _triplet(value: str) -> np.ndarray:
|
|
||||||
result = np.asarray([float(item) for item in value.split()], dtype=float)
|
|
||||||
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
|
||||||
raise ValueError(f"invalid URDF triplet: {value}")
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
|
|
||||||
return {
|
|
||||||
str(joint.get("name")): joint
|
|
||||||
for joint in ET.parse(Path(path)).getroot().findall("joint")
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def _verify_expected_origin_offsets(
|
|
||||||
source: str | Path,
|
|
||||||
corrected: str | Path,
|
|
||||||
expected_offsets_rad: Mapping[str, float],
|
|
||||||
) -> None:
|
|
||||||
offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
|
|
||||||
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
|
|
||||||
not math.isfinite(value) for value in offsets.values()
|
|
||||||
):
|
|
||||||
raise ValueError("expected offsets must contain all finite active static-zero values")
|
|
||||||
before = _joint_elements(source)
|
|
||||||
after = _joint_elements(corrected)
|
|
||||||
maximum_rotation_error = 0.0
|
|
||||||
for name, original_joint in before.items():
|
|
||||||
corrected_joint = after[name]
|
|
||||||
original_origin = original_joint.find("origin")
|
|
||||||
corrected_origin = corrected_joint.find("origin")
|
|
||||||
if original_origin is None or corrected_origin is None:
|
|
||||||
if original_origin is not corrected_origin:
|
|
||||||
raise ValueError(f"corrected URDF changed origin presence for {name}")
|
|
||||||
continue
|
|
||||||
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
|
|
||||||
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
|
|
||||||
if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
|
|
||||||
raise ValueError(f"corrected URDF changed origin.xyz for {name}")
|
|
||||||
original_rotation = Rotation.from_euler(
|
|
||||||
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
|
|
||||||
)
|
|
||||||
corrected_rotation = Rotation.from_euler(
|
|
||||||
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
|
|
||||||
)
|
|
||||||
expected_rotation = original_rotation
|
|
||||||
if name in offsets:
|
|
||||||
axis_node = original_joint.find("axis")
|
|
||||||
axis = _triplet(
|
|
||||||
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
|
|
||||||
)
|
|
||||||
norm = float(np.linalg.norm(axis))
|
|
||||||
if norm <= 1.0e-12:
|
|
||||||
raise ValueError(f"source URDF joint {name} has a degenerate axis")
|
|
||||||
expected_rotation = expected_rotation * Rotation.from_rotvec(
|
|
||||||
axis / norm * offsets[name]
|
|
||||||
)
|
|
||||||
error = float((expected_rotation.inv() * corrected_rotation).magnitude())
|
|
||||||
maximum_rotation_error = max(maximum_rotation_error, error)
|
|
||||||
# Schema v4 intentionally stores zero offsets at eight decimal places.
|
|
||||||
# Older sessions wrote the corrected URDF from the full-precision solve,
|
|
||||||
# so comparing that file with the published JSON necessarily permits half
|
|
||||||
# of one last-place unit. This is about 2.9e-7 degrees and is far below
|
|
||||||
# any calibration or URDF numerical significance.
|
|
||||||
if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD:
|
|
||||||
raise ValueError(
|
|
||||||
"corrected URDF origin.rpy does not match the published zero offsets"
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def verify_corrected_urdf(
|
|
||||||
source: str | Path,
|
|
||||||
corrected: str | Path,
|
|
||||||
*,
|
|
||||||
expected_offsets_rad: Mapping[str, float] | None = None,
|
|
||||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
|
||||||
correction_plan: UrdfCorrectionPlan | None = None,
|
|
||||||
) -> tuple[str, ...]:
|
|
||||||
"""Prove that only active-joint origin.rpy attributes changed.
|
|
||||||
|
|
||||||
Passive joint blocks, including their mimic elements, are compared as raw
|
|
||||||
UTF-8 text so formatting and numeric spelling are protected as well.
|
|
||||||
"""
|
|
||||||
source_text = Path(source).read_text(encoding="utf-8")
|
|
||||||
corrected_text = Path(corrected).read_text(encoding="utf-8")
|
|
||||||
if correction_plan is not None:
|
|
||||||
correction_plan.verify_source(source)
|
|
||||||
if expected_offsets_rad is None:
|
|
||||||
raise ValueError(
|
|
||||||
"correction-plan validation requires expected offsets"
|
|
||||||
)
|
|
||||||
correction_plan.authorize_offsets(expected_offsets_rad)
|
|
||||||
before = _joint_blocks(source_text)
|
|
||||||
after = _joint_blocks(corrected_text)
|
|
||||||
if set(before) != set(after):
|
|
||||||
raise ValueError("corrected URDF changed the joint set")
|
|
||||||
# Text patchers may normalize only the final newline. It has no URDF/XML
|
|
||||||
# semantics; every joint block and every non-rpy byte remains guarded
|
|
||||||
# below.
|
|
||||||
masked_source = _mask_endpoint_coordinate_fields(
|
|
||||||
_mask_active_origin_rpy_fields(source_text)
|
|
||||||
).rstrip("\r\n")
|
|
||||||
masked_corrected = _mask_endpoint_coordinate_fields(
|
|
||||||
_mask_active_origin_rpy_fields(corrected_text)
|
|
||||||
).rstrip("\r\n")
|
|
||||||
if masked_source != masked_corrected:
|
|
||||||
raise ValueError(
|
|
||||||
"corrected URDF changed content outside active origin.rpy or "
|
|
||||||
"endpoint-calibrated fields"
|
|
||||||
)
|
|
||||||
changed: list[str] = []
|
|
||||||
for name in before:
|
|
||||||
if before[name] == after[name]:
|
|
||||||
continue
|
|
||||||
if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS:
|
|
||||||
raise ValueError(f"corrected URDF changed protected joint {name}")
|
|
||||||
if name in ACTIVE_ZERO_JOINTS:
|
|
||||||
changed.append(name)
|
|
||||||
if expected_offsets_rad is not None:
|
|
||||||
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
|
|
||||||
endpoint_offsets = {
|
|
||||||
str(name): float(value)
|
|
||||||
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
|
||||||
}
|
|
||||||
if endpoint_offsets:
|
|
||||||
if correction_plan is not None and not set(endpoint_offsets).issubset(
|
|
||||||
correction_plan.endpoint_limit_joints
|
|
||||||
):
|
|
||||||
raise ValueError(
|
|
||||||
"endpoint offsets are not authorized by the correction plan"
|
|
||||||
)
|
|
||||||
source_joints = _joint_elements(source)
|
|
||||||
corrected_joints = _joint_elements(corrected)
|
|
||||||
for name, offset in endpoint_offsets.items():
|
|
||||||
original_limit = source_joints[name].find("limit")
|
|
||||||
corrected_limit = corrected_joints[name].find("limit")
|
|
||||||
expected_upper = float(original_limit.get("upper")) - offset
|
|
||||||
# Schema v4 stores the corresponding zero at eight decimal
|
|
||||||
# places, while the URDF is written from the full-precision solve.
|
|
||||||
# Match the half-last-place tolerance used for origin rotations.
|
|
||||||
if (
|
|
||||||
abs(float(corrected_limit.get("upper")) - expected_upper)
|
|
||||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
|
||||||
):
|
|
||||||
raise ValueError(f"corrected URDF has invalid {name} upper limit")
|
|
||||||
for name in PASSIVE_JOINTS:
|
|
||||||
original_mimic = source_joints[name].find("mimic")
|
|
||||||
corrected_mimic = corrected_joints[name].find("mimic")
|
|
||||||
if original_mimic is None or corrected_mimic is None:
|
|
||||||
continue
|
|
||||||
source_name = str(original_mimic.get("joint"))
|
|
||||||
multiplier = float(original_mimic.get("multiplier", "1"))
|
|
||||||
expected = float(original_mimic.get("offset", "0"))
|
|
||||||
expected += multiplier * endpoint_offsets.get(source_name, 0.0)
|
|
||||||
# The active endpoint offset comes from schema-v4 JSON rounded to
|
|
||||||
# eight decimal places, while the URDF mimic was written from the
|
|
||||||
# full-precision solve. Propagate exactly the same accepted
|
|
||||||
# quantization through the mimic multiplier; retain a much
|
|
||||||
# smaller allowance for XML decimal formatting itself.
|
|
||||||
tolerance = (
|
|
||||||
abs(multiplier)
|
|
||||||
* SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
|
||||||
+ 1.0e-12
|
|
||||||
)
|
|
||||||
if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance:
|
|
||||||
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
|
|
||||||
return tuple(sorted(changed))
|
|
||||||
|
|
||||||
|
|
||||||
def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
|
|
||||||
"""Return every session-local mesh after proving it resolves safely."""
|
|
||||||
path = Path(urdf).expanduser().resolve()
|
|
||||||
root = ET.parse(path).getroot()
|
|
||||||
resources: dict[str, Path] = {}
|
|
||||||
for mesh in root.findall(".//mesh"):
|
|
||||||
filename = str(mesh.get("filename", "")).strip()
|
|
||||||
if not filename or "://" in filename or filename.startswith("package:"):
|
|
||||||
continue
|
|
||||||
relative = Path(filename)
|
|
||||||
if relative.is_absolute() or ".." in relative.parts:
|
|
||||||
raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
|
|
||||||
resolved = (path.parent / relative).resolve()
|
|
||||||
try:
|
|
||||||
resolved.relative_to(path.parent)
|
|
||||||
except ValueError as error:
|
|
||||||
raise ValueError(f"URDF mesh escapes the session: {filename}") from error
|
|
||||||
if not resolved.is_file():
|
|
||||||
raise ValueError(f"URDF mesh resource is missing: {filename}")
|
|
||||||
resources[relative.as_posix()] = resolved
|
|
||||||
return dict(sorted(resources.items()))
|
|
||||||
|
|
||||||
|
|
||||||
def validate_runtime_curves_against_urdf_limits(
|
|
||||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
|
||||||
) -> None:
|
|
||||||
"""Reject a curve whose q leaves its runtime URDF coordinate limits.
|
|
||||||
|
|
||||||
Curves in schema v4 are expressed in the corrected URDF joint coordinate,
|
|
||||||
not in the source-CAD coordinate. Endpoint zero calibration can therefore
|
|
||||||
move a corrected coordinate limit while preserving the same physical CAD
|
|
||||||
endpoint; callers publishing a calibrated pair must pass that corrected
|
|
||||||
URDF here.
|
|
||||||
"""
|
|
||||||
validate_compact_payload(payload)
|
|
||||||
runtime_joints = _joint_elements(runtime_urdf)
|
|
||||||
for name, calibration in payload["joints"].items():
|
|
||||||
joint = runtime_joints.get(str(name))
|
|
||||||
if joint is None:
|
|
||||||
raise ValueError(f"runtime URDF is missing joint {name}")
|
|
||||||
limit = joint.find("limit")
|
|
||||||
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
|
|
||||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
|
||||||
lower = float(limit.get("lower"))
|
|
||||||
upper = float(limit.get("upper"))
|
|
||||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
|
||||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
|
||||||
curve = np.asarray(calibration["angle_rad"], dtype=float)
|
|
||||||
minimum = float(np.min(curve))
|
|
||||||
maximum = float(np.max(curve))
|
|
||||||
tolerance = 1.0e-7
|
|
||||||
if minimum < lower - tolerance or maximum > upper + tolerance:
|
|
||||||
raise ValueError(
|
|
||||||
f"runtime curve exceeds runtime URDF limit for {name}: "
|
|
||||||
f"[{minimum:.9g}, {maximum:.9g}] not within "
|
|
||||||
f"[{lower:.9g}, {upper:.9g}]"
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def clamp_compact_payload_to_urdf_limits(
|
|
||||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
|
||||||
) -> tuple[dict[str, Any], dict[str, int]]:
|
|
||||||
"""Return a schema-preserving payload bounded in its runtime coordinates."""
|
|
||||||
result = copy.deepcopy(dict(payload))
|
|
||||||
validate_compact_payload(result)
|
|
||||||
runtime_joints = _joint_elements(runtime_urdf)
|
|
||||||
clipped_by_joint: dict[str, int] = {}
|
|
||||||
for name, calibration in result["joints"].items():
|
|
||||||
joint = runtime_joints.get(str(name))
|
|
||||||
limit = None if joint is None else joint.find("limit")
|
|
||||||
if (
|
|
||||||
limit is None
|
|
||||||
or limit.get("lower") is None
|
|
||||||
or limit.get("upper") is None
|
|
||||||
):
|
|
||||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
|
||||||
lower = float(limit.get("lower"))
|
|
||||||
upper = float(limit.get("upper"))
|
|
||||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
|
||||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
|
||||||
source = np.asarray(calibration["angle_rad"], dtype=float)
|
|
||||||
bounded = np.clip(source, lower, upper)
|
|
||||||
count = int(np.count_nonzero(bounded != source))
|
|
||||||
if count:
|
|
||||||
clipped_by_joint[str(name)] = count
|
|
||||||
calibration["angle_rad"] = [
|
|
||||||
round(float(value), 8) for value in bounded
|
|
||||||
]
|
|
||||||
validate_compact_payload(result)
|
|
||||||
return result, clipped_by_joint
|
|
||||||
|
|
||||||
|
|
||||||
def build_mujoco_validation_commands(
|
|
||||||
baseline: Sequence[int],
|
|
||||||
) -> dict[str, Any]:
|
|
||||||
if len(baseline) != 20:
|
|
||||||
raise ValueError("baseline must contain exactly 20 commands")
|
|
||||||
base = [int(value) for value in baseline]
|
|
||||||
if any(not 0 <= value <= 255 for value in base):
|
|
||||||
raise ValueError("baseline commands must be in [0, 255]")
|
|
||||||
|
|
||||||
def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
|
|
||||||
values = list(base)
|
|
||||||
for index, value in changes.items():
|
|
||||||
values[int(index)] = int(value)
|
|
||||||
return {"name": name, "label_zh": label, "command_u8": values}
|
|
||||||
|
|
||||||
return {
|
|
||||||
"schema_version": 1,
|
|
||||||
"topic": "/g20/cb_right_hand_control_cmd",
|
|
||||||
"serial_number": "G20_RIGHT_001",
|
|
||||||
"poses": [
|
|
||||||
pose("all_open", "全开", {}),
|
|
||||||
pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
|
|
||||||
pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
|
|
||||||
pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
|
|
||||||
pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
|
|
||||||
pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
|
|
||||||
pose(
|
|
||||||
"half_grip",
|
|
||||||
"四指半握",
|
|
||||||
{1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
|
|
||||||
),
|
|
||||||
pose(
|
|
||||||
"light_pinch",
|
|
||||||
"轻捏",
|
|
||||||
{0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
|
|
||||||
),
|
|
||||||
],
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
|
|
||||||
parent = Path(root).resolve()
|
|
||||||
target = Path(session).resolve()
|
|
||||||
if target.parent != parent:
|
|
||||||
raise ValueError("session pointer target must be a direct child of the serial root")
|
|
||||||
if not target.is_dir():
|
|
||||||
raise ValueError(f"session directory does not exist: {target}")
|
|
||||||
if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}:
|
|
||||||
raise ValueError("unsupported session pointer name")
|
|
||||||
parent.mkdir(parents=True, exist_ok=True)
|
|
||||||
destination = parent / name
|
|
||||||
temporary = parent / f".{name}.{os.getpid()}.tmp"
|
|
||||||
if temporary.is_symlink():
|
|
||||||
temporary.unlink()
|
|
||||||
elif temporary.exists():
|
|
||||||
raise ValueError(f"temporary pointer path is occupied: {temporary}")
|
|
||||||
os.symlink(target.name, temporary, target_is_directory=True)
|
|
||||||
os.replace(temporary, destination)
|
|
||||||
return destination
|
|
||||||
|
|
||||||
|
|
||||||
def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
|
||||||
validate_compact_payload(payload)
|
|
||||||
result = {
|
|
||||||
name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
|
|
||||||
for name in ACTIVE_ZERO_JOINTS
|
|
||||||
}
|
|
||||||
if set(result) != ACTIVE_ZERO_JOINTS or any(
|
|
||||||
not math.isfinite(value) for value in result.values()
|
|
||||||
):
|
|
||||||
raise ValueError(
|
|
||||||
"payload does not contain all finite observable active zero offsets"
|
|
||||||
)
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
|
||||||
"""Validate a standalone thumb artifact and expand CAD finger zeros."""
|
|
||||||
if (
|
|
||||||
payload.get("schema_version") != 1
|
|
||||||
or payload.get("artifact_type")
|
|
||||||
!= "g20_right_standalone_thumb_calibration"
|
|
||||||
or payload.get("side") != "right"
|
|
||||||
or payload.get("non_thumb_zero_policy") != "source_cad_unchanged"
|
|
||||||
or not bool(payload.get("quality", {}).get("passed"))
|
|
||||||
):
|
|
||||||
raise ValueError("standalone thumb JSON contract is invalid")
|
|
||||||
joints = payload.get("joints")
|
|
||||||
expected_joint_names = {
|
|
||||||
"thumb_cmc_roll",
|
|
||||||
"thumb_cmc_yaw",
|
|
||||||
"thumb_cmc_pitch",
|
|
||||||
"thumb_mcp",
|
|
||||||
"thumb_ip",
|
|
||||||
}
|
|
||||||
if not isinstance(joints, Mapping) or set(joints) != expected_joint_names:
|
|
||||||
raise ValueError("standalone thumb JSON has the wrong joint set")
|
|
||||||
thumb_names = expected_joint_names - {"thumb_ip"}
|
|
||||||
result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS}
|
|
||||||
for name in thumb_names:
|
|
||||||
try:
|
|
||||||
value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"])
|
|
||||||
except (KeyError, TypeError, ValueError) as error:
|
|
||||||
raise ValueError(
|
|
||||||
f"standalone thumb JSON is missing zero for {name}"
|
|
||||||
) from error
|
|
||||||
curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float)
|
|
||||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
|
||||||
raise ValueError(f"standalone thumb JSON curve is invalid: {name}")
|
|
||||||
result[name] = value
|
|
||||||
passive_curve = np.asarray(
|
|
||||||
joints["thumb_ip"].get("angle_rad", ()), dtype=float
|
|
||||||
)
|
|
||||||
if (
|
|
||||||
passive_curve.shape != (256,)
|
|
||||||
or not np.all(np.isfinite(passive_curve))
|
|
||||||
or not bool(joints["thumb_ip"].get("passive"))
|
|
||||||
or any(not math.isfinite(value) for value in result.values())
|
|
||||||
):
|
|
||||||
raise ValueError("standalone thumb JSON passive curve is invalid")
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def verify_partial_scope_preserves_certified_zeros(
|
|
||||||
*,
|
|
||||||
scope: str,
|
|
||||||
source_session: str | Path,
|
|
||||||
serial_root: str | Path | None = None,
|
|
||||||
serial_number: str,
|
|
||||||
current_offsets: Mapping[str, float],
|
|
||||||
) -> None:
|
|
||||||
"""Reject a partial artifact that changes any certified non-target zero."""
|
|
||||||
selected = str(scope).strip().lower()
|
|
||||||
if selected not in {"thumb", "fingers"}:
|
|
||||||
return
|
|
||||||
source = Path(source_session).expanduser()
|
|
||||||
if not source.is_absolute():
|
|
||||||
if serial_root is None:
|
|
||||||
raise ValueError(
|
|
||||||
"relative partial base session requires its serial root"
|
|
||||||
)
|
|
||||||
source = Path(serial_root).expanduser() / source
|
|
||||||
source = source.resolve()
|
|
||||||
base_payload = _load_json(
|
|
||||||
source / f"g20_right_{serial_number}_calibration.json"
|
|
||||||
)
|
|
||||||
base_offsets = (
|
|
||||||
standalone_thumb_offsets(base_payload)
|
|
||||||
if base_payload.get("artifact_type")
|
|
||||||
== "g20_right_standalone_thumb_calibration"
|
|
||||||
else active_offsets(base_payload)
|
|
||||||
)
|
|
||||||
preserve_thumb = selected == "fingers"
|
|
||||||
changed = {
|
|
||||||
name: abs(float(current_offsets[name]) - float(base_offsets[name]))
|
|
||||||
for name in ACTIVE_ZERO_JOINTS
|
|
||||||
if name.startswith("thumb_") == preserve_thumb
|
|
||||||
and abs(float(current_offsets[name]) - float(base_offsets[name]))
|
|
||||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
|
||||||
}
|
|
||||||
if changed:
|
|
||||||
raise ValueError(
|
|
||||||
f"partial {selected} calibration changed certified non-target "
|
|
||||||
"zeros: "
|
|
||||||
+ ",".join(sorted(changed))
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def compare_session_offsets(
|
|
||||||
first: Mapping[str, Any],
|
|
||||||
second: Mapping[str, Any],
|
|
||||||
*,
|
|
||||||
maximum_difference_rad: float,
|
|
||||||
) -> dict[str, float]:
|
|
||||||
first_standalone = (
|
|
||||||
first.get("artifact_type")
|
|
||||||
== "g20_right_standalone_thumb_calibration"
|
|
||||||
)
|
|
||||||
second_standalone = (
|
|
||||||
second.get("artifact_type")
|
|
||||||
== "g20_right_standalone_thumb_calibration"
|
|
||||||
)
|
|
||||||
if first_standalone != second_standalone:
|
|
||||||
raise ValueError("cannot compare standalone thumb and full-hand artifacts")
|
|
||||||
left = (
|
|
||||||
standalone_thumb_offsets(first)
|
|
||||||
if first_standalone
|
|
||||||
else active_offsets(first)
|
|
||||||
)
|
|
||||||
right = (
|
|
||||||
standalone_thumb_offsets(second)
|
|
||||||
if second_standalone
|
|
||||||
else active_offsets(second)
|
|
||||||
)
|
|
||||||
differences = {
|
|
||||||
name: abs(left[name] - right[name])
|
|
||||||
for name in sorted(left)
|
|
||||||
}
|
|
||||||
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
|
|
||||||
if failed:
|
|
||||||
details = ", ".join(
|
|
||||||
f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
|
|
||||||
)
|
|
||||||
raise ValueError(f"independent-session static-zero mismatch: {details}")
|
|
||||||
return differences
|
|
||||||
|
|
||||||
|
|
||||||
def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
|
|
||||||
directory = Path(session).resolve()
|
|
||||||
urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
|
|
||||||
if len(urdfs) != 1:
|
|
||||||
raise ValueError("session must contain exactly one corrected URDF")
|
|
||||||
return {
|
|
||||||
"json": directory / f"g20_right_{serial_number}_calibration.json",
|
|
||||||
"urdf": urdfs[0],
|
|
||||||
"summary": directory / "calibration_summary_zh.json",
|
|
||||||
"commands": directory / "mujoco_validation_commands.json",
|
|
||||||
"raw": directory / "raw_samples.jsonl",
|
|
||||||
"log": directory / "calibration.log",
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def find_compatible_prior_session(
|
|
||||||
config: ProductConfig,
|
|
||||||
current: str | Path,
|
|
||||||
current_payload: Mapping[str, Any],
|
|
||||||
) -> tuple[Path | None, dict[str, float]]:
|
|
||||||
current_path = Path(current).resolve()
|
|
||||||
for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
|
|
||||||
if candidate.resolve() == current_path:
|
|
||||||
continue
|
|
||||||
json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
|
|
||||||
summary_path = candidate / "calibration_summary_zh.json"
|
|
||||||
if not json_path.is_file() or not summary_path.is_file():
|
|
||||||
continue
|
|
||||||
try:
|
|
||||||
summary = _load_json(summary_path)
|
|
||||||
if not bool(summary.get("quality", {}).get("passed")):
|
|
||||||
continue
|
|
||||||
payload = _load_json(json_path)
|
|
||||||
differences = compare_session_offsets(
|
|
||||||
payload,
|
|
||||||
current_payload,
|
|
||||||
maximum_difference_rad=config.static_repeatability_rad,
|
|
||||||
)
|
|
||||||
except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
|
|
||||||
continue
|
|
||||||
return candidate, differences
|
|
||||||
return None, {}
|
|
||||||
|
|
||||||
|
|
||||||
def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
|
|
||||||
# The formal product uses the independently held-out fourth sweep cycle.
|
|
||||||
# The optional eight-pose check remains a developer diagnostic because the
|
|
||||||
# axis-line zero solve does not establish an absolute Cartesian hand-base
|
|
||||||
# transform. When explicitly enabled, retain its strict coverage and
|
|
||||||
# error checks.
|
|
||||||
if not bool(combination.get("enabled")):
|
|
||||||
return
|
|
||||||
position_p95 = float(combination.get("position_p95_m", float("inf")))
|
|
||||||
orientation_p95 = float(
|
|
||||||
combination.get("orientation_p95_rad", float("inf"))
|
|
||||||
)
|
|
||||||
required = {str(value) for value in combination.get("required_targets", ())}
|
|
||||||
expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
|
|
||||||
observations = combination.get("observation_counts")
|
|
||||||
validations = combination.get("validation_counts")
|
|
||||||
if required != expected:
|
|
||||||
raise ValueError("combination validation has the wrong required-target set")
|
|
||||||
if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
|
|
||||||
raise ValueError("combination validation is missing per-target coverage")
|
|
||||||
missing_observations = sorted(
|
|
||||||
key for key in expected if int(observations.get(key, 0)) < 2
|
|
||||||
)
|
|
||||||
missing_validations = sorted(
|
|
||||||
key for key in expected if int(validations.get(key, 0)) < 1
|
|
||||||
)
|
|
||||||
if missing_observations or missing_validations:
|
|
||||||
raise ValueError(
|
|
||||||
"combination validation target coverage is incomplete: "
|
|
||||||
f"observations={','.join(missing_observations) or '-'}; "
|
|
||||||
f"validations={','.join(missing_validations) or '-'}"
|
|
||||||
)
|
|
||||||
if (
|
|
||||||
not bool(combination.get("completed"))
|
|
||||||
or int(combination.get("completed_poses", 0)) != 8
|
|
||||||
or position_p95 > 0.003
|
|
||||||
or orientation_p95 > math.radians(2.0)
|
|
||||||
):
|
|
||||||
raise ValueError(
|
|
||||||
"eight-pose four-finger combination validation is incomplete or failed"
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def finalize_session_artifacts(
|
|
||||||
config: ProductConfig,
|
|
||||||
session: str | Path,
|
|
||||||
*,
|
|
||||||
node_status: Mapping[str, Any],
|
|
||||||
) -> tuple[dict[str, Any], bool]:
|
|
||||||
"""Create diagnostics and update latest_passed only after every guard."""
|
|
||||||
directory = Path(session).resolve()
|
|
||||||
paths = session_artifact_paths(directory, config.serial_number)
|
|
||||||
for name in ("json", "urdf", "raw", "log"):
|
|
||||||
if not paths[name].is_file():
|
|
||||||
raise ValueError(f"session is missing {name}: {paths[name]}")
|
|
||||||
payload = _load_json(paths["json"])
|
|
||||||
standalone_thumb = bool(
|
|
||||||
payload.get("artifact_type")
|
|
||||||
== "g20_right_standalone_thumb_calibration"
|
|
||||||
)
|
|
||||||
if standalone_thumb:
|
|
||||||
standalone_thumb_offsets(payload)
|
|
||||||
else:
|
|
||||||
validate_compact_payload(payload)
|
|
||||||
if payload.get("schema_version") != 4 or payload.get("side") != "right":
|
|
||||||
raise ValueError("runtime JSON is not the compact right-hand schema v4")
|
|
||||||
if not bool(payload.get("quality", {}).get("passed")):
|
|
||||||
raise ValueError("runtime JSON quality is not passed")
|
|
||||||
combination = node_status.get("combination_validation")
|
|
||||||
if not isinstance(combination, Mapping):
|
|
||||||
raise ValueError("node status is missing combination validation")
|
|
||||||
_verify_combination_validation(combination)
|
|
||||||
resume = node_status.get("resume", {})
|
|
||||||
if not isinstance(resume, Mapping):
|
|
||||||
raise ValueError("node status has invalid resume provenance")
|
|
||||||
calibration_scope = str(
|
|
||||||
resume.get("recalibration_scope", "full")
|
|
||||||
).strip().lower()
|
|
||||||
if calibration_scope not in {"full", "thumb", "fingers"}:
|
|
||||||
raise ValueError("node status has an unsupported recalibration scope")
|
|
||||||
recalibration_tasks = tuple(
|
|
||||||
str(value) for value in resume.get("recalibration_task_keys", ())
|
|
||||||
)
|
|
||||||
if calibration_scope == "thumb" and not standalone_thumb and (
|
|
||||||
not bool(resume.get("used"))
|
|
||||||
or not str(resume.get("source_session", ""))
|
|
||||||
or len(recalibration_tasks) != 4
|
|
||||||
or any("thumb_" not in name for name in recalibration_tasks)
|
|
||||||
):
|
|
||||||
raise ValueError(
|
|
||||||
"thumb recalibration is missing its passed base-session provenance"
|
|
||||||
)
|
|
||||||
if calibration_scope == "fingers" and (
|
|
||||||
not bool(resume.get("used"))
|
|
||||||
or not str(resume.get("source_session", ""))
|
|
||||||
or len(recalibration_tasks) != 12
|
|
||||||
or any("thumb_" in name for name in recalibration_tasks)
|
|
||||||
):
|
|
||||||
raise ValueError(
|
|
||||||
"finger recalibration is missing its certified thumb-session "
|
|
||||||
"provenance"
|
|
||||||
)
|
|
||||||
if standalone_thumb and calibration_scope != "thumb":
|
|
||||||
raise ValueError("standalone thumb artifact has the wrong node scope")
|
|
||||||
offsets = (
|
|
||||||
standalone_thumb_offsets(payload)
|
|
||||||
if standalone_thumb
|
|
||||||
else active_offsets(payload)
|
|
||||||
)
|
|
||||||
if calibration_scope != "full" and not standalone_thumb:
|
|
||||||
verify_partial_scope_preserves_certified_zeros(
|
|
||||||
scope=calibration_scope,
|
|
||||||
source_session=str(resume.get("source_session", "")),
|
|
||||||
serial_root=directory.parent,
|
|
||||||
serial_number=config.serial_number,
|
|
||||||
current_offsets=offsets,
|
|
||||||
)
|
|
||||||
endpoint_offsets = {
|
|
||||||
name: offsets[name]
|
|
||||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
|
||||||
}
|
|
||||||
typed_profile = config.calibration_contract.typed_profile
|
|
||||||
frozen_names = typed_profile.scope.frozen_joints[calibration_scope]
|
|
||||||
correction_plan = build_correction_plan(
|
|
||||||
typed_profile,
|
|
||||||
source_sha256=config.source_urdf_sha256,
|
|
||||||
scope=calibration_scope,
|
|
||||||
frozen_offsets_rad={name: offsets[name] for name in frozen_names},
|
|
||||||
)
|
|
||||||
changed_joints = verify_corrected_urdf(
|
|
||||||
config.source_urdf,
|
|
||||||
paths["urdf"],
|
|
||||||
expected_offsets_rad=offsets,
|
|
||||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
|
||||||
correction_plan=correction_plan,
|
|
||||||
)
|
|
||||||
if standalone_thumb:
|
|
||||||
clipped_runtime_joints = {}
|
|
||||||
else:
|
|
||||||
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
|
||||||
payload, paths["urdf"]
|
|
||||||
)
|
|
||||||
if clipped_runtime_joints:
|
|
||||||
atomic_write_json(paths["json"], payload)
|
|
||||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
|
||||||
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
|
|
||||||
mesh_hashes = {
|
|
||||||
name: sha256_file(path) for name, path in mesh_resources.items()
|
|
||||||
}
|
|
||||||
commands = (
|
|
||||||
{
|
|
||||||
"artifact_type": "standalone_thumb_no_full_hand_validation",
|
|
||||||
"poses": [],
|
|
||||||
}
|
|
||||||
if standalone_thumb
|
|
||||||
else build_mujoco_validation_commands(payload["baseline_command_u8"])
|
|
||||||
)
|
|
||||||
atomic_write_json(paths["commands"], commands)
|
|
||||||
|
|
||||||
prior, differences = find_compatible_prior_session(config, directory, payload)
|
|
||||||
release_ready = config.required_independent_passes == 1 or prior is not None
|
|
||||||
quality = dict(payload["quality"])
|
|
||||||
preserved_certified_zeros = sorted(
|
|
||||||
name
|
|
||||||
for name in ACTIVE_ZERO_JOINTS
|
|
||||||
if (
|
|
||||||
calibration_scope == "thumb"
|
|
||||||
and not name.startswith("thumb_")
|
|
||||||
)
|
|
||||||
or (
|
|
||||||
calibration_scope == "fingers"
|
|
||||||
and name.startswith("thumb_")
|
|
||||||
)
|
|
||||||
)
|
|
||||||
if standalone_thumb:
|
|
||||||
preserved_certified_zeros = []
|
|
||||||
calibrated_zero_joints = (
|
|
||||||
sorted(
|
|
||||||
name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_")
|
|
||||||
)
|
|
||||||
if standalone_thumb
|
|
||||||
else sorted(ACTIVE_ZERO_JOINTS)
|
|
||||||
)
|
|
||||||
summary: dict[str, Any] = {
|
|
||||||
"schema_version": 1,
|
|
||||||
"serial_number": config.serial_number,
|
|
||||||
"session_id": f"{config.serial_number}_{directory.name}",
|
|
||||||
"calibration_scope": calibration_scope,
|
|
||||||
"inherited_base_session": (
|
|
||||||
None
|
|
||||||
if calibration_scope == "full" or standalone_thumb
|
|
||||||
else str(resume.get("source_session"))
|
|
||||||
),
|
|
||||||
"freshly_calibrated_task_keys": list(recalibration_tasks),
|
|
||||||
"preserved_certified_zero_joints": preserved_certified_zeros,
|
|
||||||
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
|
|
||||||
"quality": quality,
|
|
||||||
"runtime_limit_clipped_bins": clipped_runtime_joints,
|
|
||||||
"runtime_curve_domain": (
|
|
||||||
"thumb_diagnostic_only"
|
|
||||||
if standalone_thumb
|
|
||||||
else "requested_command_u8"
|
|
||||||
),
|
|
||||||
"static_zero_calibrated_joints": calibrated_zero_joints,
|
|
||||||
"non_thumb_zero_policy": (
|
|
||||||
"source_cad_unchanged" if standalone_thumb else None
|
|
||||||
),
|
|
||||||
"retained_active_urdf_zero_joints": sorted(
|
|
||||||
RETAINED_ACTIVE_ZERO_JOINTS
|
|
||||||
),
|
|
||||||
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
|
|
||||||
"mimic_derived_dynamic_joints": [],
|
|
||||||
"visually_measured_passive_joints": sorted(
|
|
||||||
VISUALLY_MEASURED_FINGER_DIPS
|
|
||||||
),
|
|
||||||
"changed_urdf_joint_origins": list(changed_joints),
|
|
||||||
"hashes": {
|
|
||||||
"source_urdf_sha256": config.source_urdf_sha256,
|
|
||||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
|
||||||
"calibration_config_sha256": config.calibration_config_sha256,
|
|
||||||
"corrected_urdf_sha256": sha256_file(paths["urdf"]),
|
|
||||||
"mesh_resources_sha256": mesh_hashes,
|
|
||||||
"calibration_json_sha256": sha256_file(paths["json"]),
|
|
||||||
"raw_samples_sha256": sha256_file(paths["raw"]),
|
|
||||||
},
|
|
||||||
"holdout": {
|
|
||||||
"training_cycles": [0, 1, 2],
|
|
||||||
"validation_cycle": 3,
|
|
||||||
"isolated": True,
|
|
||||||
},
|
|
||||||
"combination_validation": dict(combination),
|
|
||||||
"formal_release": {
|
|
||||||
"passed": release_ready,
|
|
||||||
"required_independent_passes": config.required_independent_passes,
|
|
||||||
"comparison_session": None if prior is None else prior.name,
|
|
||||||
"maximum_static_difference_deg": (
|
|
||||||
None if not differences else math.degrees(max(differences.values()))
|
|
||||||
),
|
|
||||||
},
|
|
||||||
"node_status": dict(node_status),
|
|
||||||
"created_at_utc": datetime.now(timezone.utc).isoformat(),
|
|
||||||
"artifacts": {name: path.name for name, path in paths.items()},
|
|
||||||
}
|
|
||||||
atomic_write_json(paths["summary"], summary)
|
|
||||||
# Recompute hashes after all files exist and validate the pair once more
|
|
||||||
# immediately before the one atomic publication operation.
|
|
||||||
if sha256_file(config.source_urdf) != config.source_urdf_sha256:
|
|
||||||
raise ValueError("source URDF changed during calibration")
|
|
||||||
verify_corrected_urdf(
|
|
||||||
config.source_urdf,
|
|
||||||
paths["urdf"],
|
|
||||||
expected_offsets_rad=offsets,
|
|
||||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
|
||||||
)
|
|
||||||
# Revalidate the same coordinate contract immediately before publication:
|
|
||||||
# verify_corrected_urdf proves that corrected endpoint limits map back to
|
|
||||||
# the original physical CAD endpoints, while the runtime curves must stay
|
|
||||||
# inside those corrected-coordinate limits.
|
|
||||||
if not standalone_thumb:
|
|
||||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
|
||||||
final_mesh_hashes = {
|
|
||||||
name: sha256_file(path)
|
|
||||||
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
|
||||||
}
|
|
||||||
if final_mesh_hashes != mesh_hashes:
|
|
||||||
raise ValueError("URDF mesh resources changed during publication")
|
|
||||||
if release_ready:
|
|
||||||
atomic_session_pointer(
|
|
||||||
config.session_root,
|
|
||||||
"latest_thumb_passed" if standalone_thumb else "latest_passed",
|
|
||||||
directory,
|
|
||||||
)
|
|
||||||
return summary, release_ready
|
|
||||||
|
|||||||
@@ -1 +1 @@
|
|||||||
"""ROS message/service shells are migrated here after controller extraction."""
|
"""ROS message/service shells selected through the local profile registry."""
|
||||||
|
|||||||
@@ -0,0 +1,23 @@
|
|||||||
|
"""Generic ROS-node dispatcher selected by the registered profile identity."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import sys
|
||||||
|
|
||||||
|
from ...compat import legacy_default_profile_key
|
||||||
|
from ...core import ProfileKey
|
||||||
|
from ...models import get_default_registry
|
||||||
|
|
||||||
|
|
||||||
|
def main(args: list[str] | None = None) -> None:
|
||||||
|
arguments = list(sys.argv[1:] if args is None else args)
|
||||||
|
selector = argparse.ArgumentParser(add_help=False)
|
||||||
|
selector.add_argument("--profile-id", default=None)
|
||||||
|
selected, remaining = selector.parse_known_args(arguments)
|
||||||
|
key = (
|
||||||
|
legacy_default_profile_key()
|
||||||
|
if selected.profile_id is None
|
||||||
|
else ProfileKey.parse(selected.profile_id)
|
||||||
|
)
|
||||||
|
get_default_registry().get(key).engine.node_main(remaining)
|
||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -26,7 +26,7 @@ setup(
|
|||||||
zip_safe=True,
|
zip_safe=True,
|
||||||
maintainer="lxp",
|
maintainer="lxp",
|
||||||
maintainer_email="support@linker-robotics.com",
|
maintainer_email="support@linker-robotics.com",
|
||||||
description="Profile-driven three-camera AprilTag hand calibration",
|
description="Profile-driven hand calibration and validated URDF correction",
|
||||||
license="MIT",
|
license="MIT",
|
||||||
entry_points={
|
entry_points={
|
||||||
"console_scripts": [
|
"console_scripts": [
|
||||||
@@ -45,7 +45,7 @@ setup(
|
|||||||
),
|
),
|
||||||
(
|
(
|
||||||
"three_camera_calibration_node = "
|
"three_camera_calibration_node = "
|
||||||
"linkerhand_calibration.three_camera_node:main"
|
"linkerhand_calibration.runtime.nodes.calibration:main"
|
||||||
),
|
),
|
||||||
(
|
(
|
||||||
"three_camera_extrinsics_node = "
|
"three_camera_extrinsics_node = "
|
||||||
@@ -55,6 +55,10 @@ setup(
|
|||||||
"offline_replay = "
|
"offline_replay = "
|
||||||
"linkerhand_calibration.offline_replay:main"
|
"linkerhand_calibration.offline_replay:main"
|
||||||
),
|
),
|
||||||
|
(
|
||||||
|
"validate_g20_goldens = "
|
||||||
|
"linkerhand_calibration.models.g20.golden_regression:main"
|
||||||
|
),
|
||||||
(
|
(
|
||||||
"camera_alignment_view = "
|
"camera_alignment_view = "
|
||||||
"linkerhand_calibration.alignment_view:main"
|
"linkerhand_calibration.alignment_view:main"
|
||||||
|
|||||||
@@ -25,13 +25,18 @@ from linkerhand_calibration.core.solver import (
|
|||||||
TaskEvaluation,
|
TaskEvaluation,
|
||||||
)
|
)
|
||||||
from linkerhand_calibration.core.urdf import UrdfCorrectionPlan
|
from linkerhand_calibration.core.urdf import UrdfCorrectionPlan
|
||||||
|
from linkerhand_calibration.extrinsics import (
|
||||||
|
validate_camera_extrinsics_payload,
|
||||||
|
)
|
||||||
from linkerhand_calibration.models import (
|
from linkerhand_calibration.models import (
|
||||||
EngineBindings,
|
EngineBindings,
|
||||||
ProfileRegistry,
|
ProfileRegistry,
|
||||||
RegisteredProfile,
|
RegisteredProfile,
|
||||||
get_default_registry,
|
get_default_registry,
|
||||||
)
|
)
|
||||||
|
from linkerhand_calibration.product import get_product_calibration_contract
|
||||||
from linkerhand_calibration.runtime import SessionController, SessionState
|
from linkerhand_calibration.runtime import SessionController, SessionState
|
||||||
|
from linkerhand_calibration.runtime.nodes import calibration as calibration_node
|
||||||
|
|
||||||
|
|
||||||
PACKAGE = Path(__file__).resolve().parents[1] / "linkerhand_calibration"
|
PACKAGE = Path(__file__).resolve().parents[1] / "linkerhand_calibration"
|
||||||
@@ -122,6 +127,7 @@ def _bindings() -> EngineBindings:
|
|||||||
preparation_waypoints=no_waypoints,
|
preparation_waypoints=no_waypoints,
|
||||||
return_waypoints=no_waypoints,
|
return_waypoints=no_waypoints,
|
||||||
cli_main=lambda args=None: None,
|
cli_main=lambda args=None: None,
|
||||||
|
node_main=lambda args=None: None,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -157,6 +163,81 @@ def test_registry_accepts_different_view_and_command_counts() -> None:
|
|||||||
assert selected.vision.view_names == ("oblique", "wrist")
|
assert selected.vision.view_names == ("oblique", "wrist")
|
||||||
|
|
||||||
|
|
||||||
|
def test_generic_extrinsics_validator_uses_profile_view_names() -> None:
|
||||||
|
identity = {
|
||||||
|
"translation_xyz_m": [0.0, 0.0, 0.0],
|
||||||
|
"quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
||||||
|
}
|
||||||
|
payload = {
|
||||||
|
"schema_version": 1,
|
||||||
|
"reference_view": "oblique",
|
||||||
|
"cameras": {
|
||||||
|
"oblique": {
|
||||||
|
"serial_number": "A",
|
||||||
|
"width": 640,
|
||||||
|
"height": 480,
|
||||||
|
"intrinsics_sha256": "a" * 64,
|
||||||
|
},
|
||||||
|
"wrist": {
|
||||||
|
"serial_number": "B",
|
||||||
|
"width": 800,
|
||||||
|
"height": 600,
|
||||||
|
"intrinsics_sha256": "b" * 64,
|
||||||
|
},
|
||||||
|
},
|
||||||
|
"oblique_from_view": {
|
||||||
|
"oblique": identity,
|
||||||
|
"wrist": {
|
||||||
|
"translation_xyz_m": [0.1, 0.0, 0.0],
|
||||||
|
"quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
||||||
|
},
|
||||||
|
},
|
||||||
|
"quality": {"passed": True, "rms": 0.2, "pair_captures": 4},
|
||||||
|
}
|
||||||
|
|
||||||
|
validate_camera_extrinsics_payload(
|
||||||
|
payload,
|
||||||
|
required_views=("oblique", "wrist"),
|
||||||
|
reference_view="oblique",
|
||||||
|
quality_limits={"rms": 0.5},
|
||||||
|
minimum_capture_counts={"pair_captures": 3},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_ros_node_dispatcher_uses_profile_registry_without_model_branch(
|
||||||
|
monkeypatch,
|
||||||
|
) -> None:
|
||||||
|
profile = _small_profile()
|
||||||
|
forwarded: list[list[str] | None] = []
|
||||||
|
bindings = _bindings()
|
||||||
|
bindings = EngineBindings(
|
||||||
|
hand_profile=bindings.hand_profile,
|
||||||
|
zero_profile=bindings.zero_profile,
|
||||||
|
motion_command=bindings.motion_command,
|
||||||
|
preparation_waypoints=bindings.preparation_waypoints,
|
||||||
|
return_waypoints=bindings.return_waypoints,
|
||||||
|
cli_main=bindings.cli_main,
|
||||||
|
node_main=forwarded.append,
|
||||||
|
)
|
||||||
|
registry = ProfileRegistry()
|
||||||
|
registry.register(RegisteredProfile(profile, bindings))
|
||||||
|
monkeypatch.setattr(
|
||||||
|
calibration_node, "get_default_registry", lambda: registry
|
||||||
|
)
|
||||||
|
|
||||||
|
calibration_node.main(
|
||||||
|
[
|
||||||
|
"--profile-id",
|
||||||
|
profile.key.profile_id,
|
||||||
|
"--ros-args",
|
||||||
|
"-p",
|
||||||
|
"sample_rate:=30",
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
assert forwarded == [["--ros-args", "-p", "sample_rate:=30"]]
|
||||||
|
|
||||||
|
|
||||||
def test_schema_v2_selects_profile_only_by_typed_profile_id() -> None:
|
def test_schema_v2_selects_profile_only_by_typed_profile_id() -> None:
|
||||||
key = product_profile_key(
|
key = product_profile_key(
|
||||||
{
|
{
|
||||||
@@ -167,6 +248,13 @@ def test_schema_v2_selects_profile_only_by_typed_profile_id() -> None:
|
|||||||
assert key == ProfileKey("TEST", "right", "two_view_three_command", 3)
|
assert key == ProfileKey("TEST", "right", "two_view_three_command", 3)
|
||||||
|
|
||||||
|
|
||||||
|
def test_retired_layout_alias_is_confined_to_compatibility_resolution() -> None:
|
||||||
|
contract = get_product_calibration_contract(
|
||||||
|
"G20", "right", "g20_right_15"
|
||||||
|
)
|
||||||
|
assert contract.typed_profile.key.layout == "g20_right_19"
|
||||||
|
|
||||||
|
|
||||||
def test_controller_uses_one_evaluator_and_solver_contract() -> None:
|
def test_controller_uses_one_evaluator_and_solver_contract() -> None:
|
||||||
profile = _small_profile()
|
profile = _small_profile()
|
||||||
|
|
||||||
|
|||||||
@@ -9,6 +9,7 @@ from xml.etree import ElementTree as ET
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
import pytest
|
import pytest
|
||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
|
import yaml
|
||||||
|
|
||||||
from linkerhand_calibration.core import (
|
from linkerhand_calibration.core import (
|
||||||
DIRECTION_DECREASING,
|
DIRECTION_DECREASING,
|
||||||
@@ -190,13 +191,13 @@ def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None:
|
|||||||
G20_RIGHT_19_LAYOUT,
|
G20_RIGHT_19_LAYOUT,
|
||||||
)
|
)
|
||||||
assert config.calibration_contract.profile.command_count == 20
|
assert config.calibration_contract.profile.command_count == 20
|
||||||
assert config.calibration_contract.profile.supports(
|
assert config.calibration_contract.typed_profile.artifacts.publish_corrected_urdf
|
||||||
"urdf_zero_publication"
|
assert (
|
||||||
)
|
config.calibration_contract.typed_profile.measurement
|
||||||
assert config.calibration_contract.profile.supports(
|
.stable_cross_view_cone_bias
|
||||||
"stable_cross_view_cone_bias"
|
|
||||||
)
|
)
|
||||||
assert config.serial_number == "G20_RIGHT_001"
|
assert config.serial_number == "G20_RIGHT_001"
|
||||||
|
assert config.namespace == "/g20_calibration"
|
||||||
assert config.required_independent_passes == 1
|
assert config.required_independent_passes == 1
|
||||||
assert set(config.cameras) == {"front", "side", "top"}
|
assert set(config.cameras) == {"front", "side", "top"}
|
||||||
assert len({camera["serial_number"] for camera in config.cameras.values()}) == 3
|
assert len({camera["serial_number"] for camera in config.cameras.values()}) == 3
|
||||||
@@ -214,6 +215,26 @@ def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None:
|
|||||||
assert "src/g20_thumb_apriltag_calibration/config/" in product_text
|
assert "src/g20_thumb_apriltag_calibration/config/" in product_text
|
||||||
|
|
||||||
|
|
||||||
|
def test_schema_v2_loads_the_same_registered_product(tmp_path) -> None:
|
||||||
|
payload = yaml.safe_load(PRODUCT.read_text(encoding="utf-8"))
|
||||||
|
payload["schema_version"] = 2
|
||||||
|
payload["profile_id"] = "G20/right/g20_right_19/v1"
|
||||||
|
payload.pop("model")
|
||||||
|
payload.pop("side")
|
||||||
|
payload.pop("tag_layout")
|
||||||
|
config_path = tmp_path / "product_v2.yaml"
|
||||||
|
config_path.write_text(
|
||||||
|
yaml.safe_dump(payload, allow_unicode=True, sort_keys=False),
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
|
||||||
|
config = load_product_config(config_path, workspace=REPO, check_can=False)
|
||||||
|
|
||||||
|
assert config.schema_version == 2
|
||||||
|
assert config.profile_key.profile_id == "G20/right/g20_right_19/v1"
|
||||||
|
assert config.calibration_contract.typed_profile.key == config.profile_key
|
||||||
|
|
||||||
|
|
||||||
def test_first_round_uses_nine_bidirectional_steady_commands() -> None:
|
def test_first_round_uses_nine_bidirectional_steady_commands() -> None:
|
||||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||||
spec = profile.sweep_specs[0]
|
spec = profile.sweep_specs[0]
|
||||||
|
|||||||
@@ -0,0 +1,72 @@
|
|||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from linkerhand_calibration.models.g20.golden_regression import (
|
||||||
|
validate_golden_sessions,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _write(path: Path, value: str) -> str:
|
||||||
|
path.write_text(value, encoding="utf-8")
|
||||||
|
return hashlib.sha256(value.encode("utf-8")).hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
def test_static_golden_validation_covers_pass_fail_and_partial_scopes(
|
||||||
|
tmp_path: Path,
|
||||||
|
) -> None:
|
||||||
|
passed = {
|
||||||
|
"20260830_181154": ("full", {}),
|
||||||
|
"20260831_141123": (
|
||||||
|
"thumb",
|
||||||
|
{"preserved_certified_zero_joints": [f"joint_{i}" for i in range(12)]},
|
||||||
|
),
|
||||||
|
"20260831_163843": ("thumb", {"standalone": True}),
|
||||||
|
}
|
||||||
|
for session_id, (scope, extras) in passed.items():
|
||||||
|
session = tmp_path / session_id
|
||||||
|
session.mkdir()
|
||||||
|
payload = (
|
||||||
|
{"artifact_type": "g20_right_standalone_thumb_calibration"}
|
||||||
|
if extras.pop("standalone", False)
|
||||||
|
else {"artifact_type": "full"}
|
||||||
|
)
|
||||||
|
json_text = json.dumps(payload) + "\n"
|
||||||
|
json_hash = _write(session / "calibration.json", json_text)
|
||||||
|
urdf_hash = _write(session / "corrected.urdf", f"<{session_id}/>\n")
|
||||||
|
summary = {
|
||||||
|
"result": "PASS",
|
||||||
|
"calibration_scope": scope,
|
||||||
|
"artifacts": {"json": "calibration.json", "urdf": "corrected.urdf"},
|
||||||
|
"hashes": {
|
||||||
|
"calibration_json_sha256": json_hash,
|
||||||
|
"corrected_urdf_sha256": urdf_hash,
|
||||||
|
},
|
||||||
|
**extras,
|
||||||
|
}
|
||||||
|
(session / "calibration_summary_zh.json").write_text(
|
||||||
|
json.dumps(summary), encoding="utf-8"
|
||||||
|
)
|
||||||
|
|
||||||
|
failures = {
|
||||||
|
"20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"),
|
||||||
|
"20260831_142322": (
|
||||||
|
"VAL-QUALITY-501",
|
||||||
|
"zero_model_validation_failed",
|
||||||
|
),
|
||||||
|
}
|
||||||
|
for session_id, (code, reason) in failures.items():
|
||||||
|
session = tmp_path / session_id
|
||||||
|
session.mkdir()
|
||||||
|
(session / "calibration_summary_zh.json").write_text(
|
||||||
|
json.dumps(
|
||||||
|
{"result": "FAIL", "error_code": code, "reason": reason}
|
||||||
|
),
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
|
||||||
|
(tmp_path / "latest_passed").symlink_to("20260830_181154")
|
||||||
|
result = validate_golden_sessions(tmp_path, replay_full_session=False)
|
||||||
|
|
||||||
|
assert result["20260830_181154"]["result"] == "PASS"
|
||||||
|
assert result["20260831_111837"]["error_code"] == "FIT-MODEL-401"
|
||||||
@@ -1,5 +1,8 @@
|
|||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
|
from linkerhand_calibration.compat.legacy import (
|
||||||
|
uses_coupled_full_hand_zero_solver,
|
||||||
|
)
|
||||||
from linkerhand_calibration.offline_replay import (
|
from linkerhand_calibration.offline_replay import (
|
||||||
_latest_attempt_records,
|
_latest_attempt_records,
|
||||||
_latest_palm_axis_records,
|
_latest_palm_axis_records,
|
||||||
@@ -67,3 +70,17 @@ def test_output_suffix_is_safe_and_explicit() -> None:
|
|||||||
for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
|
for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
|
||||||
with pytest.raises(ValueError, match="output tag"):
|
with pytest.raises(ValueError, match="output tag"):
|
||||||
_output_suffix(invalid)
|
_output_suffix(invalid)
|
||||||
|
|
||||||
|
|
||||||
|
def test_v1_session_solver_selection_is_confined_to_compatibility() -> None:
|
||||||
|
legacy = {
|
||||||
|
"sample_schema_version": 1,
|
||||||
|
"capabilities": ["palm_axis_side_channel_v2"],
|
||||||
|
}
|
||||||
|
current = {
|
||||||
|
"sample_schema_version": 1,
|
||||||
|
"capabilities": ["palm_axis_relative_motion_v3"],
|
||||||
|
}
|
||||||
|
|
||||||
|
assert uses_coupled_full_hand_zero_solver(legacy)
|
||||||
|
assert not uses_coupled_full_hand_zero_solver(current)
|
||||||
|
|||||||
@@ -1,13 +1,21 @@
|
|||||||
import importlib
|
import importlib
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
import re
|
import re
|
||||||
|
import sys
|
||||||
|
|
||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
|
|
||||||
def test_legacy_python_package_forwards_only_public_config_api() -> None:
|
def test_legacy_python_package_forwards_only_public_config_api() -> None:
|
||||||
with pytest.warns(DeprecationWarning, match="linkerhand_calibration"):
|
with pytest.warns(DeprecationWarning, match="linkerhand_calibration"):
|
||||||
legacy = importlib.import_module("g20_thumb_apriltag_calibration")
|
if "g20_thumb_apriltag_calibration" in sys.modules:
|
||||||
|
legacy = importlib.reload(
|
||||||
|
sys.modules["g20_thumb_apriltag_calibration"]
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
legacy = importlib.import_module(
|
||||||
|
"g20_thumb_apriltag_calibration"
|
||||||
|
)
|
||||||
|
|
||||||
current = importlib.import_module("linkerhand_calibration.product")
|
current = importlib.import_module("linkerhand_calibration.product")
|
||||||
assert legacy.ProductConfig is current.ProductConfig
|
assert legacy.ProductConfig is current.ProductConfig
|
||||||
@@ -24,3 +32,7 @@ def test_new_and_legacy_executable_names_share_one_implementation() -> None:
|
|||||||
r'"linkerhand_calibration\.runtime\.runner:main"',
|
r'"linkerhand_calibration\.runtime\.runner:main"',
|
||||||
setup_text,
|
setup_text,
|
||||||
)
|
)
|
||||||
|
assert (
|
||||||
|
"linkerhand_calibration.runtime.nodes.calibration:main"
|
||||||
|
in setup_text
|
||||||
|
)
|
||||||
|
|||||||
Reference in New Issue
Block a user