5 Commits

128 changed files with 3912 additions and 440 deletions
@@ -1,5 +0,0 @@
"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb."""
from .core import BASELINE_COMMAND, COMMAND_NAMES
__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"]
@@ -1,4 +0,0 @@
[develop]
script_dir=$base/lib/g20_thumb_apriltag_calibration
[install]
install_scripts=$base/lib/g20_thumb_apriltag_calibration
@@ -1,17 +1,53 @@
# G20 左右手 AprilTag 标定
# LinkerHand 专业标定包
## G20右手正式一键标定
固定三相机和19张Tag安装完成后,用户只运行:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
ros2 run linkerhand_calibration calibrate_hand
```
旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名;
旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。
构建和正式调用统一为:
```bash
colcon build --packages-select linkerhand_calibration
ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml>
```
## 代码边界
- `core/`:无 ROS、无具体型号,包含领域类型、PnP/旋转数学、拟合接口、统一样本
契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。
- `runtime/`:通用会话状态机与注册 Profile 分发;ROS 消息和硬件适配只能位于
`runtime/nodes`、`runtime/adapters`。
- `models/g20/`:G20 right-19、legacy-11、运动、零位、产物和中文诊断策略。
后续型号或左右手作为新的独立 Profile 加入 `models/`,不在通用层增加分支。
- `compat/`:v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留
一版最小转发 shim,不包含算法副本。
产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、
Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改;v2 配置使用
`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明,通用层
不要求 `front/side/top`,也不假设固定 20 个命令。
仓库中存在已审定硬件会话时,可执行只读金标准检查(不会覆盖任何产物):
```bash
ros2 run linkerhand_calibration validate_g20_goldens \
calibration_output/G20_RIGHT_001
```
它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话;完整
整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。
完全独立地只标定大拇指4项任务时,使用:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
ros2 run linkerhand_calibration calibrate_hand \
--scope thumb
```
@@ -23,7 +59,7 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定,才额外使用:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
ros2 run linkerhand_calibration calibrate_hand \
--scope thumb \
--base-session calibration_output/G20_RIGHT_001/latest_passed
```
@@ -35,7 +71,7 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
只重新采集12项四指任务并合成完整整手URDF:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
ros2 run linkerhand_calibration calibrate_hand \
--scope fingers \
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
```
@@ -114,7 +150,7 @@ schema v4 JSON,不再生成schema v5运行文件。
`tag_layout:=legacy_11`,两套配置和结果schema互不覆盖:
```bash
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
hand_type:=right \
tag_layout:=g20_right_19 \
serial_number:=G20_RIGHT_001 \
@@ -271,7 +307,7 @@ ID 9 必须在拇指横摆的完整行程中持续可见。
```bash
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_extrinsics.launch.py \
output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
@@ -312,7 +348,7 @@ ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
先使用禁止运动模式检查三个机位、外参、内参和标签:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_calibration.launch.py \
hand_type:=left \
serial_number:=G20_LEFT_001 \
@@ -335,7 +371,7 @@ ros2 run image_view image_view --ros-args \
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后,重启正式流程:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_calibration.launch.py \
hand_type:=left \
serial_number:=G20_LEFT_001 \
@@ -347,7 +383,7 @@ ros2 launch g20_thumb_apriltag_calibration \
右手使用同一入口;默认自动选择右手SDK话题和原始URDF:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_calibration.launch.py \
hand_type:=right \
serial_number:=G20_RIGHT_001 \
@@ -526,7 +562,7 @@ calibration_output/G20_RIGHT_001/<时间戳>/
命令。`--output-tag` 为新产物增加安全后缀,已有JSON、URDF和验证报告不会被覆盖:
```bash
python3 -m g20_thumb_apriltag_calibration.offline_replay \
python3 -m linkerhand_calibration.offline_replay \
calibration_output/G20_RIGHT_001/20260811_120146 \
--output-tag AXIS_FRAME_V3 \
--write
@@ -543,7 +579,7 @@ python3 -m g20_thumb_apriltag_calibration.offline_replay \
`JointState`(包括5个被动关节):
```bash
ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
hand_type:=right \
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
```
@@ -615,7 +651,7 @@ sudo apt-get install -y \
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install \
--packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
--packages-select linker_hand_ros2_sdk linkerhand_calibration
source install/setup.bash
```
@@ -642,7 +678,7 @@ source install/setup.bash
先单独启动相机(不会连接机械手,也不会发送关节命令):
```bash
ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
ros2 run linkerhand_calibration hikrobot_camera_node --ros-args \
--remap __ns:=/camera/camera/color \
-p serial_number:=DB2163742 \
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
@@ -680,7 +716,7 @@ ros2 run camera_calibration cameracalibrator \
但标定节点不会发送位置运动命令,也不会允许解锁全行程扫描:
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
commands_enabled:=false
@@ -691,7 +727,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
速度,并使用单终点连续运动:
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
can_interface:=can0 \
@@ -804,7 +840,7 @@ calibration_output/<序列号>/<时间戳>/
恢复时必须显式复用原目录,否则会创建新会话:
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
```
@@ -884,7 +920,7 @@ for name, joint in data["joints"].items():
URDF:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
front_cmc_pitch_zero.launch.py \
serial_number:=G20_LEFT_001
```
@@ -961,7 +997,7 @@ zero_angles.table_projected_zero_rad
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量,不读取T3标签朝向。
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
front_cmc_roll_calibration.launch.py \
serial_number:=G20_LEFT_001
```
@@ -0,0 +1,21 @@
"""One-release compatibility surface for the former Python package name.
New code must import :mod:`linkerhand_calibration`. Only the documented
configuration loader is re-exported here; calibration algorithms continue to
have a single implementation in the renamed package.
"""
from __future__ import annotations
import warnings
warnings.warn(
"g20_thumb_apriltag_calibration is deprecated; "
"import linkerhand_calibration instead",
DeprecationWarning,
stacklevel=2,
)
from linkerhand_calibration.product import ProductConfig, load_product_config
__all__ = ["ProductConfig", "load_product_config"]
@@ -0,0 +1,9 @@
"""Deprecated forwarding entry point for the runtime joint-state bridge."""
from linkerhand_calibration.calibrated_joint_state_bridge import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -0,0 +1,9 @@
"""Deprecated forwarding entry point for offline replay."""
from linkerhand_calibration.offline_replay import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -0,0 +1,9 @@
"""Deprecated forwarding entry point for the former Python package."""
from linkerhand_calibration.one_command import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -14,7 +14,7 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("input_topic", default_value=""),
DeclareLaunchArgument("output_topic", default_value=""),
Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="calibrated_joint_state_bridge",
name=[
"g20_calibrated_joint_state_bridge_",
@@ -48,7 +48,7 @@ def _launch_stack(context):
tag_config = LaunchConfiguration("tag_config").perform(context)
zero_config = LaunchConfiguration("zero_config").perform(context)
camera = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -158,7 +158,7 @@ def _launch_stack(context):
)
zero_node = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="cmc_pitch_zero_node",
name="g20_thumb_cmc_pitch_zero",
output="screen",
@@ -203,7 +203,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
return LaunchDescription(
[
@@ -50,7 +50,7 @@ def _launch_stack(context):
"calibration_config"
).perform(context)
camera = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -160,7 +160,7 @@ def _launch_stack(context):
)
calibration_node = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="cmc_roll_calibration_node",
name="g20_thumb_cmc_roll_calibration",
output="screen",
@@ -205,7 +205,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
return LaunchDescription(
[
@@ -82,7 +82,7 @@ def _launch_stack(context):
)
camera = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -234,7 +234,7 @@ def _launch_stack(context):
)
calibration = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="calibration_node",
name="g20_thumb_calibration",
output="screen",
@@ -315,7 +315,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
default_output = str(Path.cwd() / "calibration_output")
return LaunchDescription(
@@ -39,7 +39,7 @@ def _default_source_urdf(hand_type: str) -> Path:
def _launch_stack(context):
from g20_thumb_apriltag_calibration.product import (
from linkerhand_calibration.product import (
get_product_calibration_contract,
)
@@ -56,7 +56,7 @@ def _launch_stack(context):
raise RuntimeError(str(error)) from error
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
tag_config = (
Path(requested_tag_config).expanduser().resolve()
@@ -88,7 +88,7 @@ def _launch_stack(context):
expected_source_hash = LaunchConfiguration(
"source_urdf_expected_sha256"
).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:
raise RuntimeError(
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
@@ -149,7 +149,7 @@ def _launch_stack(context):
detection_topics.append(detection_topic)
cameras.append(
Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace=namespace,
@@ -271,11 +271,15 @@ def _launch_stack(context):
],
)
calibration = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="three_camera_calibration_node",
name="g20_calibration",
output="screen",
emulate_tty=True,
arguments=[
"--profile-id",
contract.typed_profile.key.profile_id,
],
parameters=[
LaunchConfiguration("calibration_config"),
{
@@ -386,7 +390,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
info_root = Path.home() / ".ros" / "camera_info"
return LaunchDescription(
@@ -26,7 +26,7 @@ def _launch(context):
namespace = f"/g20_extrinsics/{view}/camera"
cameras.append(
Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace=namespace,
@@ -85,7 +85,7 @@ def _launch(context):
output="screen",
)
solver = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="three_camera_extrinsics_node",
name="g20_camera_extrinsics",
output="screen",
@@ -138,7 +138,7 @@ def _launch(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
camera_info = Path.home() / ".ros" / "camera_info"
return LaunchDescription(
@@ -0,0 +1,5 @@
"""Profile-driven LinkerHand calibration and validated URDF correction."""
from .core import CalibrationProfile, ProfileKey
__all__ = ["CalibrationProfile", "ProfileKey"]
@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
import numpy as np
from .core import PAIR_NAMES, robust_rotation_summary
from .core import robust_rotation_summary
from .pnp import SquareTagPose
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
"t3_t4": ("t3", "t4"),
"t4_t5": ("t4", "t5"),
}
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
@dataclass(frozen=True)
@@ -0,0 +1,21 @@
"""Compatibility adapters for one-release calibration migrations."""
from .config_v1 import (
legacy_default_profile_key,
product_profile_key,
resolve_legacy_profile_alias,
)
from .defaults import (
default_product_config_path,
default_three_camera_config_path,
)
from .paths import resolve_renamed_package_path
__all__ = [
"default_product_config_path",
"default_three_camera_config_path",
"legacy_default_profile_key",
"product_profile_key",
"resolve_legacy_profile_alias",
"resolve_renamed_package_path",
]
@@ -0,0 +1,47 @@
"""Identity migration for deployed product configuration schemas."""
from __future__ import annotations
from typing import Any, Mapping
from ..core import ProfileKey
def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
version = int(raw.get("schema_version", -1))
if version == 2:
key = ProfileKey.parse(str(raw.get("profile_id", "")))
for field, actual in (
("model", key.model),
("side", key.side),
("tag_layout", key.layout),
):
configured = str(raw.get(field, "")).strip()
if configured and configured.lower() != actual.lower():
raise ValueError(f"{field} differs from profile_id")
return key
if version != 1:
raise ValueError("product config schema_version must be 1 or 2")
model = str(raw.get("model", "")).strip().upper()
side = str(raw.get("side", "")).strip().lower()
layout = str(raw.get("tag_layout", "")).strip().lower()
if not layout and (model, side) == ("G20", "right"):
layout = "g20_right_19"
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
@@ -0,0 +1,16 @@
"""One-release default selection for invocations without ``--config``."""
from pathlib import Path
from ament_index_python.packages import get_package_share_directory
def default_product_config_path() -> Path:
share = Path(get_package_share_directory("linkerhand_calibration"))
return share / "config/g20_right_product.yaml"
def default_three_camera_config_path() -> Path:
"""Resolve the installed calibration defaults through the ROS index."""
share = Path(get_package_share_directory("linkerhand_calibration"))
return share / "config/three_camera_calibration.yaml"
@@ -0,0 +1,4 @@
"""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
)
@@ -0,0 +1,29 @@
"""Path compatibility for immutable v1 product configurations."""
from __future__ import annotations
from pathlib import Path
_LEGACY_SOURCE_PREFIX = Path("src/g20_thumb_apriltag_calibration")
_CURRENT_SOURCE_PREFIX = Path("src/linkerhand_calibration")
def resolve_renamed_package_path(value: str | Path, workspace: Path) -> Path:
"""Resolve a path and remap only the former source-package prefix.
Deployed v1 product YAML files are kept byte-for-byte stable because the
artifact paths participate in operational review. Existing paths always
win; the rename mapping is used only when the literal legacy path no
longer exists.
"""
raw = Path(value).expanduser()
candidate = raw if raw.is_absolute() else workspace / raw
candidate = candidate.resolve()
if candidate.exists() or raw.is_absolute():
return candidate
try:
suffix = raw.relative_to(_LEGACY_SOURCE_PREFIX)
except ValueError:
return candidate
return (workspace / _CURRENT_SOURCE_PREFIX / suffix).resolve()
@@ -0,0 +1,76 @@
"""Hardware- and model-independent calibration kernel."""
from .domain import (
ArtifactPolicy,
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
ProfileValidationError,
QualityPolicy,
SampleRecord,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
validate_profile,
)
from .domain.task import (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
DIRECTIONS,
PHASE_ROOT,
PHASE_TIP,
)
from .fitting import FitResult, isotonic_nonincreasing
from .geometry import (
delta_rotation_vector,
fit_rotation_axis,
image_plane_tag_quaternion_xyzw,
normalize_quaternion_xyzw,
relative_quaternion_xyzw,
robust_rotation_summary,
rotation_inlier_fraction,
rotation_rms_rad,
rotation_spread_rad,
)
__all__ = [
"ArtifactPolicy",
"CalibrationProfile",
"CommandLayout",
"DIRECTION_DECREASING",
"DIRECTION_INCREASING",
"DIRECTIONS",
"FitResult",
"MeasurementPolicy",
"MeasurementSpec",
"MotionPolicy",
"PHASE_ROOT",
"PHASE_TIP",
"ProfileKey",
"ProfileValidationError",
"QualityPolicy",
"SampleRecord",
"ScopePolicy",
"TagSpec",
"TaskSpec",
"ViewSpec",
"VisionRigSpec",
"ZeroSolvePolicy",
"delta_rotation_vector",
"fit_rotation_axis",
"image_plane_tag_quaternion_xyzw",
"isotonic_nonincreasing",
"normalize_quaternion_xyzw",
"relative_quaternion_xyzw",
"robust_rotation_summary",
"rotation_inlier_fraction",
"rotation_rms_rad",
"rotation_spread_rad",
"validate_profile",
]
@@ -0,0 +1,5 @@
"""Artifact schema and release validation contracts."""
from .release import ReleaseValidation, ReleaseValidator
__all__ = ["ReleaseValidation", "ReleaseValidator"]
@@ -0,0 +1,27 @@
"""Release validation protocol used before atomic publication."""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Mapping, Protocol
from ..domain import CalibrationProfile
from ..urdf import UrdfCorrectionPlan
@dataclass(frozen=True)
class ReleaseValidation:
passed: bool
errors: tuple[str, ...] = ()
verified_hashes: Mapping[str, str] | None = None
class ReleaseValidator(Protocol):
def validate_release(
self,
profile: CalibrationProfile,
plan: UrdfCorrectionPlan,
calibration_json: Path,
corrected_urdf: Path,
) -> ReleaseValidation: ...
@@ -0,0 +1,41 @@
"""Calibration domain types."""
from .profile import (
ArtifactPolicy,
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
ProfileValidationError,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
validate_profile,
)
from .sample import SampleRecord
__all__ = [
"ArtifactPolicy",
"CalibrationProfile",
"CommandLayout",
"MeasurementPolicy",
"MeasurementSpec",
"MotionPolicy",
"ProfileKey",
"ProfileValidationError",
"QualityPolicy",
"SampleRecord",
"ScopePolicy",
"TagSpec",
"TaskSpec",
"ViewSpec",
"VisionRigSpec",
"ZeroSolvePolicy",
"validate_profile",
]
@@ -0,0 +1,302 @@
"""Typed, hardware-independent calibration profile contracts."""
from __future__ import annotations
from dataclasses import dataclass, field
from pathlib import PurePath
from typing import Mapping
@dataclass(frozen=True, order=True)
class ProfileKey:
"""Stable identity for one independently reviewed hand profile."""
model: str
side: str
layout: str
revision: int = 1
def __post_init__(self) -> None:
object.__setattr__(self, "model", str(self.model).strip().upper())
object.__setattr__(self, "side", str(self.side).strip().lower())
object.__setattr__(self, "layout", str(self.layout).strip().lower())
object.__setattr__(self, "revision", int(self.revision))
if not self.model or not self.side or not self.layout:
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:
raise ValueError("profile revision must be positive")
@property
def profile_id(self) -> str:
return f"{self.model}/{self.side}/{self.layout}/v{self.revision}"
@classmethod
def parse(cls, value: str) -> "ProfileKey":
parts = str(value).strip().split("/")
if len(parts) != 4 or not parts[3].startswith("v"):
raise ValueError(
"profile_id must be MODEL/side/layout/vREVISION"
)
return cls(parts[0], parts[1], parts[2], int(parts[3][1:]))
@dataclass(frozen=True)
class CommandLayout:
"""Command channels, joint bindings, and the reviewed baseline pose."""
names: tuple[str, ...]
baseline_u8: tuple[int, ...]
command_index_by_joint: Mapping[str, int]
disabled_indices: frozenset[int] = frozenset()
@property
def command_count(self) -> int:
return len(self.names)
@dataclass(frozen=True)
class TagSpec:
role: str
tag_id: int
fixed_reference: bool = False
@dataclass(frozen=True)
class ViewSpec:
name: str
tags: tuple[TagSpec, ...]
@dataclass(frozen=True)
class VisionRigSpec:
"""Any number of named views and their Tag roles."""
views: tuple[ViewSpec, ...]
common_frame: 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
def view_names(self) -> tuple[str, ...]:
return tuple(view.name for view in self.views)
@property
def tag_ids(self) -> frozenset[int]:
return frozenset(tag.tag_id for view in self.views for tag in view.tags)
@dataclass(frozen=True)
class TaskSpec:
key: str
view: str
command_index: int
joints: tuple[str, ...]
auxiliary_commands: tuple[tuple[int, int], ...] = ()
validation_only: bool = False
@dataclass(frozen=True)
class MotionPolicy:
"""Reviewed motion tasks and optional safe waypoint sequences."""
tasks: tuple[TaskSpec, ...]
preparation_waypoints_u8: tuple[tuple[int, ...], ...] = ()
safe_return_waypoints_u8: tuple[tuple[int, ...], ...] = ()
speed_parameters: Mapping[str, float] = field(default_factory=dict)
precheck_sweeps: bool = False
steady_command_checkpoints: bool = False
@dataclass(frozen=True)
class MeasurementSpec:
joint: str
kind: str
view: str | None
parent_role: str | None
child_role: str | None
validation_source: str | None = None
# Some measured trajectories publish only a dynamic curve while their
# static URDF zero/axis remains CAD- or mimic-owned. For those joints a
# monocular 3-D axis-line residual is useful diagnostic evidence, but it
# must not reject an otherwise clean image/SO(3) trajectory merely because
# the hand was placed at a different valid position in the camera view.
pose_axis_line_required: bool = True
@dataclass(frozen=True)
class MeasurementPolicy:
measurements: Mapping[str, MeasurementSpec]
cross_view_sources: Mapping[str, str] = field(default_factory=dict)
image_curve_joints: frozenset[str] = frozenset()
directional_zero: bool = False
cross_view_roll_curve: bool = False
stable_cross_view_cone_bias: bool = False
@dataclass(frozen=True)
class ZeroSolvePolicy:
active_joints: frozenset[str]
passive_joints: frozenset[str]
direct_zero_joints: tuple[str, ...]
axis_joints: tuple[str, ...]
mechanical_endpoint_joints: frozenset[str]
post_solve_endpoint_joints: frozenset[str]
mimic_source_by_joint: Mapping[str, str]
cad_frozen_joints: frozenset[str]
@dataclass(frozen=True)
class QualityPolicy:
training_cycles: tuple[int, ...]
holdout_cycle: int | None
hard_threshold_keys: frozenset[str]
retry_metric_scope: Mapping[str, str] = field(default_factory=dict)
isolated_holdout: bool = False
@dataclass(frozen=True)
class ScopePolicy:
calibrate_joints: Mapping[str, frozenset[str]]
frozen_joints: Mapping[str, frozenset[str]]
default_scope: str = "full"
def selected_joints(self, scope: str) -> frozenset[str]:
try:
return self.calibrate_joints[str(scope)]
except KeyError as error:
raise ValueError(f"unsupported calibration scope: {scope}") from error
@dataclass(frozen=True)
class ArtifactPolicy:
output_schema_version: int
calibration_filename: str
corrected_urdf_filename: str
protected_input_fields: frozenset[str]
publication_pointer: str = "latest_passed"
session_compatibility_tokens: frozenset[str] = frozenset()
publish_corrected_urdf: bool = False
@dataclass(frozen=True)
class CalibrationProfile:
key: ProfileKey
namespace: str
command: CommandLayout
vision: VisionRigSpec
motion: MotionPolicy
measurement: MeasurementPolicy
zero: ZeroSolvePolicy
quality: QualityPolicy
scope: ScopePolicy
artifacts: ArtifactPolicy
class ProfileValidationError(ValueError):
"""Raised before hardware startup when a profile is internally unsafe."""
def validate_profile(profile: CalibrationProfile) -> None:
"""Hard-check all cross-policy references before hardware is enabled."""
errors: list[str] = []
command = profile.command
if not command.names or len(command.names) != len(command.baseline_u8):
errors.append("command names and baseline must be non-empty and aligned")
if len(set(command.names)) != len(command.names):
errors.append("command names must be unique")
if any(value < 0 or value > 255 for value in command.baseline_u8):
errors.append("baseline command values must be in [0, 255]")
indices = set(range(command.command_count))
if not set(command.disabled_indices).issubset(indices):
errors.append("disabled command index is out of range")
if any(index not in indices for index in command.command_index_by_joint.values()):
errors.append("joint command index is out of range")
view_names = profile.vision.view_names
if not view_names or len(set(view_names)) != len(view_names):
errors.append("vision views must be non-empty and unique")
if profile.vision.extrinsic_reference_view not in view_names:
errors.append("extrinsic reference view is not declared")
tag_ids = [tag.tag_id for view in profile.vision.views for tag in view.tags]
tag_roles = [tag.role for view in profile.vision.views for tag in view.tags]
if len(set(tag_ids)) != len(tag_ids):
errors.append("Tag IDs must be unique across views")
if len(set(tag_roles)) != len(tag_roles):
errors.append("Tag roles must be unique across views")
if not any(
tag.fixed_reference for view in profile.vision.views for tag in view.tags
):
errors.append("at least one fixed reference Tag is required")
task_keys = [task.key for task in profile.motion.tasks]
if not task_keys or len(set(task_keys)) != len(task_keys):
errors.append("motion task keys must be non-empty and unique")
measurement_names = set(profile.measurement.measurements)
for task in profile.motion.tasks:
if task.view not in view_names:
errors.append(f"task {task.key} uses an unknown view")
if task.command_index not in indices:
errors.append(f"task {task.key} command index is out of range")
if not task.joints or not set(task.joints).issubset(measurement_names):
errors.append(f"task {task.key} references unknown measurements")
if any(index not in indices for index, _ in task.auxiliary_commands):
errors.append(f"task {task.key} auxiliary index is out of range")
for name, spec in profile.measurement.measurements.items():
if name != spec.joint:
errors.append(f"measurement mapping key differs for {name}")
if spec.view is not None and spec.view not in view_names:
errors.append(f"measurement {name} uses an unknown view")
for primary, validation in profile.measurement.cross_view_sources.items():
if primary not in measurement_names or validation not in measurement_names:
errors.append("cross-view measurement source is unknown")
zero = profile.zero
if zero.active_joints & zero.passive_joints:
errors.append("active and passive joints must be disjoint")
all_joints = zero.active_joints | zero.passive_joints
if not zero.active_joints.issubset(command.command_index_by_joint):
errors.append("every active joint must bind to a command channel")
if not set(zero.direct_zero_joints).issubset(zero.active_joints):
errors.append("direct zero targets must be active joints")
if not set(zero.axis_joints).issubset(all_joints):
errors.append("axis targets must be known joints")
if not zero.mechanical_endpoint_joints.issubset(zero.active_joints):
errors.append("mechanical endpoint targets must be active joints")
if not zero.post_solve_endpoint_joints.issubset(zero.active_joints):
errors.append("post-solve endpoint targets must be active joints")
if not set(zero.mimic_source_by_joint).issubset(zero.passive_joints):
errors.append("mimic targets must be passive joints")
if not set(zero.mimic_source_by_joint.values()).issubset(all_joints):
errors.append("mimic sources must be known joints")
scopes = set(profile.scope.calibrate_joints)
if profile.scope.default_scope not in scopes:
errors.append("default scope is not declared")
if scopes != set(profile.scope.frozen_joints):
errors.append("scope calibration and frozen mappings must align")
for name in scopes:
selected = profile.scope.calibrate_joints[name]
frozen = profile.scope.frozen_joints[name]
if selected & frozen or selected | frozen != zero.active_joints:
errors.append(f"scope {name} must partition all active joints")
artifacts = profile.artifacts
if artifacts.output_schema_version < 1:
errors.append("artifact schema version must be positive")
for label, filename in (
("calibration", artifacts.calibration_filename),
("corrected URDF", artifacts.corrected_urdf_filename),
("publication pointer", artifacts.publication_pointer),
):
if not filename or PurePath(filename).name != filename:
errors.append(f"{label} filename must not contain a directory")
if not profile.namespace.startswith("/"):
errors.append("runtime namespace must be absolute")
if errors:
raise ProfileValidationError("; ".join(errors))
@@ -0,0 +1,27 @@
"""Normalized records shared by online evaluation and offline replay."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Mapping
@dataclass(frozen=True)
class SampleRecord:
task_key: str
measurement: str
view: str
cycle: int
direction: str
command_u8: int
timestamp_ns: int
values: Mapping[str, Any]
quality: Mapping[str, float] = field(default_factory=dict)
def __post_init__(self) -> None:
if not self.task_key or not self.measurement or not self.view:
raise ValueError("sample task, measurement, and view are required")
if self.cycle < 0 or not 0 <= self.command_u8 <= 255:
raise ValueError("sample cycle or command is out of range")
if self.timestamp_ns < 0:
raise ValueError("sample timestamp must be non-negative")
@@ -0,0 +1,19 @@
"""Shared task direction vocabulary."""
DIRECTION_DECREASING = "decreasing"
DIRECTION_INCREASING = "increasing"
DIRECTIONS: tuple[str, ...] = (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
)
PHASE_ROOT = "root"
PHASE_TIP = "tip"
__all__ = [
"DIRECTION_DECREASING",
"DIRECTION_INCREASING",
"DIRECTIONS",
"PHASE_ROOT",
"PHASE_TIP",
]
@@ -0,0 +1,5 @@
"""Pure curve and axis fitting."""
from .curve import FitResult, isotonic_nonincreasing
__all__ = ["FitResult", "isotonic_nonincreasing"]
@@ -0,0 +1,70 @@
"""Model-independent curve fitting result and monotonic projection."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Sequence
import numpy as np
from ..geometry import delta_rotation_vector
def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
"""Unweighted PAVA projection onto non-increasing values."""
original = np.asarray(values, dtype=float)
if original.ndim != 1 or not np.all(np.isfinite(original)):
raise ValueError("values must be a finite vector")
negated = -original
levels: list[float] = []
weights: list[int] = []
starts: list[int] = []
for index, value in enumerate(negated):
levels.append(float(value))
weights.append(1)
starts.append(index)
while len(levels) >= 2 and levels[-2] > levels[-1]:
total_weight = weights[-2] + weights[-1]
merged = (
levels[-2] * weights[-2] + levels[-1] * weights[-1]
) / total_weight
levels[-2:] = [merged]
weights[-2:] = [total_weight]
starts.pop()
projected = np.empty_like(original)
for block_index, (level, start) in enumerate(zip(levels, starts)):
end = (
starts[block_index + 1]
if block_index + 1 < len(starts)
else len(original)
)
projected[start:end] = -level
return projected
@dataclass(frozen=True)
class FitResult:
joints: dict[str, dict[str, Any]]
axes: dict[str, tuple[float, float, float]]
references: dict[str, tuple[float, float, float, float]]
ip_coupling: dict[str, float]
max_monotonic_correction_rad: float
max_hysteresis_rad: float
measurement_mode: str = "rotation"
trajectory_models: dict[str, Any] = field(default_factory=dict)
trajectory_quality: dict[str, Any] = field(default_factory=dict)
def measure_from_reference(
self,
joint_name: str,
observed_quaternion_xyzw: Sequence[float],
reference_quaternion_xyzw: Sequence[float] | None = None,
) -> float:
reference = (
reference_quaternion_xyzw
if reference_quaternion_xyzw is not None
else self.references[joint_name]
)
vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
axis = np.asarray(self.axes[joint_name], dtype=float)
return float(vector @ axis)
@@ -0,0 +1,41 @@
"""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 (
delta_rotation_vector,
fit_rotation_axis,
image_plane_tag_quaternion_xyzw,
normalize_quaternion_xyzw,
relative_quaternion_xyzw,
robust_rotation_summary,
rotation_inlier_fraction,
rotation_rms_rad,
rotation_spread_rad,
)
__all__ = [
"CameraCalibrationIdentity",
"CameraExtrinsics",
"camera_info_fingerprint",
"delta_rotation_vector",
"fit_rotation_axis",
"image_plane_tag_quaternion_xyzw",
"load_camera_extrinsics",
"matrix_payload",
"normalize_quaternion_xyzw",
"relative_quaternion_xyzw",
"robust_rotation_summary",
"rotation_inlier_fraction",
"rotation_rms_rad",
"rotation_spread_rad",
"transform_matrix",
"validate_camera_extrinsics_payload",
]
@@ -13,9 +13,6 @@ import yaml
from scipy.spatial.transform import Rotation
VIEWS: tuple[str, ...] = ("front", "side", "top")
def camera_info_fingerprint(
*,
width: int,
@@ -81,66 +78,61 @@ class CameraCalibrationIdentity:
@dataclass(frozen=True)
class ThreeCameraExtrinsics:
"""Transforms points from each camera optical frame into front optical."""
class CameraExtrinsics:
"""Transforms every declared camera into one Profile-selected reference."""
cameras: Mapping[str, CameraCalibrationIdentity]
front_from_view: Mapping[str, np.ndarray]
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.front_from_view:
if view not in self.reference_from_view:
raise KeyError(f"extrinsics do not contain view {view}")
return np.asarray(self.front_from_view[view], dtype=float).copy()
def camera_matches(
self,
view: str,
*,
serial_number: str,
width: int,
height: int,
intrinsics_sha256: str,
) -> bool:
expected = self.cameras.get(view)
return bool(
expected is not None
and expected.serial_number == str(serial_number)
and expected.width == int(width)
and expected.height == int(height)
and expected.intrinsics_sha256 == str(intrinsics_sha256)
)
return np.asarray(self.reference_from_view[view], dtype=float).copy()
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
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")
if payload.get("reference_view") != "front":
raise ValueError("camera extrinsics reference_view must be front")
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("front_from_view")
transforms = payload.get(f"{reference}_from_view")
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")
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")
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():
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 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:
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")
@@ -158,14 +150,23 @@ def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
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):
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_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
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}")
@@ -173,7 +174,15 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
payload = yaml.safe_load(stream)
if not isinstance(payload, Mapping):
raise ValueError("camera extrinsics file must contain an object")
validate_extrinsics_payload(payload)
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"]),
@@ -183,45 +192,21 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
payload["cameras"][view]["intrinsics_sha256"]
),
)
for view in VIEWS
for view in views
}
transforms = {
view: transform_matrix(
payload["front_from_view"][view]["translation_xyz_m"],
payload["front_from_view"][view]["quaternion_xyzw"],
payload[transform_key][view]["translation_xyz_m"],
payload[transform_key][view]["quaternion_xyzw"],
)
for view in VIEWS
for view in views
}
return ThreeCameraExtrinsics(
return CameraExtrinsics(
cameras=cameras,
front_from_view=transforms,
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()
},
)
def dump_three_camera_extrinsics(
path: str | Path,
*,
cameras: Mapping[str, Mapping[str, Any]],
front_from_view: Mapping[str, Sequence[Sequence[float]]],
quality: Mapping[str, Any],
) -> None:
payload = {
"schema_version": 1,
"reference_view": "front",
"cameras": {view: dict(cameras[view]) for view in VIEWS},
"front_from_view": {
view: matrix_payload(front_from_view[view]) for view in VIEWS
},
"quality": dict(quality),
}
validate_extrinsics_payload(payload)
destination = Path(path).expanduser().resolve()
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_suffix(destination.suffix + ".tmp")
with temporary.open("w", encoding="utf-8") as stream:
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
temporary.replace(destination)
@@ -0,0 +1,157 @@
"""Pure quaternion summaries and rotation-axis fitting."""
from __future__ import annotations
import math
from typing import Sequence
import numpy as np
from scipy.spatial.transform import Rotation
def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
quaternion = np.asarray(values, dtype=float)
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
raise ValueError("quaternion must contain four finite xyzw values")
norm = float(np.linalg.norm(quaternion))
if norm < 1e-12:
raise ValueError("quaternion norm is zero")
return quaternion / norm
def relative_quaternion_xyzw(
parent_camera_quaternion: Sequence[float],
child_camera_quaternion: Sequence[float],
) -> tuple[float, float, float, float]:
"""Compute parent-to-child orientation from two camera-to-Tag rotations."""
parent = Rotation.from_quat(
normalize_quaternion_xyzw(parent_camera_quaternion)
)
child = Rotation.from_quat(
normalize_quaternion_xyzw(child_camera_quaternion)
)
quaternion = (parent.inv() * child).as_quat()
return tuple(float(value) for value in quaternion)
def image_plane_tag_quaternion_xyzw(
corners_xy: Sequence[Sequence[float]],
) -> tuple[float, float, float, float]:
"""Estimate Tag orientation about the optical axis from ordered corners."""
corners = np.asarray(corners_xy, dtype=float)
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
raise ValueError("corners_xy must contain four finite xy points")
x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
if float(np.linalg.norm(x_axis)) < 1e-9:
raise ValueError("tag x-axis is degenerate")
angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
return tuple(float(value) for value in quaternion)
def robust_rotation_summary(
quaternions_xyzw: Sequence[Sequence[float]],
) -> tuple[tuple[float, float, float, float], float]:
"""Return a robust orientation and maximum angular residual in radians."""
if not quaternions_xyzw:
raise ValueError("at least one quaternion is required")
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
reference = rotations[0]
delta_vectors = (reference.inv() * rotations).as_rotvec()
median_delta = np.median(delta_vectors, axis=0)
robust = reference * Rotation.from_rotvec(median_delta)
residuals = (robust.inv() * rotations).magnitude()
maximum = float(np.max(residuals)) if residuals.size else 0.0
return tuple(float(value) for value in robust.as_quat()), maximum
def rotation_spread_rad(
quaternions_xyzw: Sequence[Sequence[float]],
) -> float:
"""Return the maximum geodesic residual around a robust orientation."""
_, spread = robust_rotation_summary(quaternions_xyzw)
return spread
def rotation_rms_rad(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float | None = None,
) -> float:
"""Return RMS geodesic noise around a robust orientation."""
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
if outlier_threshold_rad is not None:
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
residuals = residuals[residuals <= threshold]
if residuals.size == 0:
return float("inf")
return float(np.sqrt(np.mean(np.square(residuals))))
def rotation_inlier_fraction(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float,
) -> float:
"""Return the fraction close to the robust orientation."""
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
return float(np.mean(residuals <= threshold))
def delta_rotation_vector(
reference_xyzw: Sequence[float],
observed_xyzw: Sequence[float],
) -> np.ndarray:
reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
return (reference.inv() * observed).as_rotvec()
def fit_rotation_axis(
vectors: Sequence[Sequence[float]],
commands: Sequence[int],
) -> np.ndarray:
"""Fit and orient the single rotational axis used by one command sweep."""
matrix = np.asarray(vectors, dtype=float)
command_values = np.asarray(commands, dtype=int)
if matrix.ndim != 2 or matrix.shape[1] != 3:
raise ValueError("vectors must have shape (N, 3)")
if command_values.shape != (matrix.shape[0],):
raise ValueError("commands must match vectors")
useful = np.linalg.norm(matrix, axis=1) > 1e-6
if int(np.count_nonzero(useful)) < 3:
raise ValueError("insufficient non-zero rotations to fit an axis")
_, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
axis = vh[0]
projections = matrix @ axis
low = projections[command_values <= 16]
high = projections[command_values >= 239]
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
axis = -axis
return axis / np.linalg.norm(axis)
@@ -0,0 +1,15 @@
"""Task acceptance and final-session solver contracts."""
from .interfaces import (
SessionSolution,
SessionSolver,
TaskEvaluation,
TaskEvaluator,
)
__all__ = [
"SessionSolution",
"SessionSolver",
"TaskEvaluation",
"TaskEvaluator",
]
@@ -0,0 +1,42 @@
"""Shared evaluator and final-solver interfaces."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Mapping, Protocol, Sequence
from ..domain import CalibrationProfile, SampleRecord, TaskSpec
@dataclass(frozen=True)
class TaskEvaluation:
accepted: bool
failures: tuple[Mapping[str, Any], ...] = ()
rescan_measurements: frozenset[str] = frozenset()
rescan_cycles: frozenset[int] = frozenset()
@dataclass(frozen=True)
class SessionSolution:
passed: bool
calibration: Mapping[str, Any]
zero_offsets_rad: Mapping[str, float]
failures: tuple[Mapping[str, Any], ...] = ()
diagnostics: Mapping[str, Any] = field(default_factory=dict)
class TaskEvaluator(Protocol):
def evaluate_task(
self,
profile: CalibrationProfile,
task: TaskSpec,
samples: Sequence[SampleRecord],
) -> TaskEvaluation: ...
class SessionSolver(Protocol):
def solve_session(
self,
profile: CalibrationProfile,
samples: Sequence[SampleRecord],
) -> SessionSolution: ...
@@ -0,0 +1,5 @@
"""URDF correction authorization and validation types."""
from .plan import UrdfCorrectionPlan, build_correction_plan
__all__ = ["UrdfCorrectionPlan", "build_correction_plan"]
@@ -0,0 +1,104 @@
"""One authorization plan shared by URDF writers and validators."""
from __future__ import annotations
from dataclasses import dataclass, field
import hashlib
from pathlib import Path
from typing import Mapping
from ..domain import CalibrationProfile
@dataclass(frozen=True)
class UrdfCorrectionPlan:
source_sha256: str
allowed_active_joints: frozenset[str]
endpoint_limit_joints: frozenset[str]
mimic_source_by_joint: Mapping[str, str]
frozen_joints: frozenset[str]
frozen_offsets_rad: Mapping[str, float] = field(default_factory=dict)
forbid_calibrated_source: bool = True
forbid_overwrite: bool = True
preserve_passive_joints: bool = True
def __post_init__(self) -> None:
if len(self.source_sha256) != 64 or any(
character not in "0123456789abcdef"
for character in self.source_sha256.lower()
):
raise ValueError("source URDF SHA-256 is invalid")
if self.allowed_active_joints & self.frozen_joints:
raise ValueError("allowed and frozen URDF joints overlap")
applied = self.allowed_active_joints | set(self.frozen_offsets_rad)
if not set(self.frozen_offsets_rad).issubset(self.frozen_joints):
raise ValueError("frozen offsets must belong to frozen joints")
if not self.endpoint_limit_joints.issubset(applied):
raise ValueError("endpoint limit joint is not an applied active joint")
if set(self.mimic_source_by_joint) & self.allowed_active_joints:
raise ValueError("dependent mimic joints cannot be active edit targets")
def authorize_offsets(self, offsets_rad: Mapping[str, float]) -> None:
required = self.allowed_active_joints | set(self.frozen_offsets_rad)
unexpected = set(offsets_rad) - required
if unexpected:
raise ValueError(
"URDF correction contains unauthorized joints: "
+ ", ".join(sorted(unexpected))
)
missing = required - set(offsets_rad)
if missing:
raise ValueError(
"URDF correction is missing active joints: "
+ ", ".join(sorted(missing))
)
changed_frozen = {
name
for name, expected in self.frozen_offsets_rad.items()
if abs(float(offsets_rad[name]) - float(expected)) > 1.0e-12
}
if changed_frozen:
raise ValueError(
"URDF correction changed frozen offsets: "
+ ", ".join(sorted(changed_frozen))
)
def verify_source(self, source_urdf: str | Path) -> None:
digest = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest()
if digest != self.source_sha256.lower():
raise ValueError("source URDF SHA-256 differs from correction plan")
def build_correction_plan(
profile: CalibrationProfile,
*,
source_sha256: str,
scope: str,
frozen_offsets_rad: Mapping[str, float] | None = None,
) -> UrdfCorrectionPlan:
"""Build one scope-aware edit authorization from typed policies."""
selected = profile.scope.selected_joints(scope)
frozen = profile.scope.frozen_joints[str(scope)]
expected_frozen = {
str(name): float(value)
for name, value in dict(frozen_offsets_rad or {}).items()
}
if set(expected_frozen) != set(frozen):
missing = set(frozen) - set(expected_frozen)
extra = set(expected_frozen) - set(frozen)
raise ValueError(
"frozen URDF offset state differs from scope policy: "
f"missing={','.join(sorted(missing)) or '-'};"
f"extra={','.join(sorted(extra)) or '-'}"
)
applied = selected | frozen
return UrdfCorrectionPlan(
source_sha256=source_sha256,
allowed_active_joints=selected,
endpoint_limit_joints=(
profile.zero.mechanical_endpoint_joints & applied
),
mimic_source_by_joint=profile.zero.mimic_source_by_joint,
frozen_joints=frozen | profile.zero.cad_frozen_joints,
frozen_offsets_rad=expected_frozen,
)
@@ -0,0 +1,130 @@
"""Three-view compatibility policy over generic camera extrinsics."""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Any, Mapping, Sequence
import numpy as np
import yaml
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")
_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,
}
@dataclass(frozen=True)
class ThreeCameraExtrinsics:
"""Transforms points from each camera optical frame into front optical."""
cameras: Mapping[str, CameraCalibrationIdentity]
front_from_view: Mapping[str, np.ndarray]
quality: Mapping[str, float]
def transform(self, view: str) -> np.ndarray:
if view not in self.front_from_view:
raise KeyError(f"extrinsics do not contain view {view}")
return np.asarray(self.front_from_view[view], dtype=float).copy()
def camera_matches(
self,
view: str,
*,
serial_number: str,
width: int,
height: int,
intrinsics_sha256: str,
) -> bool:
expected = self.cameras.get(view)
return bool(
expected is not None
and expected.serial_number == str(serial_number)
and expected.width == int(width)
and expected.height == int(height)
and expected.intrinsics_sha256 == str(intrinsics_sha256)
)
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
validate_camera_extrinsics_payload(
payload,
required_views=VIEWS,
reference_view="front",
quality_limits=_QUALITY_LIMITS,
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
)
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
generic = load_camera_extrinsics(
path,
required_views=VIEWS,
reference_view="front",
quality_limits=_QUALITY_LIMITS,
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
)
return ThreeCameraExtrinsics(
cameras=generic.cameras,
front_from_view=generic.reference_from_view,
quality=generic.quality,
)
def dump_three_camera_extrinsics(
path: str | Path,
*,
cameras: Mapping[str, Mapping[str, Any]],
front_from_view: Mapping[str, Sequence[Sequence[float]]],
quality: Mapping[str, Any],
) -> None:
payload = {
"schema_version": 1,
"reference_view": "front",
"cameras": {view: dict(cameras[view]) for view in VIEWS},
"front_from_view": {
view: matrix_payload(front_from_view[view]) for view in VIEWS
},
"quality": dict(quality),
}
validate_extrinsics_payload(payload)
destination = Path(path).expanduser().resolve()
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_suffix(destination.suffix + ".tmp")
with temporary.open("w", encoding="utf-8") as stream:
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
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",
]
@@ -0,0 +1,7 @@
"""One-release module alias for the relocated model profile implementation."""
import sys
from .models.g20 import profile as _implementation
sys.modules[__name__] = _implementation
@@ -203,7 +203,7 @@ def configure_fastdds_large_image_transport() -> Path:
from ament_index_python.packages import get_package_share_directory
profile = (
Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
Path(get_package_share_directory("linkerhand_calibration"))
/ "config"
/ "fastdds_large_images.xml"
)
@@ -0,0 +1,15 @@
"""Model- and side-specific calibration policies."""
from .registry import (
EngineBindings,
ProfileRegistry,
RegisteredProfile,
get_default_registry,
)
__all__ = [
"EngineBindings",
"ProfileRegistry",
"RegisteredProfile",
"get_default_registry",
]
@@ -0,0 +1,15 @@
"""Registered profiles for this hand family."""
from ..registry import ProfileRegistry
def register_profiles(registry: ProfileRegistry) -> None:
from .legacy_11 import build_left_profile, build_right_profile
from .right_19 import build_profile
registry.register(build_profile())
registry.register(build_left_profile())
registry.register(build_right_profile())
__all__ = ["register_profiles"]
@@ -0,0 +1,219 @@
"""Adapt reviewed family profiles to the shared typed contract."""
from __future__ import annotations
from ...core import (
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
)
from .profile import MIMIC_DERIVED_FINGER_DIPS
from ..registry import EngineBindings, RegisteredProfile
from .artifacts import build_artifact_policy
from .motion import (
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
)
_HARD_THRESHOLD_KEYS = frozenset(
{
"minimum_detection_rate",
"maximum_reprojection_error_px",
"maximum_axis_cycle_difference_rad",
"maximum_pose_line_rms_m",
"maximum_hysteresis_rad",
"maximum_validation_error_rad",
}
)
def _run_cli(args: list[str] | None = None) -> None:
from .runner import main
main(args)
def _run_node(args: list[str] | None = None) -> None:
from .node import main
main(args)
def adapt_profile(
*,
key: ProfileKey,
namespace: str,
hand_profile,
zero_profile,
mechanical_endpoint_joints: frozenset[str] = frozenset(),
post_solve_endpoint_joints: frozenset[str] = frozenset(),
) -> RegisteredProfile:
fixed_by_view = {
view: frozenset(roles)
for view, roles in hand_profile.preflight_view_roles.items()
}
views = tuple(
ViewSpec(
name=view,
tags=tuple(
TagSpec(
role=role,
tag_id=int(tag_id),
fixed_reference=role in fixed_by_view.get(view, frozenset()),
)
for role, tag_id in roles.items()
),
)
for view, roles in hand_profile.view_tags.items()
)
record_specs = hand_profile.record_specs
command_index_by_joint = {
name: int(spec.motor_index) for name, spec in record_specs.items()
}
command_index_by_joint.update(
{
name: int(spec.motor_index)
for name, spec in hand_profile.joint_specs.items()
}
)
measurements = {
name: MeasurementSpec(
joint=name,
kind=str(spec.zero_kind or "curve"),
view=spec.view,
parent_role=spec.parent_role,
child_role=spec.child_role,
validation_source=(hand_profile.axis_validation_sources or {}).get(
name
),
pose_axis_line_required=bool(
spec.pose_axis_line_required
),
)
for name, spec in record_specs.items()
}
tasks = tuple(
TaskSpec(
key=spec.key,
view=spec.view,
command_index=int(spec.motor_index),
joints=tuple(spec.joints),
auxiliary_commands=tuple(spec.auxiliary_commands),
validation_only=bool(spec.validation_only),
)
for spec in hand_profile.sweep_specs
)
active = frozenset(hand_profile.active_joints)
passive = frozenset(hand_profile.passive_joints)
thumb = frozenset(name for name in active if name.startswith("thumb_"))
fingers = active - thumb
typed = CalibrationProfile(
key=key,
namespace=namespace,
command=CommandLayout(
names=tuple(hand_profile.command_names),
baseline_u8=tuple(int(value) for value in hand_profile.baseline_command),
command_index_by_joint=command_index_by_joint,
disabled_indices=frozenset(
index
for index, name in enumerate(hand_profile.command_names)
if name.startswith("reserved_")
),
),
vision=VisionRigSpec(
views=views,
common_frame="calibration_common",
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(
tasks=tasks,
precheck_sweeps=bool(hand_profile.precheck_sweeps),
steady_command_checkpoints=bool(
hand_profile.steady_command_checkpoints
),
),
measurement=MeasurementPolicy(
measurements=measurements,
cross_view_sources=dict(
hand_profile.axis_validation_sources or {}
),
image_curve_joints=frozenset(
hand_profile.image_trajectory_joints
),
directional_zero=bool(hand_profile.directional_zero),
cross_view_roll_curve=bool(hand_profile.cross_view_roll_curve),
stable_cross_view_cone_bias=bool(
hand_profile.stable_cross_view_cone_bias
),
),
zero=ZeroSolvePolicy(
active_joints=active,
passive_joints=passive,
direct_zero_joints=tuple(zero_profile.direct_zero_joints),
axis_joints=tuple(zero_profile.axis_joints),
mechanical_endpoint_joints=mechanical_endpoint_joints,
post_solve_endpoint_joints=post_solve_endpoint_joints,
mimic_source_by_joint={
target: source
for target, source in MIMIC_DERIVED_FINGER_DIPS.items()
if target in passive and source in active
},
cad_frozen_joints=frozenset(
passive - set(zero_profile.static_output_zero_offsets_rad)
),
),
quality=QualityPolicy(
training_cycles=(0, 1, 2),
holdout_cycle=3 if key.layout != "legacy_11" else None,
hard_threshold_keys=_HARD_THRESHOLD_KEYS,
isolated_holdout=bool(hand_profile.isolated_holdout),
),
scope=ScopePolicy(
calibrate_joints={
"full": active,
"thumb": thumb,
"fingers": fingers,
},
frozen_joints={
"full": frozenset(),
"thumb": fingers,
"fingers": thumb,
},
),
artifacts=build_artifact_policy(
frozenset(hand_profile.capabilities)
),
)
return RegisteredProfile(
profile=typed,
engine=EngineBindings(
hand_profile=hand_profile,
zero_profile=zero_profile,
motion_command=build_calibration_motion_command,
preparation_waypoints=build_calibration_preparation_waypoints,
return_waypoints=build_calibration_return_waypoints,
cli_main=_run_cli,
node_main=_run_node,
),
)
@@ -0,0 +1,30 @@
"""Runtime JSON and corrected-URDF naming policy."""
from ...core import ArtifactPolicy
def build_artifact_policy(
compatibility_tokens: frozenset[str],
) -> ArtifactPolicy:
return ArtifactPolicy(
output_schema_version=4,
calibration_filename="g20_{side}_{serial_number}_calibration.json",
corrected_urdf_filename=(
"linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf"
),
protected_input_fields=frozenset(
{
"source_urdf_sha256",
"camera_extrinsics_sha256",
"calibration_config_sha256",
"tag_config_sha256",
}
),
session_compatibility_tokens=frozenset(compatibility_tokens),
publish_corrected_urdf=(
"urdf_zero_publication" in compatibility_tokens
),
)
__all__ = ["build_artifact_policy"]
@@ -0,0 +1,26 @@
"""Reviewed command-channel layouts for this hand family."""
G20_COMMAND_NAMES: tuple[str, ...] = (
"thumb_cmc_pitch",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
"thumb_cmc_roll",
"index_mcp_roll",
"middle_mcp_roll",
"ring_mcp_roll",
"pinky_mcp_roll",
"thumb_cmc_yaw",
"reserved_11",
"reserved_12",
"reserved_13",
"reserved_14",
"thumb_mcp",
"index_pip",
"middle_pip",
"ring_pip",
"pinky_pip",
)
__all__ = ["G20_COMMAND_NAMES"]
@@ -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()
@@ -0,0 +1,35 @@
"""One-release typed wrappers for the legacy 11-Tag layouts."""
from ...core import ProfileKey
from .profile import get_hand_calibration_profile
from ...urdf_zero import get_zero_calibration_profile
from ..registry import RegisteredProfile
from ._adapter import adapt_profile
LEFT_KEY = ProfileKey("G20", "left", "legacy_11", 1)
RIGHT_KEY = ProfileKey("G20", "right", "legacy_11", 1)
def build_left_profile() -> RegisteredProfile:
hand = get_hand_calibration_profile(LEFT_KEY.side, LEFT_KEY.layout)
return adapt_profile(
key=LEFT_KEY,
namespace="/g20_calibration",
hand_profile=hand,
zero_profile=get_zero_calibration_profile(
LEFT_KEY.side, LEFT_KEY.layout
),
)
def build_right_profile() -> RegisteredProfile:
hand = get_hand_calibration_profile(RIGHT_KEY.side, RIGHT_KEY.layout)
return adapt_profile(
key=RIGHT_KEY,
namespace="/g20_calibration",
hand_profile=hand,
zero_profile=get_zero_calibration_profile(
RIGHT_KEY.side, RIGHT_KEY.layout
),
)
@@ -0,0 +1,15 @@
"""Reviewed motion and safe-waypoint strategy exports."""
from .profile import (
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
build_calibration_speed_profile,
)
__all__ = [
"build_calibration_motion_command",
"build_calibration_preparation_waypoints",
"build_calibration_return_waypoints",
"build_calibration_speed_profile",
]
@@ -26,7 +26,7 @@ from sensor_msgs.msg import CameraInfo, JointState
from std_msgs.msg import String
from std_srvs.srv import Trigger
from .acquisition import (
from ...acquisition import (
StateSample,
TagQuality,
interpolate_state_u8,
@@ -34,20 +34,20 @@ from .acquisition import (
tag_quality_is_valid,
update_pnp_reset_watchdog,
)
from .core import (
COMMAND_NAMES,
from ...core import (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
robust_rotation_summary,
)
from .extrinsics import (
from ...core.urdf import build_correction_plan
from ...extrinsics import (
ThreeCameraExtrinsics,
camera_info_fingerprint,
load_three_camera_extrinsics,
matrix_payload,
transform_matrix,
)
from .full_hand import (
from .profile import (
G20_COMBINATION_REQUIRED_TARGET_KEYS,
G20_REFERENCE_THUMB_CMC_JOINTS,
G20_RIGHT_19_LAYOUT,
@@ -73,23 +73,24 @@ from .full_hand import (
fit_joint_image_curve,
get_hand_calibration_profile,
)
from .hikrobot_camera import configure_fastdds_large_image_transport
from .pnp import (
from ...hikrobot_camera import configure_fastdds_large_image_transport
from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
from ...pnp import (
SquareTagGroupPoseTracker,
SquareTagPose,
SquareTagPoseTracker,
)
from .product import get_product_calibration_contract
from .sample_schema import (
from ...product import get_product_calibration_contract
from ...sample_schema import (
SampleDataContractError,
canonical_sample_record,
fitting_sample_record,
fitting_sample_records,
validate_sample_records,
)
from .storage import append_jsonl, append_jsonl_many, atomic_write_json
from .three_camera_diagnostics import render_three_camera_status_text_zh
from .urdf_zero import (
from ...storage import append_jsonl, append_jsonl_many, atomic_write_json
from .reporting_zh import render_three_camera_status_text_zh
from .zero_solver import (
LEFT_ZERO_PROFILE,
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
@@ -318,13 +319,19 @@ def _palm_axis_observer_schema(
def _palm_axis_resume_policy(
profile: HandCalibrationProfile,
session_start: Mapping[str, Any],
compatibility_tokens: frozenset[str] | None = None,
) -> tuple[bool, tuple[str, ...]]:
"""Validate optional palm-axis checkpoint data when a model uses it."""
capability = "palm_axis_relative_motion_v3"
previous = {str(value) for value in session_start.get("capabilities", [])}
if not profile.palm_axis_observers and capability not in profile.capabilities:
current = (
frozenset(profile.capabilities)
if compatibility_tokens is None
else frozenset(compatibility_tokens)
)
if not profile.palm_axis_observers and capability not in current:
return True, ()
required_previous = set(profile.capabilities) - {capability}
required_previous = set(current) - {capability}
if not required_previous.issubset(previous):
raise ValueError("resume checkpoint lacks required capabilities")
if capability in previous:
@@ -741,7 +748,7 @@ def _requires_pnp_tracker_reset_for_sweep(
return False
if is_fit_retry:
return True
if profile.supports("precheck_sweeps"):
if profile.precheck_sweeps:
return bool(item.precheck)
return not item.precheck and item.cycle == 0
@@ -813,6 +820,52 @@ def _maximum_corner_drift_px(
)
def _resume_fixed_base_position_compatibility(
rows: Sequence[Mapping[str, Any]],
current_corners_by_view: Mapping[
str, Sequence[Sequence[float]] | None
],
maximum_corner_drift_px: float,
) -> tuple[dict[str, float], tuple[str, ...], tuple[str, ...]]:
"""Compare the previous and current session-start palm references.
Calibration measurements are invariant to one rigid hand placement, but
samples expressed in two independently established common frames must
never be combined. Fixed palm-Tag corners provide a camera-native check
before any durable task is imported.
"""
previous_corners_by_view: dict[str, Any] = {}
for row in rows:
if str(row.get("kind", "")) != "fixed_base_reference_locked":
continue
view = str(row.get("view", ""))
if view in current_corners_by_view:
# Keep the last lock in case a future compatible schema records a
# deliberate pre-scan relock in the same raw stream.
previous_corners_by_view[view] = row.get("corner_reference_xy")
drift_by_view_px: dict[str, float] = {}
changed_views: list[str] = []
unverifiable_views: list[str] = []
for view, current in current_corners_by_view.items():
previous = previous_corners_by_view.get(str(view))
if previous is None or current is None:
unverifiable_views.append(str(view))
continue
drift = _maximum_corner_drift_px(previous, current)
if not math.isfinite(drift):
unverifiable_views.append(str(view))
continue
drift_by_view_px[str(view)] = drift
if drift > float(maximum_corner_drift_px):
changed_views.append(str(view))
return (
drift_by_view_px,
tuple(sorted(changed_views)),
tuple(sorted(unverifiable_views)),
)
def _selected_pose_qualities(
selected: Mapping[str, SquareTagPose],
live_qualities: Mapping[str, TagQuality],
@@ -1421,6 +1474,19 @@ def _unresolved_fit_failure_tasks(
# failures that mixed velocity lag or firmware tracking
# deadband with mechanical hysteresis are safe to revalidate.
return True
if (
profile.layout_id == G20_RIGHT_19_LAYOUT
and metric == "axis_pose_line_rms_mm"
and joint_name in profile.record_specs
and not profile.record_specs[
joint_name
].pose_axis_line_required
):
# Current profile policy owns whether this monocular 3-D
# diagnostic is release-critical. Revalidate the complete
# raw task under that policy instead of making an old failure
# force another acquisition forever.
return True
if (
profile.layout_id == G20_RIGHT_19_LAYOUT
and joint_name
@@ -2015,7 +2081,7 @@ def _build_sweep_plan(
"""
plan: list[SweepItem] = []
for spec in profile.sweep_specs:
if profile.supports("precheck_sweeps"):
if profile.precheck_sweeps:
plan.extend(
SweepItem(spec, -1, direction, precheck=True)
for direction in (
@@ -2708,6 +2774,7 @@ class G20ThreeCameraCalibrationNode(Node):
)
self.profile = product_contract.profile
self.zero_profile = product_contract.zero_profile
self.calibration_profile = product_contract.typed_profile
self.serial_number = str(value("serial_number"))
if self.serial_number == "UNSET":
raise ValueError("serial_number is required")
@@ -2766,6 +2833,11 @@ class G20ThreeCameraCalibrationNode(Node):
)
self.resumed_task_keys: tuple[str, ...] = ()
self.resume_source_session = ""
self.resume_checkpoint_pending = False
self.resume_position_policy = "not_requested"
self.resume_position_changed_views: tuple[str, ...] = ()
self.resume_position_unverifiable_views: tuple[str, ...] = ()
self.resume_position_drift_by_view_px: dict[str, float] = {}
self.camera_extrinsics_file = Path(
str(value("camera_extrinsics_file"))
).expanduser().resolve()
@@ -5226,7 +5298,19 @@ class G20ThreeCameraCalibrationNode(Node):
(
palm_axis_schema_compatible,
palm_axis_invalidated_tasks,
) = _palm_axis_resume_policy(self.profile, start)
) = _palm_axis_resume_policy(
self.profile,
start,
getattr(
getattr(
getattr(self, "calibration_profile", None),
"artifacts",
None,
),
"session_compatibility_tokens",
frozenset(self.profile.capabilities),
),
)
except ValueError as error:
raise RuntimeError(
"resume checkpoint algorithm capabilities differ"
@@ -5247,6 +5331,54 @@ class G20ThreeCameraCalibrationNode(Node):
"resume checkpoint geometry, algorithm capabilities, Tag "
"layout, baseline or source URDF differs"
)
current_corners_by_view = {
str(view): getattr(runtime, "locked_base_corners_xy", None)
for view, runtime in getattr(self, "views", {}).items()
}
if not current_corners_by_view:
raise RuntimeError(
"resume checkpoint requires current fixed-base references"
)
(
position_drift_by_view_px,
position_changed_views,
position_unverifiable_views,
) = _resume_fixed_base_position_compatibility(
rows,
current_corners_by_view,
float(
getattr(self, "fixed_base_maximum_corner_drift_px", 2.0)
),
)
self.resume_source_session = source.parent.name
self.resume_position_drift_by_view_px = dict(
position_drift_by_view_px
)
self.resume_position_changed_views = position_changed_views
self.resume_position_unverifiable_views = (
position_unverifiable_views
)
start_position_invalidated_tasks: tuple[str, ...] = ()
if position_changed_views or position_unverifiable_views:
if str(getattr(self, "recalibration_scope", "full")) != "full":
affected = sorted(
{*position_changed_views, *position_unverifiable_views}
)
raise RuntimeError(
"resume checkpoint start pose differs or cannot be "
"verified for partial recalibration: "
+ ",".join(affected)
)
start_position_invalidated_tasks = tuple(
spec.key for spec in self.profile.sweep_specs
)
self.resume_position_policy = (
"discard_all_tasks_for_new_start_pose"
if position_changed_views
else "discard_all_tasks_for_unverified_start_pose"
)
else:
self.resume_position_policy = "reuse_same_start_pose"
try:
changed_tag_size_ids, size_invalidated_tasks = (
resume_tasks_invalidated_by_tag_size_changes(
@@ -5261,6 +5393,7 @@ class G20ThreeCameraCalibrationNode(Node):
) from error
invalidated_task_set = set(size_invalidated_tasks)
invalidated_task_set.update(palm_axis_invalidated_tasks)
invalidated_task_set.update(start_position_invalidated_tasks)
scope_invalidated_tasks = tuple(
getattr(self, "recalibration_task_keys", ()) or ()
)
@@ -5368,7 +5501,6 @@ class G20ThreeCameraCalibrationNode(Node):
)
self.resumed_task_keys = completed
G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self)
self.resume_source_session = source.parent.name
missing_tasks = [
spec.key
for spec in self.profile.sweep_specs
@@ -5398,6 +5530,20 @@ class G20ThreeCameraCalibrationNode(Node):
"scope_invalidated_task_keys": list(
scope_invalidated_tasks
),
"start_position_policy": self.resume_position_policy,
"start_position_changed_views": list(
position_changed_views
),
"start_position_unverifiable_views": list(
position_unverifiable_views
),
"start_position_drift_by_view_px": {
view: round(float(value), 6)
for view, value in position_drift_by_view_px.items()
},
"start_position_invalidated_task_keys": list(
start_position_invalidated_tasks
),
"imported_record_count": len(reusable),
"imported_attempt_floor_by_task": attempt_floor_by_task,
"source_raw_samples_sha256": _file_sha256(source),
@@ -5551,6 +5697,19 @@ class G20ThreeCameraCalibrationNode(Node):
return response
self.started = True
self.startup_baseline_recovered = False
self.resumed_task_keys = ()
self.resume_source_session = ""
self.resume_checkpoint_pending = bool(
self.resume_raw_samples_path is not None
)
self.resume_position_policy = (
"pending_new_start_pose_check"
if self.resume_checkpoint_pending
else "not_requested"
)
self.resume_position_changed_views = ()
self.resume_position_unverifiable_views = ()
self.resume_position_drift_by_view_px = {}
self.sweep_items = []
self.pnp_task_spec = None
selected_sweep_specs = list(self.profile.sweep_specs)
@@ -5646,7 +5805,19 @@ class G20ThreeCameraCalibrationNode(Node):
self.recalibration_task_keys
),
"command_names": list(_command_names(self)),
"capabilities": sorted(self.profile.capabilities),
# Retained only as an old-session serialization token. Live
# decisions use the typed motion/measurement policies.
"capabilities": sorted(
getattr(
getattr(
getattr(self, "calibration_profile", None),
"artifacts",
None,
),
"session_compatibility_tokens",
frozenset(self.profile.capabilities),
)
),
"reference_finger": self.profile.reference_finger,
"view_tags": {
view: dict(tags)
@@ -5706,10 +5877,16 @@ class G20ThreeCameraCalibrationNode(Node):
)
try:
if self.resume_raw_samples_path is not None:
self.state = STATE_IMPORTING_BASE
self.reason = "reading_base_session_records"
if not self.resume_raw_samples_path.is_file():
raise RuntimeError(
"resume raw samples do not exist: "
f"{self.resume_raw_samples_path}"
)
self.resume_source_session = (
self.resume_raw_samples_path.parent.name
)
self.base_import_progress = {
"phase": "reading",
"phase": "waiting_for_new_start_pose_reference",
"records_read": 0,
"bytes_read": 0,
"total_bytes": int(
@@ -5717,10 +5894,9 @@ class G20ThreeCameraCalibrationNode(Node):
),
"fraction": 0.0,
}
self._publish_status(time.monotonic())
restored_tasks = self._restore_durable_task_checkpoint()
except Exception as error:
self.started = False
self.resume_checkpoint_pending = False
response.success = False
response.message = f"CFG-RESUME-009:{error}"
return response
@@ -5728,8 +5904,11 @@ class G20ThreeCameraCalibrationNode(Node):
response.success = True
response.message = (
"three-camera calibration started"
if restored_tasks == 0
else f"calibration resumed with {restored_tasks} completed tasks"
if not self.resume_checkpoint_pending
else (
"three-camera calibration started; checkpoint will be "
"verified after the new start-pose reference is locked"
)
)
return response
@@ -8157,7 +8336,7 @@ class G20ThreeCameraCalibrationNode(Node):
}
)
continue
if profile.supports("steady_command_checkpoints"):
if profile.steady_command_checkpoints:
command_store = getattr(
self, "command_records_by_joint", None
)
@@ -8438,7 +8617,7 @@ class G20ThreeCameraCalibrationNode(Node):
"maximum",
),
)
if not profile.supports("directional_zero"):
if not profile.directional_zero:
checks = checks + (
(
"hysteresis_deg",
@@ -8584,6 +8763,35 @@ class G20ThreeCameraCalibrationNode(Node):
# error is an admissibility check for constrained circles.
for metric, actual, limit in axis_checks:
if actual > limit:
if (
metric == "axis_pose_line_rms_mm"
and not joint_spec.pose_axis_line_required
):
append_jsonl(
self.raw_path,
{
"kind": (
"position_invariant_quality_diagnostic"
),
"task_name": spec.key,
"joint": joint_name,
"cycle": cycle + 1,
"metric": metric,
"actual": round(float(actual), 6),
"reference_limit": round(
float(limit), 6
),
"decision": "diagnostic_only",
"authoritative_quality": [
"tag_pnp",
"image_trajectory",
"relative_rotation",
"synchronisation",
"isolated_holdout",
],
},
)
continue
failure_joint = joint_name
quality_sources: tuple[str, ...] = ()
if metric == "axis_pose_line_rms_mm":
@@ -8789,7 +8997,7 @@ class G20ThreeCameraCalibrationNode(Node):
if index != outlier
]
failures.append(failure)
if profile.supports("cross_view_roll_curve"):
if profile.cross_view_roll_curve:
for primary_name, validation_name in (
profile.axis_validation_sources or {}
).items():
@@ -11565,6 +11773,20 @@ class G20ThreeCameraCalibrationNode(Node):
name: published_zero_offsets[name]
for name in self.zero_profile.direct_zero_joints
}
correction_plan = None
typed_profile = getattr(self, "calibration_profile", None)
if typed_profile is not None:
frozen_names = typed_profile.scope.frozen_joints[
self.recalibration_scope
]
correction_plan = build_correction_plan(
typed_profile,
source_sha256=_file_sha256(self.source_urdf_path),
scope=self.recalibration_scope,
frozen_offsets_rad={
name: urdf_offsets[name] for name in frozen_names
},
)
self.corrected_urdf_path = write_zero_corrected_urdf(
source_urdf=self.source_urdf_path,
output_directory=self.corrected_urdf_output_dir,
@@ -11572,6 +11794,7 @@ class G20ThreeCameraCalibrationNode(Node):
offsets_rad=urdf_offsets,
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
timestamp=stamp,
correction_plan=correction_plan,
)
try:
if self.standalone_thumb_calibration:
@@ -11885,6 +12108,21 @@ class G20ThreeCameraCalibrationNode(Node):
elif self.startup_baseline_recovered and self._all_preflight_ready(now):
locker = getattr(self, "_lock_fixed_base_references", None)
if locker is None or locker():
if getattr(self, "resume_checkpoint_pending", False):
self.resume_checkpoint_pending = False
self.state = STATE_IMPORTING_BASE
self.reason = "reading_base_session_records"
self.base_import_progress = {
"phase": "reading",
"records_read": 0,
"bytes_read": 0,
"total_bytes": int(
self.resume_raw_samples_path.stat().st_size
),
"fraction": 0.0,
}
self._publish_status(time.monotonic())
self._restore_durable_task_checkpoint()
self._start_next_sweep()
else:
self.reason = "locking_fixed_base_references"
@@ -12932,7 +13170,32 @@ class G20ThreeCameraCalibrationNode(Node):
"camera_extrinsics_error": self.extrinsics_error,
"resume": {
"used": bool(self.resumed_task_keys),
"checkpoint_requested": bool(
self.resume_raw_samples_path is not None
),
"checkpoint_pending": bool(
getattr(self, "resume_checkpoint_pending", False)
),
"source_session": self.resume_source_session,
"start_position_policy": getattr(
self, "resume_position_policy", "not_requested"
),
"start_position_changed_views": list(
getattr(self, "resume_position_changed_views", ())
),
"start_position_unverifiable_views": list(
getattr(
self,
"resume_position_unverifiable_views",
(),
)
),
"start_position_drift_by_view_px": {
view: round(float(value), 6)
for view, value in getattr(
self, "resume_position_drift_by_view_px", {}
).items()
},
"recalibration_scope": self.recalibration_scope,
"recalibration_task_keys": list(
self.recalibration_task_keys
@@ -19,8 +19,10 @@ import numpy as np
from scipy.spatial.transform import Rotation
import yaml
from .extrinsics import load_three_camera_extrinsics
from .full_hand import (
from ...compat import default_three_camera_config_path
from ...compat.legacy import uses_coupled_full_hand_zero_solver
from ...extrinsics import load_three_camera_extrinsics
from .profile import (
G20_REFERENCE_THUMB_CMC_JOINTS,
G20_RIGHT_19_LAYOUT,
RIGHT_19_END_ON_IMAGE_CURVE_JOINTS,
@@ -36,10 +38,11 @@ from .full_hand import (
get_hand_calibration_profile,
validate_compact_payload,
)
from .product import get_product_calibration_contract
from .sample_schema import fitting_sample_record, fitting_sample_records
from .storage import atomic_write_json
from .urdf_zero import (
from ...product import get_product_calibration_contract
from ...sample_schema import fitting_sample_record, fitting_sample_records
from ...storage import atomic_write_json
from .publication import clamp_compact_payload_to_urdf_limits
from .zero_solver import (
JointAxisMeasurement,
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
UrdfKinematicModel,
@@ -76,6 +79,42 @@ def _sha256(path: Path) -> str:
return digest.hexdigest()
def _payload_sha256(payload: Mapping[str, Any]) -> str:
encoded = (
json.dumps(payload, ensure_ascii=False, indent=2) + "\n"
).encode("utf-8")
return hashlib.sha256(encoded).hexdigest()
def _legacy_published_quality(
session: Path, *, side: str, serial_number: str
) -> dict[str, Any]:
"""Recover v1 random-validation aggregates that raw JSONL omitted."""
path = session / f"g20_{side}_{serial_number}_calibration.json"
if not path.is_file():
raise ValueError(
"legacy replay requires its published JSON because v1 raw "
"samples omitted random-validation observations"
)
payload = json.loads(path.read_text(encoding="utf-8"))
quality = payload.get("quality")
if not isinstance(quality, Mapping) or set(quality) != {
"passed",
"validation_mae_rad",
"validation_p95_rad",
}:
raise ValueError("legacy published JSON has an invalid quality block")
if not bool(quality["passed"]):
raise ValueError("legacy published JSON did not pass validation")
values = (
float(quality["validation_mae_rad"]),
float(quality["validation_p95_rad"]),
)
if any(not math.isfinite(value) or value < 0.0 for value in values):
raise ValueError("legacy published quality contains an invalid error")
return dict(quality)
def _output_suffix(output_tag: str | None) -> str:
"""Return a filename-safe suffix for a non-destructive replay variant."""
if output_tag is None:
@@ -795,6 +834,7 @@ def _quality_failures(
if (
profile.record_specs[name].zero_kind
!= "axis_cross_view_validation"
and profile.record_specs[name].pose_axis_line_required
and cross_view_side_line_source(axis) is None
and axis.pose_axis_line_rms_m
> float(parameters["axis_maximum_pose_line_rms_m"])
@@ -1009,9 +1049,8 @@ def replay_session(
output_tag: str | None = None,
) -> dict[str, Any]:
session = Path(session_dir).expanduser().resolve()
package_root = Path(__file__).resolve().parents[1]
config = (
package_root / "config" / "three_camera_calibration.yaml"
default_three_camera_config_path()
if config_file is None
else Path(config_file).expanduser().resolve()
)
@@ -1024,6 +1063,7 @@ def replay_session(
palm_axis_records_by_source,
raw_path,
) = _load_raw_session(session)
legacy_coupled_zero_solver = uses_coupled_full_hand_zero_solver(start)
model = str(start.get("model", "G20")).strip().upper()
side = str(start["hand_type"]).lower()
requested_layout_id = str(
@@ -1199,9 +1239,33 @@ def replay_session(
raise ValueError(
f"{name} has {len(records)}/18 steady command checkpoints"
)
command_fits[name] = _fit_curve(
name, records, profile=profile, baseline=baseline
zero_command = int(
baseline[profile.joint_specs[name].motor_index]
)
if name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS:
command_fits[name] = fit_joint_image_curve(
records,
maximum_radial_rms_px=float(
parameters["image_trajectory_maximum_radial_rms_px"]
),
maximum_radial_p95_px=float(
parameters["image_trajectory_maximum_radial_p95_px"]
),
minimum_radius_px=float(
parameters["image_trajectory_minimum_radius_px"]
),
minimum_arc_rad=math.radians(
float(parameters["trajectory_minimum_arc_deg"])
),
)
else:
command_fits[name] = fit_rotation_joint_curve(
records,
zero_command_u8=zero_command,
canonical_zero_direction=canonical_zero_direction(
profile, name
),
)
command_gap_limit = math.radians(
float(parameters.get("command_maximum_direction_gap_deg", 2.0))
)
@@ -1300,10 +1364,23 @@ def replay_session(
source_urdf=source_urdf,
repetitions=repetitions,
)
palm_records_for_fit = palm_axis_records_by_source
if legacy_coupled_zero_solver:
palm_records_for_fit = {
name: [
{
key: value
for key, value in record.items()
if key != "child_pose_common"
}
for record in records
]
for name, records in palm_axis_records_by_source.items()
}
palm_orientation_measurements, palm_orientation_rejections = (
fit_partial_palm_orientation_measurements(
sources=profile.palm_orientation_sources,
records_by_joint=palm_axis_records_by_source,
records_by_joint=palm_records_for_fit,
motor_by_source=profile.palm_axis_motor_by_source,
baseline_command_u8=baseline,
cycles=range(repetitions),
@@ -1543,7 +1620,7 @@ def replay_session(
fits: Mapping[str, JointCurveFit],
arguments: Mapping[str, Any],
):
if layout_id != G20_RIGHT_19_LAYOUT:
if layout_id != G20_RIGHT_19_LAYOUT or legacy_coupled_zero_solver:
return solve_urdf_zero_offsets(curves=fits, **arguments)
thumb_profile = get_right_19_thumb_zero_profile()
thumb_names = set(thumb_profile.direct_zero_joints)
@@ -1800,6 +1877,19 @@ def replay_session(
else None
),
)
payload, _clipped_runtime_joints = (
clamp_compact_payload_to_urdf_limits(payload, candidate)
)
legacy_quality_source = None
if legacy_coupled_zero_solver:
payload["quality"] = _legacy_published_quality(
session,
side=side,
serial_number=hand_serial,
)
legacy_quality_source = (
"published_v1_artifact_random_validation_omitted_from_raw"
)
validate_compact_payload(payload)
if write_outputs:
if _sha256(source_urdf) != source_hash_before:
@@ -1901,6 +1991,8 @@ def replay_session(
"maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
},
"corrected_urdf_sha256": candidate_hash,
"computed_final_json_sha256": _payload_sha256(payload),
"legacy_quality_source": legacy_quality_source,
"final_json": str(final_json) if write_outputs else None,
"corrected_urdf": str(final_urdf) if write_outputs else None,
}
@@ -17,15 +17,15 @@ import xml.etree.ElementTree as ET
import numpy as np
from .core import COMMAND_NAMES
from .trajectory import (
from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
from ...trajectory import (
_angle_for_circle,
_fit_circle_with_axis,
_fit_joint_curve,
_fit_plane_axis,
_orient_circle_positive,
)
from .zero_calibration import (
from ...zero_calibration import (
_fit_circle,
_trajectory_arc_rad,
circular_median_rad,
@@ -51,6 +51,12 @@ class JointSpec:
child_role: str | None
source_joint: str | None = None
zero_kind: str | None = None
# Keep the physical axis-line gate only when that line contributes to a
# released URDF zero/axis decision. Curve-only passive measurements may
# retain the monocular line residual as a diagnostic while their image
# trajectory, relative rotation, synchronisation and holdout gates remain
# release-critical.
pose_axis_line_required: bool = True
@property
def measured(self) -> bool:
@@ -129,6 +135,12 @@ class HandCalibrationProfile:
command_names: tuple[str, ...] = COMMAND_NAMES
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
capabilities: frozenset[str] = frozenset()
precheck_sweeps: bool = False
steady_command_checkpoints: bool = False
directional_zero: bool = False
isolated_holdout: bool = False
cross_view_roll_curve: bool = False
stable_cross_view_cone_bias: bool = False
@property
def command_count(self) -> int:
@@ -537,7 +549,13 @@ def _build_right_19_profile() -> HandCalibrationProfile:
zero_kind="urdf_axis_chain",
),
"thumb_ip": JointSpec(
"thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
"thumb_ip",
15,
False,
"front",
"thumb_mcp",
"thumb_ip",
pose_axis_line_required=False,
),
}
for finger in ("index", "middle", "ring", "pinky"):
@@ -703,6 +721,12 @@ def _build_right_19_profile() -> HandCalibrationProfile:
"palm_axis_relative_motion_v3",
}
),
precheck_sweeps=True,
steady_command_checkpoints=True,
directional_zero=True,
isolated_holdout=True,
cross_view_roll_curve=True,
stable_cross_view_cone_bias=True,
)
@@ -15,15 +15,16 @@ import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation
from .full_hand import (
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 .urdf_zero import (
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,
)
@@ -202,6 +203,7 @@ def verify_corrected_urdf(
*,
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.
@@ -210,6 +212,13 @@ def verify_corrected_urdf(
"""
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):
@@ -243,6 +252,12 @@ def verify_corrected_urdf(
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():
@@ -745,11 +760,20 @@ def finalize_session_artifacts(
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 = {}
@@ -0,0 +1,27 @@
"""Independent reviewed profile for the right 19-Tag product layout."""
from ...core import ProfileKey
from .profile import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile
from .zero_policy import (
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
get_zero_calibration_profile,
)
from ..registry import RegisteredProfile
from ._adapter import adapt_profile
KEY = ProfileKey("G20", "right", G20_RIGHT_19_LAYOUT, 1)
def build_profile() -> RegisteredProfile:
hand = get_hand_calibration_profile(KEY.side, KEY.layout)
zero = get_zero_calibration_profile(KEY.side, KEY.layout)
return adapt_profile(
key=KEY,
namespace="/g20_calibration",
hand_profile=hand,
zero_profile=zero,
mechanical_endpoint_joints=RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
post_solve_endpoint_joints=RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
)
@@ -20,11 +20,15 @@ from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger
from .hikrobot_camera import configure_fastdds_large_image_transport
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
from .product import ProductConfig, load_product_config, sha256_file
from ...hikrobot_camera import configure_fastdds_large_image_transport
from ...operator_report import (
ProgressEstimator,
build_failure_report,
render_progress_zh,
)
from ...product import ProductConfig, load_product_config, sha256_file
from .publication import atomic_session_pointer, finalize_session_artifacts
from .storage import atomic_write_json
from ...storage import atomic_write_json
EXIT_PASS = 0
@@ -137,7 +141,7 @@ class ProgressConsole:
def _default_product_config() -> Path:
try:
installed = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
) / "config" / "g20_right_product.yaml"
if installed.is_file():
return installed
@@ -145,7 +149,7 @@ def _default_product_config() -> Path:
pass
return (
Path.cwd()
/ "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
/ "src/linkerhand_calibration/config/g20_right_product.yaml"
).resolve()
@@ -188,7 +192,7 @@ def _launch_command(
return [
"ros2",
"launch",
"g20_thumb_apriltag_calibration",
"linkerhand_calibration",
"three_camera_calibration.launch.py",
*(f"{name}:={value}" for name, value in values.items()),
]
@@ -0,0 +1,17 @@
"""Reviewed static-zero, endpoint, and mimic topology exports."""
from .profile import MIMIC_DERIVED_FINGER_DIPS
from .zero_solver import (
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
get_zero_calibration_profile,
)
__all__ = [
"MIMIC_DERIVED_FINGER_DIPS",
"RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS",
"RIGHT_19_MECHANICAL_ENDPOINT_JOINTS",
"RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS",
"get_zero_calibration_profile",
]
@@ -17,8 +17,9 @@ from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation
from scipy.stats import t as student_t
from .core import fit_rotation_axis, robust_rotation_summary
from .full_hand import (
from ...core import fit_rotation_axis, robust_rotation_summary
from ...core.urdf import UrdfCorrectionPlan
from .profile import (
G20_RIGHT_19_LAYOUT,
IMAGE_TRAJECTORY_JOINTS,
LEFT_HAND_PROFILE,
@@ -28,8 +29,8 @@ from .full_hand import (
JointCurveFit,
get_hand_calibration_profile,
)
from .sample_schema import explicit_domain_value
from .trajectory import (
from ...sample_schema import explicit_domain_value
from ...trajectory import (
_fit_circle_with_axis,
_fit_joint_curve,
_fit_plane_axis,
@@ -3850,7 +3851,7 @@ def solve_urdf_zero_offsets(
if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad:
cone_range = float(np.ptp(cone_mismatches))
stable_product_bias = bool(
profile.hand.supports("stable_cross_view_cone_bias")
profile.hand.stable_cross_view_cone_bias
and maximum_systematic_axis_cone_bias_rad is not None
and len(cone_mismatches) >= 4
and maximum_cone_mismatch
@@ -4454,6 +4455,7 @@ def write_zero_corrected_urdf(
offsets_rad: Mapping[str, float],
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
timestamp: str | None = None,
correction_plan: UrdfCorrectionPlan | None = None,
) -> Path:
source = Path(source_urdf).expanduser().resolve()
output = Path(output_directory).expanduser().resolve()
@@ -4475,6 +4477,15 @@ def write_zero_corrected_urdf(
}
if not set(endpoint_offsets) <= set(offsets):
raise ValueError("endpoint-anchored offsets must be URDF zero targets")
if correction_plan is not None:
correction_plan.verify_source(source)
correction_plan.authorize_offsets(offsets)
if not set(endpoint_offsets).issubset(
correction_plan.endpoint_limit_joints
):
raise ValueError(
"endpoint offsets are not authorized by the correction plan"
)
if any(
not math.isfinite(value) or abs(value) > math.radians(90.0)
for value in offsets.values()
@@ -0,0 +1 @@
"""Future profiles live here; no publishable profile is registered yet."""
@@ -0,0 +1,84 @@
"""Local, deterministic profile registry for this ROS package."""
from __future__ import annotations
from dataclasses import dataclass
from typing import Any, Callable, Iterator
from ..core import CalibrationProfile, ProfileKey, validate_profile
@dataclass(frozen=True)
class EngineBindings:
"""Temporary bridge from typed policies to the proven engine objects."""
hand_profile: Any
zero_profile: Any
motion_command: Callable[..., list[int]]
preparation_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
return_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
cli_main: Callable[[list[str] | None], None]
node_main: Callable[[list[str] | None], None]
@dataclass(frozen=True)
class RegisteredProfile:
profile: CalibrationProfile
engine: EngineBindings
class ProfileRegistry:
"""An in-package registry; no discovery plugins or string evaluation."""
def __init__(self) -> None:
self._profiles: dict[ProfileKey, RegisteredProfile] = {}
def register(self, registered: RegisteredProfile) -> None:
validate_profile(registered.profile)
key = registered.profile.key
existing = self._profiles.get(key)
if existing is not None and existing != registered:
raise ValueError(f"profile is already registered: {key.profile_id}")
self._profiles[key] = registered
def get(self, key: ProfileKey) -> RegisteredProfile:
try:
return self._profiles[key]
except KeyError as error:
supported = ", ".join(
item.profile_id for item in sorted(self._profiles)
)
raise ValueError(
f"unsupported calibration profile {key.profile_id}; "
f"registered={supported}"
) from error
def resolve(
self,
model: str,
side: str,
layout: str,
revision: int = 1,
) -> RegisteredProfile:
return self.get(ProfileKey(model, side, layout, revision))
def __iter__(self) -> Iterator[RegisteredProfile]:
for key in sorted(self._profiles):
yield self._profiles[key]
def __len__(self) -> int:
return len(self._profiles)
_DEFAULT_REGISTRY: ProfileRegistry | None = None
def get_default_registry() -> ProfileRegistry:
global _DEFAULT_REGISTRY
if _DEFAULT_REGISTRY is None:
from .g20 import register_profiles
registry = ProfileRegistry()
register_profiles(registry)
_DEFAULT_REGISTRY = registry
return _DEFAULT_REGISTRY
@@ -36,7 +36,7 @@ from .acquisition import (
interpolate_state_u8,
tag_quality_is_valid,
)
from .core import (
from .compat.legacy.thumb_core import (
BASELINE_COMMAND,
COMMAND_NAMES,
DIRECTION_DECREASING,
@@ -0,0 +1,7 @@
"""One-release module alias for the relocated model replay implementation."""
import sys
from .models.g20 import offline_replay as _implementation
sys.modules[__name__] = _implementation
@@ -0,0 +1,9 @@
"""Compatibility import for the generic registered-profile runner."""
from .runtime.runner import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -305,13 +305,24 @@ def render_progress_zh(
f"{fit_attempt_limit - 1} 次异常轮补采)"
)
resume = status.get("resume", {})
if isinstance(resume, Mapping) and resume.get("used"):
lines.append(
"断点:已恢复 "
f"{int(resume.get('completed_task_count', 0))}/"
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
"失败任务已丢弃并重新采集"
)
if isinstance(resume, Mapping):
position_policy = str(resume.get("start_position_policy", ""))
if resume.get("checkpoint_pending"):
lines.append(
"断点:正在建立本次起始位置基准,完成后再决定是否复用旧任务"
)
elif position_policy.startswith("discard_all_tasks_"):
lines.append(
"位置:检测到标定前机械手位置已变化或旧位置无法可靠验证;"
"未混用旧断点,本次整手从头采集"
)
elif resume.get("used"):
lines.append(
"断点:已恢复 "
f"{int(resume.get('completed_task_count', 0))}/"
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
"起始位置一致,失败任务已丢弃并重新采集"
)
return "\n".join(lines)
@@ -0,0 +1,3 @@
"""Compatibility import for the model-independent PnP implementation."""
from .core.geometry.pnp import * # noqa: F401,F403
@@ -13,114 +13,67 @@ import hashlib
from pathlib import Path
import re
from typing import Any, Mapping
import xml.etree.ElementTree as ET
import yaml
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
from .full_hand import (
G20_RIGHT_19_LAYOUT,
HandCalibrationProfile,
get_hand_calibration_profile,
from .compat import (
product_profile_key,
resolve_legacy_profile_alias,
resolve_renamed_package_path,
)
from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile
VIEWS = ("front", "side", "top")
from .core import CalibrationProfile, ProfileKey
from .core.geometry import camera_info_fingerprint, load_camera_extrinsics
from .models import RegisteredProfile, get_default_registry
@dataclass(frozen=True)
class ProductCalibrationContract:
"""Declarative boundary between one hand model and the shared engine."""
"""Compatibility facade over one typed, locally registered profile."""
model: str
side: str
layout_id: str
profile: HandCalibrationProfile
zero_profile: ZeroCalibrationProfile
registered: RegisteredProfile
@property
def model(self) -> str:
return self.registered.profile.key.model
@property
def side(self) -> str:
return self.registered.profile.key.side
@property
def layout_id(self) -> str:
return self.registered.profile.key.layout
@property
def typed_profile(self) -> CalibrationProfile:
return self.registered.profile
@property
def profile(self):
return self.registered.engine.hand_profile
@property
def zero_profile(self):
return self.registered.engine.zero_profile
@property
def required_tag_ids(self) -> frozenset[int]:
return frozenset(
int(tag_id)
for tags in self.profile.view_tags.values()
for tag_id in tags.values()
)
return self.typed_profile.vision.tag_ids
@property
def views(self) -> tuple[str, ...]:
return tuple(self.profile.view_tags)
_PRODUCT_CONTRACTS: dict[
tuple[str, str, str], ProductCalibrationContract
] = {}
def register_product_calibration_contract(
contract: ProductCalibrationContract,
) -> None:
"""Register one model/side/layout without modifying shared workflow code."""
model = str(contract.model).strip().upper()
side = str(contract.side).strip().lower()
layout = str(contract.layout_id).strip().lower()
if not model or side not in {"left", "right"} or not layout:
raise ValueError("product calibration contract identity is invalid")
if contract.profile.side != side:
raise ValueError("product contract side differs from hand profile")
if contract.profile.layout_id.lower() != layout:
raise ValueError("product contract layout differs from hand profile")
if contract.zero_profile.hand != contract.profile:
raise ValueError("zero-calibration profile differs from hand profile")
if len(contract.profile.baseline_command) != contract.profile.command_count:
raise ValueError("profile baseline and command names differ in length")
if not contract.required_tag_ids:
raise ValueError("product contract must declare at least one Tag")
key = (model, side, layout)
existing = _PRODUCT_CONTRACTS.get(key)
if existing is not None and existing != contract:
raise ValueError(f"product calibration contract already registered: {key}")
_PRODUCT_CONTRACTS[key] = contract
return self.typed_profile.vision.view_names
def get_product_calibration_contract(
model: str, side: str, layout_id: str
model: str, side: str, layout_id: str, revision: int = 1
) -> ProductCalibrationContract:
key = (
str(model).strip().upper(),
str(side).strip().lower(),
str(layout_id).strip().lower(),
key = resolve_legacy_profile_alias(
ProfileKey(model, side, layout_id, revision)
)
try:
return _PRODUCT_CONTRACTS[key]
except KeyError as error:
supported = ", ".join("/".join(item) for item in sorted(_PRODUCT_CONTRACTS))
raise ValueError(
f"unsupported calibration product {key}; registered={supported}"
) from error
register_product_calibration_contract(
ProductCalibrationContract(
model="G20",
side="right",
layout_id=G20_RIGHT_19_LAYOUT,
profile=get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
zero_profile=get_zero_calibration_profile(
"right", G20_RIGHT_19_LAYOUT
),
)
)
for _legacy_side in ("left", "right"):
register_product_calibration_contract(
ProductCalibrationContract(
model="G20",
side=_legacy_side,
layout_id="legacy_11",
profile=get_hand_calibration_profile(_legacy_side, "legacy_11"),
zero_profile=get_zero_calibration_profile(
_legacy_side, "legacy_11"
),
)
return ProductCalibrationContract(
get_default_registry().get(key)
)
@@ -142,10 +95,7 @@ def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path:
text = str(value).strip()
if not text:
raise ValueError(f"{name} is required")
candidate = Path(text).expanduser()
if not candidate.is_absolute():
candidate = workspace / candidate
return candidate.resolve()
return resolve_renamed_package_path(text, workspace)
def _camera_info_fingerprint(path: Path) -> str:
@@ -190,12 +140,18 @@ def _custom_pnp_tag_sizes_m_by_id(
) -> dict[int, float]:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
parameters = _mapping(
_mapping(payload.get("g20_calibration"), "g20_calibration").get(
"ros__parameters"
),
"ros__parameters",
)
candidates = [
_mapping(node, str(name)).get("ros__parameters")
for name, node in payload.items()
if isinstance(node, Mapping)
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))
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
@@ -208,10 +164,37 @@ def _custom_pnp_tag_sizes_m_by_id(
return result
def _validate_profile_urdf(
profile: CalibrationProfile, source_urdf: Path
) -> None:
"""Verify every zero and mimic target before a hardware command is sent."""
root = ET.parse(source_urdf).getroot()
urdf_joints = {
str(joint.get("name"))
for joint in root.findall("joint")
if joint.get("name")
}
required = (
set(profile.zero.active_joints)
| set(profile.zero.passive_joints)
| set(profile.zero.mimic_source_by_joint)
| set(profile.zero.mimic_source_by_joint.values())
)
missing = required - urdf_joints
if missing:
raise ValueError(
"source URDF is missing profile joints: "
+ ", ".join(sorted(missing))
)
@dataclass(frozen=True)
class ProductConfig:
path: Path
workspace: Path
schema_version: int
profile_key: ProfileKey
namespace: str
model: str
side: str
tag_layout: str
@@ -248,23 +231,22 @@ def load_product_config(
raise ValueError(f"product config does not exist: {source}")
with source.open("r", encoding="utf-8") as stream:
raw = _mapping(yaml.safe_load(stream), str(source))
if int(raw.get("schema_version", -1)) != 1:
raise ValueError("product config schema_version must be 1")
profile_key = product_profile_key(raw)
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
serial = str(raw.get("serial_number", ""))
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
raise ValueError("serial_number is invalid")
model = str(raw.get("model", "")).strip().upper()
side = str(raw.get("side", "")).strip().lower()
# Keep the deployed G20 schema compatible while making the layout an
# explicit product choice for all new configurations.
layout = str(
raw.get(
"tag_layout",
G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "",
)
).strip().lower()
contract = get_product_calibration_contract(model, side, layout)
model = profile_key.model
side = profile_key.side
layout = profile_key.layout
contract = get_product_calibration_contract(
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()
if not can_interface:
raise ValueError("can_interface is required")
@@ -317,6 +299,7 @@ def load_product_config(
actual = sha256_file(candidate)
if actual != expected:
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
_validate_profile_urdf(contract.typed_profile, source_urdf)
expected_tag_ids = set(contract.required_tag_ids)
tag_sizes = _tag_sizes_m_by_id(tag_config)
@@ -335,13 +318,16 @@ def load_product_config(
camera_raw = _mapping(raw.get("cameras"), "cameras")
required_views = tuple(contract.views)
if set(required_views) != set(VIEWS):
raise ValueError(
"the current shared engine requires front/side/top views"
)
if set(camera_raw) != set(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]] = {}
serials: set[str] = set()
for view in required_views:
@@ -382,6 +368,9 @@ def load_product_config(
return ProductConfig(
path=source,
workspace=root,
schema_version=int(raw["schema_version"]),
profile_key=profile_key,
namespace=namespace,
model=model,
side=side,
tag_layout=layout,
@@ -0,0 +1,7 @@
"""One-release module alias for the relocated model publication policy."""
import sys
from .models.g20 import publication as _implementation
sys.modules[__name__] = _implementation
@@ -0,0 +1,11 @@
"""Common orchestration; ROS integration is isolated below ``nodes``."""
from .controller import ControllerSnapshot, SessionController, SessionState
from .runner import build_session_controller
__all__ = [
"ControllerSnapshot",
"SessionController",
"SessionState",
"build_session_controller",
]
@@ -0,0 +1 @@
"""Camera, detector, and hand-SDK runtime adapters."""
@@ -0,0 +1,160 @@
"""ROS-free session state machine shared by live and replay runners."""
from __future__ import annotations
from dataclasses import dataclass
from enum import Enum
from typing import Sequence
from ..core import CalibrationProfile, SampleRecord, validate_profile
from ..core.solver import (
SessionSolution,
SessionSolver,
TaskEvaluation,
TaskEvaluator,
)
class SessionState(str, Enum):
CREATED = "CREATED"
PREFLIGHT = "PREFLIGHT"
PREPARING = "PREPARING"
CAPTURING = "CAPTURING"
EVALUATING = "EVALUATING"
RESCAN = "RESCAN"
SOLVING = "SOLVING"
VALIDATING = "VALIDATING"
PUBLISHING = "PUBLISHING"
COMPLETE = "COMPLETE"
FAILED = "FAILED"
@dataclass(frozen=True)
class ControllerSnapshot:
state: SessionState
completed_task_keys: tuple[str, ...]
current_task_key: str | None
failure_reason: str | None
class SessionController:
"""Advance one typed task graph without depending on ROS clocks/messages."""
def __init__(
self,
profile: CalibrationProfile,
evaluator: TaskEvaluator,
solver: SessionSolver,
) -> None:
validate_profile(profile)
self.profile = profile
self.evaluator = evaluator
self.solver = solver
self.state = SessionState.CREATED
self._task_index = 0
self._samples: list[SampleRecord] = []
self.last_evaluation: TaskEvaluation | None = None
self.solution: SessionSolution | None = None
self.failure_reason: str | None = None
@property
def current_task(self):
if self._task_index >= len(self.profile.motion.tasks):
return None
return self.profile.motion.tasks[self._task_index]
@property
def samples(self) -> tuple[SampleRecord, ...]:
return tuple(self._samples)
def snapshot(self) -> ControllerSnapshot:
completed = tuple(
task.key for task in self.profile.motion.tasks[: self._task_index]
)
task = self.current_task
return ControllerSnapshot(
state=self.state,
completed_task_keys=completed,
current_task_key=None if task is None else task.key,
failure_reason=self.failure_reason,
)
def start(self) -> None:
self._require(SessionState.CREATED)
self.state = SessionState.PREFLIGHT
def finish_preflight(self, *, passed: bool, reason: str = "") -> None:
self._require(SessionState.PREFLIGHT)
if not passed:
self._fail(reason or "static preflight failed")
return
self.state = SessionState.PREPARING
def task_pose_ready(self) -> None:
if self.state not in {SessionState.PREPARING, SessionState.RESCAN}:
self._raise_transition("task pose can only follow prepare or rescan")
if self.current_task is None:
self._raise_transition("there is no task left to capture")
self.state = SessionState.CAPTURING
def submit_task_samples(
self, samples: Sequence[SampleRecord]
) -> TaskEvaluation:
self._require(SessionState.CAPTURING)
task = self.current_task
if task is None:
self._raise_transition("there is no active task")
rows = tuple(samples)
if not rows or any(row.task_key != task.key for row in rows):
raise ValueError("captured samples do not belong to the active task")
self.state = SessionState.EVALUATING
evaluation = self.evaluator.evaluate_task(self.profile, task, rows)
self.last_evaluation = evaluation
if not evaluation.accepted:
if evaluation.rescan_measurements or evaluation.rescan_cycles:
self.state = SessionState.RESCAN
else:
self._fail("task evaluation failed without a safe rescan scope")
return evaluation
self._samples.extend(rows)
self._task_index += 1
self.state = (
SessionState.SOLVING
if self.current_task is None
else SessionState.PREPARING
)
return evaluation
def solve(self) -> SessionSolution:
self._require(SessionState.SOLVING)
solution = self.solver.solve_session(self.profile, self.samples)
self.solution = solution
if solution.passed:
self.state = SessionState.VALIDATING
else:
self._fail("session solver rejected the captured samples")
return solution
def finish_release_validation(
self, *, passed: bool, reason: str = ""
) -> None:
self._require(SessionState.VALIDATING)
if not passed:
self._fail(reason or "release validation failed")
return
self.state = SessionState.PUBLISHING
def finish_publication(self) -> None:
self._require(SessionState.PUBLISHING)
self.state = SessionState.COMPLETE
def _fail(self, reason: str) -> None:
self.failure_reason = str(reason)
self.state = SessionState.FAILED
def _require(self, expected: SessionState) -> None:
if self.state != expected:
self._raise_transition(f"expected {expected.value}")
def _raise_transition(self, detail: str) -> None:
raise RuntimeError(f"invalid session transition from {self.state.value}: {detail}")
@@ -0,0 +1 @@
"""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)
@@ -0,0 +1,3 @@
from .codes import CalibrationErrorCode
__all__ = ["CalibrationErrorCode"]
@@ -0,0 +1,14 @@
"""Model-independent runtime error categories."""
from enum import Enum
class CalibrationErrorCode(str, Enum):
PROFILE_INVALID = "profile_invalid"
PREFLIGHT_FAILED = "preflight_failed"
MOTION_FAILED = "motion_failed"
ACQUISITION_FAILED = "acquisition_failed"
TASK_QUALITY_FAILED = "task_quality_failed"
SESSION_SOLVE_FAILED = "session_solve_failed"
URDF_VALIDATION_FAILED = "urdf_validation_failed"
PUBLICATION_FAILED = "publication_failed"
@@ -0,0 +1,41 @@
"""Generic assembly of a registered profile and the session controller."""
from __future__ import annotations
import argparse
from ..compat import default_product_config_path
from ..core import ProfileKey
from ..core.solver import SessionSolver, TaskEvaluator
from ..models import ProfileRegistry, get_default_registry
from ..product import load_product_config
from .controller import SessionController
def build_session_controller(
profile_key: ProfileKey,
evaluator: TaskEvaluator,
solver: SessionSolver,
*,
registry: ProfileRegistry | None = None,
) -> SessionController:
selected_registry = registry or get_default_registry()
registered = selected_registry.get(profile_key)
return SessionController(registered.profile, evaluator, solver)
def main(args: list[str] | None = None) -> None:
"""Select the profile first, then delegate to its reviewed CLI strategy."""
selector = argparse.ArgumentParser(add_help=False)
selector.add_argument(
"--config", default=str(default_product_config_path())
)
selector.add_argument("--workspace", default=None)
selected, _ = selector.parse_known_args(args)
product = load_product_config(
selected.config,
workspace=selected.workspace,
check_can=False,
)
registered = get_default_registry().get(product.profile_key)
registered.engine.cli_main(args)
@@ -0,0 +1,3 @@
"""Compatibility import for the canonical sample data contract."""
from .core.domain.sample_schema import * # noqa: F401,F403
@@ -0,0 +1,3 @@
"""Compatibility import for crash-safe artifact storage."""
from .core.artifacts.storage import * # noqa: F401,F403
@@ -0,0 +1,4 @@
"""Compatibility import for the installed Chinese model diagnostics."""
from .models.g20.reporting_zh import * # noqa: F401,F403
from .models.g20.reporting_zh import _task_text
@@ -0,0 +1,7 @@
"""One-release module alias for the relocated model-specific ROS node."""
import sys
from .models.g20 import node as _implementation
sys.modules[__name__] = _implementation
@@ -0,0 +1 @@
"""Operator tools for extrinsics, alignment, and runtime bridges."""
@@ -0,0 +1,7 @@
"""One-release module alias for the relocated model zero solver."""
import sys
from .models.g20 import zero_solver as _implementation
sys.modules[__name__] = _implementation

Some files were not shown because too many files have changed in this diff Show More