Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 1ed36ecdd8 | |||
| 7f84225ba8 | |||
| ba9f1b25e8 | |||
| 0d606c2ba2 | |||
| 06c050e446 |
@@ -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
|
||||
+55
-19
@@ -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"]
|
||||
+9
@@ -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()
|
||||
+1
-1
@@ -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_",
|
||||
+3
-3
@@ -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(
|
||||
[
|
||||
+3
-3
@@ -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(
|
||||
[
|
||||
+3
-3
@@ -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(
|
||||
+10
-6
@@ -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(
|
||||
+3
-3
@@ -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"]
|
||||
+2
-1
@@ -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",
|
||||
]
|
||||
+66
-81
@@ -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
|
||||
+1
-1
@@ -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",
|
||||
]
|
||||
+292
-29
@@ -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
|
||||
+104
-12
@@ -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,
|
||||
}
|
||||
+28
-4
@@ -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,
|
||||
)
|
||||
|
||||
|
||||
+28
-4
@@ -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,
|
||||
)
|
||||
+11
-7
@@ -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",
|
||||
]
|
||||
+16
-5
@@ -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
|
||||
+1
-1
@@ -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()
|
||||
+18
-7
@@ -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
|
||||
+105
-116
@@ -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
Reference in New Issue
Block a user