Compare commits
5 Commits
bd3
..
1ed36ecdd8
| 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右手正式一键标定
|
## G20右手正式一键标定
|
||||||
|
|
||||||
固定三相机和19张Tag安装完成后,用户只运行:
|
固定三相机和19张Tag安装完成后,用户只运行:
|
||||||
|
|
||||||
```bash
|
```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项任务时,使用:
|
完全独立地只标定大拇指4项任务时,使用:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
--scope thumb
|
--scope thumb
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -23,7 +59,7 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
|||||||
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定,才额外使用:
|
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定,才额外使用:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
--scope thumb \
|
--scope thumb \
|
||||||
--base-session calibration_output/G20_RIGHT_001/latest_passed
|
--base-session calibration_output/G20_RIGHT_001/latest_passed
|
||||||
```
|
```
|
||||||
@@ -35,7 +71,7 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
|||||||
只重新采集12项四指任务并合成完整整手URDF:
|
只重新采集12项四指任务并合成完整整手URDF:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
ros2 run linkerhand_calibration calibrate_hand \
|
||||||
--scope fingers \
|
--scope fingers \
|
||||||
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
|
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
|
||||||
```
|
```
|
||||||
@@ -114,7 +150,7 @@ schema v4 JSON,不再生成schema v5运行文件。
|
|||||||
`tag_layout:=legacy_11`,两套配置和结果schema互不覆盖:
|
`tag_layout:=legacy_11`,两套配置和结果schema互不覆盖:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
|
ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
|
||||||
hand_type:=right \
|
hand_type:=right \
|
||||||
tag_layout:=g20_right_19 \
|
tag_layout:=g20_right_19 \
|
||||||
serial_number:=G20_RIGHT_001 \
|
serial_number:=G20_RIGHT_001 \
|
||||||
@@ -271,7 +307,7 @@ ID 9 必须在拇指横摆的完整行程中持续可见。
|
|||||||
|
|
||||||
```bash
|
```bash
|
||||||
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
|
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
|
||||||
ros2 launch g20_thumb_apriltag_calibration \
|
ros2 launch linkerhand_calibration \
|
||||||
three_camera_extrinsics.launch.py \
|
three_camera_extrinsics.launch.py \
|
||||||
output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
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
|
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
|
```bash
|
||||||
ros2 launch g20_thumb_apriltag_calibration \
|
ros2 launch linkerhand_calibration \
|
||||||
three_camera_calibration.launch.py \
|
three_camera_calibration.launch.py \
|
||||||
hand_type:=left \
|
hand_type:=left \
|
||||||
serial_number:=G20_LEFT_001 \
|
serial_number:=G20_LEFT_001 \
|
||||||
@@ -335,7 +371,7 @@ ros2 run image_view image_view --ros-args \
|
|||||||
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后,重启正式流程:
|
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后,重启正式流程:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch g20_thumb_apriltag_calibration \
|
ros2 launch linkerhand_calibration \
|
||||||
three_camera_calibration.launch.py \
|
three_camera_calibration.launch.py \
|
||||||
hand_type:=left \
|
hand_type:=left \
|
||||||
serial_number:=G20_LEFT_001 \
|
serial_number:=G20_LEFT_001 \
|
||||||
@@ -347,7 +383,7 @@ ros2 launch g20_thumb_apriltag_calibration \
|
|||||||
右手使用同一入口;默认自动选择右手SDK话题和原始URDF:
|
右手使用同一入口;默认自动选择右手SDK话题和原始URDF:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch g20_thumb_apriltag_calibration \
|
ros2 launch linkerhand_calibration \
|
||||||
three_camera_calibration.launch.py \
|
three_camera_calibration.launch.py \
|
||||||
hand_type:=right \
|
hand_type:=right \
|
||||||
serial_number:=G20_RIGHT_001 \
|
serial_number:=G20_RIGHT_001 \
|
||||||
@@ -526,7 +562,7 @@ calibration_output/G20_RIGHT_001/<时间戳>/
|
|||||||
命令。`--output-tag` 为新产物增加安全后缀,已有JSON、URDF和验证报告不会被覆盖:
|
命令。`--output-tag` 为新产物增加安全后缀,已有JSON、URDF和验证报告不会被覆盖:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
python3 -m linkerhand_calibration.offline_replay \
|
||||||
calibration_output/G20_RIGHT_001/20260811_120146 \
|
calibration_output/G20_RIGHT_001/20260811_120146 \
|
||||||
--output-tag AXIS_FRAME_V3 \
|
--output-tag AXIS_FRAME_V3 \
|
||||||
--write
|
--write
|
||||||
@@ -543,7 +579,7 @@ python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
|||||||
`JointState`(包括5个被动关节):
|
`JointState`(包括5个被动关节):
|
||||||
|
|
||||||
```bash
|
```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 \
|
hand_type:=right \
|
||||||
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
|
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
|
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||||||
source /opt/ros/jazzy/setup.bash
|
source /opt/ros/jazzy/setup.bash
|
||||||
colcon build --symlink-install \
|
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
|
source install/setup.bash
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -642,7 +678,7 @@ source install/setup.bash
|
|||||||
先单独启动相机(不会连接机械手,也不会发送关节命令):
|
先单独启动相机(不会连接机械手,也不会发送关节命令):
|
||||||
|
|
||||||
```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 \
|
--remap __ns:=/camera/camera/color \
|
||||||
-p serial_number:=DB2163742 \
|
-p serial_number:=DB2163742 \
|
||||||
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
|
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||||
@@ -680,7 +716,7 @@ ros2 run camera_calibration cameracalibrator \
|
|||||||
但标定节点不会发送位置运动命令,也不会允许解锁全行程扫描:
|
但标定节点不会发送位置运动命令,也不会允许解锁全行程扫描:
|
||||||
|
|
||||||
```bash
|
```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 \
|
serial_number:=G20_LEFT_001 \
|
||||||
camera_serial_number:=DB2163742 \
|
camera_serial_number:=DB2163742 \
|
||||||
commands_enabled:=false
|
commands_enabled:=false
|
||||||
@@ -691,7 +727,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
|||||||
速度,并使用单终点连续运动:
|
速度,并使用单终点连续运动:
|
||||||
|
|
||||||
```bash
|
```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 \
|
serial_number:=G20_LEFT_001 \
|
||||||
camera_serial_number:=DB2163742 \
|
camera_serial_number:=DB2163742 \
|
||||||
can_interface:=can0 \
|
can_interface:=can0 \
|
||||||
@@ -804,7 +840,7 @@ calibration_output/<序列号>/<时间戳>/
|
|||||||
恢复时必须显式复用原目录,否则会创建新会话:
|
恢复时必须显式复用原目录,否则会创建新会话:
|
||||||
|
|
||||||
```bash
|
```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 \
|
serial_number:=G20_LEFT_001 \
|
||||||
session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
|
session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
|
||||||
```
|
```
|
||||||
@@ -884,7 +920,7 @@ for name, joint in data["joints"].items():
|
|||||||
URDF:
|
URDF:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch g20_thumb_apriltag_calibration \
|
ros2 launch linkerhand_calibration \
|
||||||
front_cmc_pitch_zero.launch.py \
|
front_cmc_pitch_zero.launch.py \
|
||||||
serial_number:=G20_LEFT_001
|
serial_number:=G20_LEFT_001
|
||||||
```
|
```
|
||||||
@@ -961,7 +997,7 @@ zero_angles.table_projected_zero_rad
|
|||||||
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量,不读取T3标签朝向。
|
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量,不读取T3标签朝向。
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch g20_thumb_apriltag_calibration \
|
ros2 launch linkerhand_calibration \
|
||||||
front_cmc_roll_calibration.launch.py \
|
front_cmc_roll_calibration.launch.py \
|
||||||
serial_number:=G20_LEFT_001
|
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("input_topic", default_value=""),
|
||||||
DeclareLaunchArgument("output_topic", default_value=""),
|
DeclareLaunchArgument("output_topic", default_value=""),
|
||||||
Node(
|
Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="calibrated_joint_state_bridge",
|
executable="calibrated_joint_state_bridge",
|
||||||
name=[
|
name=[
|
||||||
"g20_calibrated_joint_state_bridge_",
|
"g20_calibrated_joint_state_bridge_",
|
||||||
+3
-3
@@ -48,7 +48,7 @@ def _launch_stack(context):
|
|||||||
tag_config = LaunchConfiguration("tag_config").perform(context)
|
tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||||
zero_config = LaunchConfiguration("zero_config").perform(context)
|
zero_config = LaunchConfiguration("zero_config").perform(context)
|
||||||
camera = Node(
|
camera = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="hikrobot_camera_node",
|
executable="hikrobot_camera_node",
|
||||||
name="hikrobot_camera",
|
name="hikrobot_camera",
|
||||||
namespace="/camera/camera/color",
|
namespace="/camera/camera/color",
|
||||||
@@ -158,7 +158,7 @@ def _launch_stack(context):
|
|||||||
)
|
)
|
||||||
|
|
||||||
zero_node = Node(
|
zero_node = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="cmc_pitch_zero_node",
|
executable="cmc_pitch_zero_node",
|
||||||
name="g20_thumb_cmc_pitch_zero",
|
name="g20_thumb_cmc_pitch_zero",
|
||||||
output="screen",
|
output="screen",
|
||||||
@@ -203,7 +203,7 @@ def _launch_stack(context):
|
|||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
package_share = Path(
|
package_share = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("linkerhand_calibration")
|
||||||
)
|
)
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
[
|
[
|
||||||
+3
-3
@@ -50,7 +50,7 @@ def _launch_stack(context):
|
|||||||
"calibration_config"
|
"calibration_config"
|
||||||
).perform(context)
|
).perform(context)
|
||||||
camera = Node(
|
camera = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="hikrobot_camera_node",
|
executable="hikrobot_camera_node",
|
||||||
name="hikrobot_camera",
|
name="hikrobot_camera",
|
||||||
namespace="/camera/camera/color",
|
namespace="/camera/camera/color",
|
||||||
@@ -160,7 +160,7 @@ def _launch_stack(context):
|
|||||||
)
|
)
|
||||||
|
|
||||||
calibration_node = Node(
|
calibration_node = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="cmc_roll_calibration_node",
|
executable="cmc_roll_calibration_node",
|
||||||
name="g20_thumb_cmc_roll_calibration",
|
name="g20_thumb_cmc_roll_calibration",
|
||||||
output="screen",
|
output="screen",
|
||||||
@@ -205,7 +205,7 @@ def _launch_stack(context):
|
|||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
package_share = Path(
|
package_share = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("linkerhand_calibration")
|
||||||
)
|
)
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
[
|
[
|
||||||
+3
-3
@@ -82,7 +82,7 @@ def _launch_stack(context):
|
|||||||
)
|
)
|
||||||
|
|
||||||
camera = Node(
|
camera = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="hikrobot_camera_node",
|
executable="hikrobot_camera_node",
|
||||||
name="hikrobot_camera",
|
name="hikrobot_camera",
|
||||||
namespace="/camera/camera/color",
|
namespace="/camera/camera/color",
|
||||||
@@ -234,7 +234,7 @@ def _launch_stack(context):
|
|||||||
)
|
)
|
||||||
|
|
||||||
calibration = Node(
|
calibration = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="calibration_node",
|
executable="calibration_node",
|
||||||
name="g20_thumb_calibration",
|
name="g20_thumb_calibration",
|
||||||
output="screen",
|
output="screen",
|
||||||
@@ -315,7 +315,7 @@ def _launch_stack(context):
|
|||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
package_share = Path(
|
package_share = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("linkerhand_calibration")
|
||||||
)
|
)
|
||||||
default_output = str(Path.cwd() / "calibration_output")
|
default_output = str(Path.cwd() / "calibration_output")
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
+10
-6
@@ -39,7 +39,7 @@ def _default_source_urdf(hand_type: str) -> Path:
|
|||||||
|
|
||||||
|
|
||||||
def _launch_stack(context):
|
def _launch_stack(context):
|
||||||
from g20_thumb_apriltag_calibration.product import (
|
from linkerhand_calibration.product import (
|
||||||
get_product_calibration_contract,
|
get_product_calibration_contract,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -56,7 +56,7 @@ def _launch_stack(context):
|
|||||||
raise RuntimeError(str(error)) from error
|
raise RuntimeError(str(error)) from error
|
||||||
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||||
package_share = Path(
|
package_share = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("linkerhand_calibration")
|
||||||
)
|
)
|
||||||
tag_config = (
|
tag_config = (
|
||||||
Path(requested_tag_config).expanduser().resolve()
|
Path(requested_tag_config).expanduser().resolve()
|
||||||
@@ -88,7 +88,7 @@ def _launch_stack(context):
|
|||||||
expected_source_hash = LaunchConfiguration(
|
expected_source_hash = LaunchConfiguration(
|
||||||
"source_urdf_expected_sha256"
|
"source_urdf_expected_sha256"
|
||||||
).perform(context).strip().lower()
|
).perform(context).strip().lower()
|
||||||
if contract.profile.supports("urdf_zero_publication"):
|
if contract.typed_profile.artifacts.publish_corrected_urdf:
|
||||||
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
||||||
@@ -149,7 +149,7 @@ def _launch_stack(context):
|
|||||||
detection_topics.append(detection_topic)
|
detection_topics.append(detection_topic)
|
||||||
cameras.append(
|
cameras.append(
|
||||||
Node(
|
Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="hikrobot_camera_node",
|
executable="hikrobot_camera_node",
|
||||||
name="hikrobot_camera",
|
name="hikrobot_camera",
|
||||||
namespace=namespace,
|
namespace=namespace,
|
||||||
@@ -271,11 +271,15 @@ def _launch_stack(context):
|
|||||||
],
|
],
|
||||||
)
|
)
|
||||||
calibration = Node(
|
calibration = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="three_camera_calibration_node",
|
executable="three_camera_calibration_node",
|
||||||
name="g20_calibration",
|
name="g20_calibration",
|
||||||
output="screen",
|
output="screen",
|
||||||
emulate_tty=True,
|
emulate_tty=True,
|
||||||
|
arguments=[
|
||||||
|
"--profile-id",
|
||||||
|
contract.typed_profile.key.profile_id,
|
||||||
|
],
|
||||||
parameters=[
|
parameters=[
|
||||||
LaunchConfiguration("calibration_config"),
|
LaunchConfiguration("calibration_config"),
|
||||||
{
|
{
|
||||||
@@ -386,7 +390,7 @@ def _launch_stack(context):
|
|||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
package_share = Path(
|
package_share = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("linkerhand_calibration")
|
||||||
)
|
)
|
||||||
info_root = Path.home() / ".ros" / "camera_info"
|
info_root = Path.home() / ".ros" / "camera_info"
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
+3
-3
@@ -26,7 +26,7 @@ def _launch(context):
|
|||||||
namespace = f"/g20_extrinsics/{view}/camera"
|
namespace = f"/g20_extrinsics/{view}/camera"
|
||||||
cameras.append(
|
cameras.append(
|
||||||
Node(
|
Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="hikrobot_camera_node",
|
executable="hikrobot_camera_node",
|
||||||
name="hikrobot_camera",
|
name="hikrobot_camera",
|
||||||
namespace=namespace,
|
namespace=namespace,
|
||||||
@@ -85,7 +85,7 @@ def _launch(context):
|
|||||||
output="screen",
|
output="screen",
|
||||||
)
|
)
|
||||||
solver = Node(
|
solver = Node(
|
||||||
package="g20_thumb_apriltag_calibration",
|
package="linkerhand_calibration",
|
||||||
executable="three_camera_extrinsics_node",
|
executable="three_camera_extrinsics_node",
|
||||||
name="g20_camera_extrinsics",
|
name="g20_camera_extrinsics",
|
||||||
output="screen",
|
output="screen",
|
||||||
@@ -138,7 +138,7 @@ def _launch(context):
|
|||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
package_share = Path(
|
package_share = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("linkerhand_calibration")
|
||||||
)
|
)
|
||||||
camera_info = Path.home() / ".ros" / "camera_info"
|
camera_info = Path.home() / ".ros" / "camera_info"
|
||||||
return LaunchDescription(
|
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
|
import numpy as np
|
||||||
|
|
||||||
from .core import PAIR_NAMES, robust_rotation_summary
|
from .core import robust_rotation_summary
|
||||||
from .pnp import SquareTagPose
|
from .pnp import SquareTagPose
|
||||||
|
|
||||||
|
|
||||||
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
|
|||||||
"t3_t4": ("t3", "t4"),
|
"t3_t4": ("t3", "t4"),
|
||||||
"t4_t5": ("t4", "t5"),
|
"t4_t5": ("t4", "t5"),
|
||||||
}
|
}
|
||||||
|
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@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
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
|
||||||
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
|
||||||
|
|
||||||
|
|
||||||
def camera_info_fingerprint(
|
def camera_info_fingerprint(
|
||||||
*,
|
*,
|
||||||
width: int,
|
width: int,
|
||||||
@@ -81,66 +78,61 @@ class CameraCalibrationIdentity:
|
|||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class ThreeCameraExtrinsics:
|
class CameraExtrinsics:
|
||||||
"""Transforms points from each camera optical frame into front optical."""
|
"""Transforms every declared camera into one Profile-selected reference."""
|
||||||
|
|
||||||
cameras: Mapping[str, CameraCalibrationIdentity]
|
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]
|
quality: Mapping[str, float]
|
||||||
|
|
||||||
def transform(self, view: str) -> np.ndarray:
|
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}")
|
raise KeyError(f"extrinsics do not contain view {view}")
|
||||||
return np.asarray(self.front_from_view[view], dtype=float).copy()
|
return np.asarray(self.reference_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:
|
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:
|
if int(payload.get("schema_version", -1)) != 1:
|
||||||
raise ValueError("camera extrinsics schema_version must be 1")
|
raise ValueError("camera extrinsics schema_version must be 1")
|
||||||
if payload.get("reference_view") != "front":
|
reference = str(reference_view)
|
||||||
raise ValueError("camera extrinsics reference_view must be front")
|
if payload.get("reference_view") != reference:
|
||||||
|
raise ValueError(
|
||||||
|
"camera extrinsics reference_view differs from the Profile"
|
||||||
|
)
|
||||||
cameras = payload.get("cameras")
|
cameras = payload.get("cameras")
|
||||||
transforms = payload.get("front_from_view")
|
transforms = payload.get(f"{reference}_from_view")
|
||||||
quality = payload.get("quality")
|
quality = payload.get("quality")
|
||||||
if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
|
views = tuple(str(view) for view in required_views)
|
||||||
raise ValueError("camera extrinsics must contain front/side/top cameras")
|
if not views or len(set(views)) != len(views):
|
||||||
if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
|
raise ValueError("required extrinsic views must be non-empty and unique")
|
||||||
raise ValueError("camera extrinsics must contain all three transforms")
|
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")):
|
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
|
||||||
raise ValueError("camera extrinsics quality is not passed")
|
raise ValueError("camera extrinsics quality is not passed")
|
||||||
quality_limits = {
|
for key, limit in dict(quality_limits or {}).items():
|
||||||
"reprojection_rms_px": 1.2,
|
|
||||||
"maximum_rotation_repeatability_deg": 0.3,
|
|
||||||
"maximum_translation_repeatability_m": 0.0015,
|
|
||||||
}
|
|
||||||
for key, limit in quality_limits.items():
|
|
||||||
value = float(quality.get(key, float("inf")))
|
value = float(quality.get(key, float("inf")))
|
||||||
if not np.isfinite(value) or value > limit:
|
if not np.isfinite(value) or value > limit:
|
||||||
raise ValueError(
|
raise ValueError(
|
||||||
f"camera extrinsics {key}={value} exceeds {limit}"
|
f"camera extrinsics {key}={value} exceeds {limit}"
|
||||||
)
|
)
|
||||||
for key in ("front_side_captures", "front_top_captures"):
|
for key, minimum in dict(minimum_capture_counts or {}).items():
|
||||||
if int(quality.get(key, 0)) < 15:
|
if int(quality.get(key, 0)) < int(minimum):
|
||||||
raise ValueError(f"camera extrinsics {key} must be at least 15")
|
raise ValueError(
|
||||||
for view in VIEWS:
|
f"camera extrinsics {key} must be at least {minimum}"
|
||||||
|
)
|
||||||
|
for view in views:
|
||||||
identity = cameras[view]
|
identity = cameras[view]
|
||||||
if not isinstance(identity, Mapping):
|
if not isinstance(identity, Mapping):
|
||||||
raise ValueError(f"{view} camera identity must be an object")
|
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("translation_xyz_m", ()),
|
||||||
transform.get("quaternion_xyzw", ()),
|
transform.get("quaternion_xyzw", ()),
|
||||||
)
|
)
|
||||||
if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
|
if view == reference and not np.allclose(
|
||||||
raise ValueError("front_from_view.front must be identity")
|
matrix, np.eye(4), atol=1.0e-9
|
||||||
serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
|
):
|
||||||
if len(set(serials)) != len(VIEWS):
|
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")
|
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()
|
source = Path(path).expanduser().resolve()
|
||||||
if not source.is_file():
|
if not source.is_file():
|
||||||
raise ValueError(f"camera extrinsics file does not exist: {source}")
|
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)
|
payload = yaml.safe_load(stream)
|
||||||
if not isinstance(payload, Mapping):
|
if not isinstance(payload, Mapping):
|
||||||
raise ValueError("camera extrinsics file must contain an object")
|
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 = {
|
cameras = {
|
||||||
view: CameraCalibrationIdentity(
|
view: CameraCalibrationIdentity(
|
||||||
serial_number=str(payload["cameras"][view]["serial_number"]),
|
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"]
|
payload["cameras"][view]["intrinsics_sha256"]
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
for view in VIEWS
|
for view in views
|
||||||
}
|
}
|
||||||
transforms = {
|
transforms = {
|
||||||
view: transform_matrix(
|
view: transform_matrix(
|
||||||
payload["front_from_view"][view]["translation_xyz_m"],
|
payload[transform_key][view]["translation_xyz_m"],
|
||||||
payload["front_from_view"][view]["quaternion_xyzw"],
|
payload[transform_key][view]["quaternion_xyzw"],
|
||||||
)
|
)
|
||||||
for view in VIEWS
|
for view in views
|
||||||
}
|
}
|
||||||
return ThreeCameraExtrinsics(
|
return CameraExtrinsics(
|
||||||
cameras=cameras,
|
cameras=cameras,
|
||||||
front_from_view=transforms,
|
reference_view=str(reference_view),
|
||||||
|
reference_from_view=transforms,
|
||||||
quality={
|
quality={
|
||||||
str(key): float(value) if isinstance(value, (int, float)) else value
|
str(key): float(value) if isinstance(value, (int, float)) else value
|
||||||
for key, value in payload["quality"].items()
|
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
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
|
||||||
profile = (
|
profile = (
|
||||||
Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
|
Path(get_package_share_directory("linkerhand_calibration"))
|
||||||
/ "config"
|
/ "config"
|
||||||
/ "fastdds_large_images.xml"
|
/ "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_msgs.msg import String
|
||||||
from std_srvs.srv import Trigger
|
from std_srvs.srv import Trigger
|
||||||
|
|
||||||
from .acquisition import (
|
from ...acquisition import (
|
||||||
StateSample,
|
StateSample,
|
||||||
TagQuality,
|
TagQuality,
|
||||||
interpolate_state_u8,
|
interpolate_state_u8,
|
||||||
@@ -34,20 +34,20 @@ from .acquisition import (
|
|||||||
tag_quality_is_valid,
|
tag_quality_is_valid,
|
||||||
update_pnp_reset_watchdog,
|
update_pnp_reset_watchdog,
|
||||||
)
|
)
|
||||||
from .core import (
|
from ...core import (
|
||||||
COMMAND_NAMES,
|
|
||||||
DIRECTION_DECREASING,
|
DIRECTION_DECREASING,
|
||||||
DIRECTION_INCREASING,
|
DIRECTION_INCREASING,
|
||||||
robust_rotation_summary,
|
robust_rotation_summary,
|
||||||
)
|
)
|
||||||
from .extrinsics import (
|
from ...core.urdf import build_correction_plan
|
||||||
|
from ...extrinsics import (
|
||||||
ThreeCameraExtrinsics,
|
ThreeCameraExtrinsics,
|
||||||
camera_info_fingerprint,
|
camera_info_fingerprint,
|
||||||
load_three_camera_extrinsics,
|
load_three_camera_extrinsics,
|
||||||
matrix_payload,
|
matrix_payload,
|
||||||
transform_matrix,
|
transform_matrix,
|
||||||
)
|
)
|
||||||
from .full_hand import (
|
from .profile import (
|
||||||
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
||||||
G20_REFERENCE_THUMB_CMC_JOINTS,
|
G20_REFERENCE_THUMB_CMC_JOINTS,
|
||||||
G20_RIGHT_19_LAYOUT,
|
G20_RIGHT_19_LAYOUT,
|
||||||
@@ -73,23 +73,24 @@ from .full_hand import (
|
|||||||
fit_joint_image_curve,
|
fit_joint_image_curve,
|
||||||
get_hand_calibration_profile,
|
get_hand_calibration_profile,
|
||||||
)
|
)
|
||||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
from ...hikrobot_camera import configure_fastdds_large_image_transport
|
||||||
from .pnp import (
|
from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
|
||||||
|
from ...pnp import (
|
||||||
SquareTagGroupPoseTracker,
|
SquareTagGroupPoseTracker,
|
||||||
SquareTagPose,
|
SquareTagPose,
|
||||||
SquareTagPoseTracker,
|
SquareTagPoseTracker,
|
||||||
)
|
)
|
||||||
from .product import get_product_calibration_contract
|
from ...product import get_product_calibration_contract
|
||||||
from .sample_schema import (
|
from ...sample_schema import (
|
||||||
SampleDataContractError,
|
SampleDataContractError,
|
||||||
canonical_sample_record,
|
canonical_sample_record,
|
||||||
fitting_sample_record,
|
fitting_sample_record,
|
||||||
fitting_sample_records,
|
fitting_sample_records,
|
||||||
validate_sample_records,
|
validate_sample_records,
|
||||||
)
|
)
|
||||||
from .storage import append_jsonl, append_jsonl_many, atomic_write_json
|
from ...storage import append_jsonl, append_jsonl_many, atomic_write_json
|
||||||
from .three_camera_diagnostics import render_three_camera_status_text_zh
|
from .reporting_zh import render_three_camera_status_text_zh
|
||||||
from .urdf_zero import (
|
from .zero_solver import (
|
||||||
LEFT_ZERO_PROFILE,
|
LEFT_ZERO_PROFILE,
|
||||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
||||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
@@ -318,13 +319,19 @@ def _palm_axis_observer_schema(
|
|||||||
def _palm_axis_resume_policy(
|
def _palm_axis_resume_policy(
|
||||||
profile: HandCalibrationProfile,
|
profile: HandCalibrationProfile,
|
||||||
session_start: Mapping[str, Any],
|
session_start: Mapping[str, Any],
|
||||||
|
compatibility_tokens: frozenset[str] | None = None,
|
||||||
) -> tuple[bool, tuple[str, ...]]:
|
) -> tuple[bool, tuple[str, ...]]:
|
||||||
"""Validate optional palm-axis checkpoint data when a model uses it."""
|
"""Validate optional palm-axis checkpoint data when a model uses it."""
|
||||||
capability = "palm_axis_relative_motion_v3"
|
capability = "palm_axis_relative_motion_v3"
|
||||||
previous = {str(value) for value in session_start.get("capabilities", [])}
|
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, ()
|
return True, ()
|
||||||
required_previous = set(profile.capabilities) - {capability}
|
required_previous = set(current) - {capability}
|
||||||
if not required_previous.issubset(previous):
|
if not required_previous.issubset(previous):
|
||||||
raise ValueError("resume checkpoint lacks required capabilities")
|
raise ValueError("resume checkpoint lacks required capabilities")
|
||||||
if capability in previous:
|
if capability in previous:
|
||||||
@@ -741,7 +748,7 @@ def _requires_pnp_tracker_reset_for_sweep(
|
|||||||
return False
|
return False
|
||||||
if is_fit_retry:
|
if is_fit_retry:
|
||||||
return True
|
return True
|
||||||
if profile.supports("precheck_sweeps"):
|
if profile.precheck_sweeps:
|
||||||
return bool(item.precheck)
|
return bool(item.precheck)
|
||||||
return not item.precheck and item.cycle == 0
|
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(
|
def _selected_pose_qualities(
|
||||||
selected: Mapping[str, SquareTagPose],
|
selected: Mapping[str, SquareTagPose],
|
||||||
live_qualities: Mapping[str, TagQuality],
|
live_qualities: Mapping[str, TagQuality],
|
||||||
@@ -1421,6 +1474,19 @@ def _unresolved_fit_failure_tasks(
|
|||||||
# failures that mixed velocity lag or firmware tracking
|
# failures that mixed velocity lag or firmware tracking
|
||||||
# deadband with mechanical hysteresis are safe to revalidate.
|
# deadband with mechanical hysteresis are safe to revalidate.
|
||||||
return True
|
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 (
|
if (
|
||||||
profile.layout_id == G20_RIGHT_19_LAYOUT
|
profile.layout_id == G20_RIGHT_19_LAYOUT
|
||||||
and joint_name
|
and joint_name
|
||||||
@@ -2015,7 +2081,7 @@ def _build_sweep_plan(
|
|||||||
"""
|
"""
|
||||||
plan: list[SweepItem] = []
|
plan: list[SweepItem] = []
|
||||||
for spec in profile.sweep_specs:
|
for spec in profile.sweep_specs:
|
||||||
if profile.supports("precheck_sweeps"):
|
if profile.precheck_sweeps:
|
||||||
plan.extend(
|
plan.extend(
|
||||||
SweepItem(spec, -1, direction, precheck=True)
|
SweepItem(spec, -1, direction, precheck=True)
|
||||||
for direction in (
|
for direction in (
|
||||||
@@ -2708,6 +2774,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
self.profile = product_contract.profile
|
self.profile = product_contract.profile
|
||||||
self.zero_profile = product_contract.zero_profile
|
self.zero_profile = product_contract.zero_profile
|
||||||
|
self.calibration_profile = product_contract.typed_profile
|
||||||
self.serial_number = str(value("serial_number"))
|
self.serial_number = str(value("serial_number"))
|
||||||
if self.serial_number == "UNSET":
|
if self.serial_number == "UNSET":
|
||||||
raise ValueError("serial_number is required")
|
raise ValueError("serial_number is required")
|
||||||
@@ -2766,6 +2833,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
self.resumed_task_keys: tuple[str, ...] = ()
|
self.resumed_task_keys: tuple[str, ...] = ()
|
||||||
self.resume_source_session = ""
|
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(
|
self.camera_extrinsics_file = Path(
|
||||||
str(value("camera_extrinsics_file"))
|
str(value("camera_extrinsics_file"))
|
||||||
).expanduser().resolve()
|
).expanduser().resolve()
|
||||||
@@ -5226,7 +5298,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
(
|
(
|
||||||
palm_axis_schema_compatible,
|
palm_axis_schema_compatible,
|
||||||
palm_axis_invalidated_tasks,
|
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:
|
except ValueError as error:
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
"resume checkpoint algorithm capabilities differ"
|
"resume checkpoint algorithm capabilities differ"
|
||||||
@@ -5247,6 +5331,54 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"resume checkpoint geometry, algorithm capabilities, Tag "
|
"resume checkpoint geometry, algorithm capabilities, Tag "
|
||||||
"layout, baseline or source URDF differs"
|
"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:
|
try:
|
||||||
changed_tag_size_ids, size_invalidated_tasks = (
|
changed_tag_size_ids, size_invalidated_tasks = (
|
||||||
resume_tasks_invalidated_by_tag_size_changes(
|
resume_tasks_invalidated_by_tag_size_changes(
|
||||||
@@ -5261,6 +5393,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
) from error
|
) from error
|
||||||
invalidated_task_set = set(size_invalidated_tasks)
|
invalidated_task_set = set(size_invalidated_tasks)
|
||||||
invalidated_task_set.update(palm_axis_invalidated_tasks)
|
invalidated_task_set.update(palm_axis_invalidated_tasks)
|
||||||
|
invalidated_task_set.update(start_position_invalidated_tasks)
|
||||||
scope_invalidated_tasks = tuple(
|
scope_invalidated_tasks = tuple(
|
||||||
getattr(self, "recalibration_task_keys", ()) or ()
|
getattr(self, "recalibration_task_keys", ()) or ()
|
||||||
)
|
)
|
||||||
@@ -5368,7 +5501,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
self.resumed_task_keys = completed
|
self.resumed_task_keys = completed
|
||||||
G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self)
|
G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self)
|
||||||
self.resume_source_session = source.parent.name
|
|
||||||
missing_tasks = [
|
missing_tasks = [
|
||||||
spec.key
|
spec.key
|
||||||
for spec in self.profile.sweep_specs
|
for spec in self.profile.sweep_specs
|
||||||
@@ -5398,6 +5530,20 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"scope_invalidated_task_keys": list(
|
"scope_invalidated_task_keys": list(
|
||||||
scope_invalidated_tasks
|
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_record_count": len(reusable),
|
||||||
"imported_attempt_floor_by_task": attempt_floor_by_task,
|
"imported_attempt_floor_by_task": attempt_floor_by_task,
|
||||||
"source_raw_samples_sha256": _file_sha256(source),
|
"source_raw_samples_sha256": _file_sha256(source),
|
||||||
@@ -5551,6 +5697,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
return response
|
return response
|
||||||
self.started = True
|
self.started = True
|
||||||
self.startup_baseline_recovered = False
|
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.sweep_items = []
|
||||||
self.pnp_task_spec = None
|
self.pnp_task_spec = None
|
||||||
selected_sweep_specs = list(self.profile.sweep_specs)
|
selected_sweep_specs = list(self.profile.sweep_specs)
|
||||||
@@ -5646,7 +5805,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.recalibration_task_keys
|
self.recalibration_task_keys
|
||||||
),
|
),
|
||||||
"command_names": list(_command_names(self)),
|
"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,
|
"reference_finger": self.profile.reference_finger,
|
||||||
"view_tags": {
|
"view_tags": {
|
||||||
view: dict(tags)
|
view: dict(tags)
|
||||||
@@ -5706,10 +5877,16 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
try:
|
try:
|
||||||
if self.resume_raw_samples_path is not None:
|
if self.resume_raw_samples_path is not None:
|
||||||
self.state = STATE_IMPORTING_BASE
|
if not self.resume_raw_samples_path.is_file():
|
||||||
self.reason = "reading_base_session_records"
|
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 = {
|
self.base_import_progress = {
|
||||||
"phase": "reading",
|
"phase": "waiting_for_new_start_pose_reference",
|
||||||
"records_read": 0,
|
"records_read": 0,
|
||||||
"bytes_read": 0,
|
"bytes_read": 0,
|
||||||
"total_bytes": int(
|
"total_bytes": int(
|
||||||
@@ -5717,10 +5894,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
),
|
),
|
||||||
"fraction": 0.0,
|
"fraction": 0.0,
|
||||||
}
|
}
|
||||||
self._publish_status(time.monotonic())
|
|
||||||
restored_tasks = self._restore_durable_task_checkpoint()
|
|
||||||
except Exception as error:
|
except Exception as error:
|
||||||
self.started = False
|
self.started = False
|
||||||
|
self.resume_checkpoint_pending = False
|
||||||
response.success = False
|
response.success = False
|
||||||
response.message = f"CFG-RESUME-009:{error}"
|
response.message = f"CFG-RESUME-009:{error}"
|
||||||
return response
|
return response
|
||||||
@@ -5728,8 +5904,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
response.success = True
|
response.success = True
|
||||||
response.message = (
|
response.message = (
|
||||||
"three-camera calibration started"
|
"three-camera calibration started"
|
||||||
if restored_tasks == 0
|
if not self.resume_checkpoint_pending
|
||||||
else f"calibration resumed with {restored_tasks} completed tasks"
|
else (
|
||||||
|
"three-camera calibration started; checkpoint will be "
|
||||||
|
"verified after the new start-pose reference is locked"
|
||||||
|
)
|
||||||
)
|
)
|
||||||
return response
|
return response
|
||||||
|
|
||||||
@@ -8157,7 +8336,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
}
|
}
|
||||||
)
|
)
|
||||||
continue
|
continue
|
||||||
if profile.supports("steady_command_checkpoints"):
|
if profile.steady_command_checkpoints:
|
||||||
command_store = getattr(
|
command_store = getattr(
|
||||||
self, "command_records_by_joint", None
|
self, "command_records_by_joint", None
|
||||||
)
|
)
|
||||||
@@ -8438,7 +8617,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"maximum",
|
"maximum",
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
if not profile.supports("directional_zero"):
|
if not profile.directional_zero:
|
||||||
checks = checks + (
|
checks = checks + (
|
||||||
(
|
(
|
||||||
"hysteresis_deg",
|
"hysteresis_deg",
|
||||||
@@ -8584,6 +8763,35 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
# error is an admissibility check for constrained circles.
|
# error is an admissibility check for constrained circles.
|
||||||
for metric, actual, limit in axis_checks:
|
for metric, actual, limit in axis_checks:
|
||||||
if actual > limit:
|
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
|
failure_joint = joint_name
|
||||||
quality_sources: tuple[str, ...] = ()
|
quality_sources: tuple[str, ...] = ()
|
||||||
if metric == "axis_pose_line_rms_mm":
|
if metric == "axis_pose_line_rms_mm":
|
||||||
@@ -8789,7 +8997,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
if index != outlier
|
if index != outlier
|
||||||
]
|
]
|
||||||
failures.append(failure)
|
failures.append(failure)
|
||||||
if profile.supports("cross_view_roll_curve"):
|
if profile.cross_view_roll_curve:
|
||||||
for primary_name, validation_name in (
|
for primary_name, validation_name in (
|
||||||
profile.axis_validation_sources or {}
|
profile.axis_validation_sources or {}
|
||||||
).items():
|
).items():
|
||||||
@@ -11565,6 +11773,20 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
name: published_zero_offsets[name]
|
name: published_zero_offsets[name]
|
||||||
for name in self.zero_profile.direct_zero_joints
|
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(
|
self.corrected_urdf_path = write_zero_corrected_urdf(
|
||||||
source_urdf=self.source_urdf_path,
|
source_urdf=self.source_urdf_path,
|
||||||
output_directory=self.corrected_urdf_output_dir,
|
output_directory=self.corrected_urdf_output_dir,
|
||||||
@@ -11572,6 +11794,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
offsets_rad=urdf_offsets,
|
offsets_rad=urdf_offsets,
|
||||||
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
|
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
|
||||||
timestamp=stamp,
|
timestamp=stamp,
|
||||||
|
correction_plan=correction_plan,
|
||||||
)
|
)
|
||||||
try:
|
try:
|
||||||
if self.standalone_thumb_calibration:
|
if self.standalone_thumb_calibration:
|
||||||
@@ -11885,6 +12108,21 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
elif self.startup_baseline_recovered and self._all_preflight_ready(now):
|
elif self.startup_baseline_recovered and self._all_preflight_ready(now):
|
||||||
locker = getattr(self, "_lock_fixed_base_references", None)
|
locker = getattr(self, "_lock_fixed_base_references", None)
|
||||||
if locker is None or locker():
|
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()
|
self._start_next_sweep()
|
||||||
else:
|
else:
|
||||||
self.reason = "locking_fixed_base_references"
|
self.reason = "locking_fixed_base_references"
|
||||||
@@ -12932,7 +13170,32 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"camera_extrinsics_error": self.extrinsics_error,
|
"camera_extrinsics_error": self.extrinsics_error,
|
||||||
"resume": {
|
"resume": {
|
||||||
"used": bool(self.resumed_task_keys),
|
"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,
|
"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_scope": self.recalibration_scope,
|
||||||
"recalibration_task_keys": list(
|
"recalibration_task_keys": list(
|
||||||
self.recalibration_task_keys
|
self.recalibration_task_keys
|
||||||
+104
-12
@@ -19,8 +19,10 @@ import numpy as np
|
|||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
from .extrinsics import load_three_camera_extrinsics
|
from ...compat import default_three_camera_config_path
|
||||||
from .full_hand import (
|
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_REFERENCE_THUMB_CMC_JOINTS,
|
||||||
G20_RIGHT_19_LAYOUT,
|
G20_RIGHT_19_LAYOUT,
|
||||||
RIGHT_19_END_ON_IMAGE_CURVE_JOINTS,
|
RIGHT_19_END_ON_IMAGE_CURVE_JOINTS,
|
||||||
@@ -36,10 +38,11 @@ from .full_hand import (
|
|||||||
get_hand_calibration_profile,
|
get_hand_calibration_profile,
|
||||||
validate_compact_payload,
|
validate_compact_payload,
|
||||||
)
|
)
|
||||||
from .product import get_product_calibration_contract
|
from ...product import get_product_calibration_contract
|
||||||
from .sample_schema import fitting_sample_record, fitting_sample_records
|
from ...sample_schema import fitting_sample_record, fitting_sample_records
|
||||||
from .storage import atomic_write_json
|
from ...storage import atomic_write_json
|
||||||
from .urdf_zero import (
|
from .publication import clamp_compact_payload_to_urdf_limits
|
||||||
|
from .zero_solver import (
|
||||||
JointAxisMeasurement,
|
JointAxisMeasurement,
|
||||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
||||||
UrdfKinematicModel,
|
UrdfKinematicModel,
|
||||||
@@ -76,6 +79,42 @@ def _sha256(path: Path) -> str:
|
|||||||
return digest.hexdigest()
|
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:
|
def _output_suffix(output_tag: str | None) -> str:
|
||||||
"""Return a filename-safe suffix for a non-destructive replay variant."""
|
"""Return a filename-safe suffix for a non-destructive replay variant."""
|
||||||
if output_tag is None:
|
if output_tag is None:
|
||||||
@@ -795,6 +834,7 @@ def _quality_failures(
|
|||||||
if (
|
if (
|
||||||
profile.record_specs[name].zero_kind
|
profile.record_specs[name].zero_kind
|
||||||
!= "axis_cross_view_validation"
|
!= "axis_cross_view_validation"
|
||||||
|
and profile.record_specs[name].pose_axis_line_required
|
||||||
and cross_view_side_line_source(axis) is None
|
and cross_view_side_line_source(axis) is None
|
||||||
and axis.pose_axis_line_rms_m
|
and axis.pose_axis_line_rms_m
|
||||||
> float(parameters["axis_maximum_pose_line_rms_m"])
|
> float(parameters["axis_maximum_pose_line_rms_m"])
|
||||||
@@ -1009,9 +1049,8 @@ def replay_session(
|
|||||||
output_tag: str | None = None,
|
output_tag: str | None = None,
|
||||||
) -> dict[str, Any]:
|
) -> dict[str, Any]:
|
||||||
session = Path(session_dir).expanduser().resolve()
|
session = Path(session_dir).expanduser().resolve()
|
||||||
package_root = Path(__file__).resolve().parents[1]
|
|
||||||
config = (
|
config = (
|
||||||
package_root / "config" / "three_camera_calibration.yaml"
|
default_three_camera_config_path()
|
||||||
if config_file is None
|
if config_file is None
|
||||||
else Path(config_file).expanduser().resolve()
|
else Path(config_file).expanduser().resolve()
|
||||||
)
|
)
|
||||||
@@ -1024,6 +1063,7 @@ def replay_session(
|
|||||||
palm_axis_records_by_source,
|
palm_axis_records_by_source,
|
||||||
raw_path,
|
raw_path,
|
||||||
) = _load_raw_session(session)
|
) = _load_raw_session(session)
|
||||||
|
legacy_coupled_zero_solver = uses_coupled_full_hand_zero_solver(start)
|
||||||
model = str(start.get("model", "G20")).strip().upper()
|
model = str(start.get("model", "G20")).strip().upper()
|
||||||
side = str(start["hand_type"]).lower()
|
side = str(start["hand_type"]).lower()
|
||||||
requested_layout_id = str(
|
requested_layout_id = str(
|
||||||
@@ -1199,9 +1239,33 @@ def replay_session(
|
|||||||
raise ValueError(
|
raise ValueError(
|
||||||
f"{name} has {len(records)}/18 steady command checkpoints"
|
f"{name} has {len(records)}/18 steady command checkpoints"
|
||||||
)
|
)
|
||||||
command_fits[name] = _fit_curve(
|
zero_command = int(
|
||||||
name, records, profile=profile, baseline=baseline
|
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(
|
command_gap_limit = math.radians(
|
||||||
float(parameters.get("command_maximum_direction_gap_deg", 2.0))
|
float(parameters.get("command_maximum_direction_gap_deg", 2.0))
|
||||||
)
|
)
|
||||||
@@ -1300,10 +1364,23 @@ def replay_session(
|
|||||||
source_urdf=source_urdf,
|
source_urdf=source_urdf,
|
||||||
repetitions=repetitions,
|
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 = (
|
palm_orientation_measurements, palm_orientation_rejections = (
|
||||||
fit_partial_palm_orientation_measurements(
|
fit_partial_palm_orientation_measurements(
|
||||||
sources=profile.palm_orientation_sources,
|
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,
|
motor_by_source=profile.palm_axis_motor_by_source,
|
||||||
baseline_command_u8=baseline,
|
baseline_command_u8=baseline,
|
||||||
cycles=range(repetitions),
|
cycles=range(repetitions),
|
||||||
@@ -1543,7 +1620,7 @@ def replay_session(
|
|||||||
fits: Mapping[str, JointCurveFit],
|
fits: Mapping[str, JointCurveFit],
|
||||||
arguments: Mapping[str, Any],
|
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)
|
return solve_urdf_zero_offsets(curves=fits, **arguments)
|
||||||
thumb_profile = get_right_19_thumb_zero_profile()
|
thumb_profile = get_right_19_thumb_zero_profile()
|
||||||
thumb_names = set(thumb_profile.direct_zero_joints)
|
thumb_names = set(thumb_profile.direct_zero_joints)
|
||||||
@@ -1800,6 +1877,19 @@ def replay_session(
|
|||||||
else None
|
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)
|
validate_compact_payload(payload)
|
||||||
if write_outputs:
|
if write_outputs:
|
||||||
if _sha256(source_urdf) != source_hash_before:
|
if _sha256(source_urdf) != source_hash_before:
|
||||||
@@ -1901,6 +1991,8 @@ def replay_session(
|
|||||||
"maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
|
"maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
|
||||||
},
|
},
|
||||||
"corrected_urdf_sha256": candidate_hash,
|
"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,
|
"final_json": str(final_json) if write_outputs else None,
|
||||||
"corrected_urdf": str(final_urdf) 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
|
import numpy as np
|
||||||
|
|
||||||
from .core import COMMAND_NAMES
|
from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
|
||||||
from .trajectory import (
|
from ...trajectory import (
|
||||||
_angle_for_circle,
|
_angle_for_circle,
|
||||||
_fit_circle_with_axis,
|
_fit_circle_with_axis,
|
||||||
_fit_joint_curve,
|
_fit_joint_curve,
|
||||||
_fit_plane_axis,
|
_fit_plane_axis,
|
||||||
_orient_circle_positive,
|
_orient_circle_positive,
|
||||||
)
|
)
|
||||||
from .zero_calibration import (
|
from ...zero_calibration import (
|
||||||
_fit_circle,
|
_fit_circle,
|
||||||
_trajectory_arc_rad,
|
_trajectory_arc_rad,
|
||||||
circular_median_rad,
|
circular_median_rad,
|
||||||
@@ -51,6 +51,12 @@ class JointSpec:
|
|||||||
child_role: str | None
|
child_role: str | None
|
||||||
source_joint: str | None = None
|
source_joint: str | None = None
|
||||||
zero_kind: 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
|
@property
|
||||||
def measured(self) -> bool:
|
def measured(self) -> bool:
|
||||||
@@ -129,6 +135,12 @@ class HandCalibrationProfile:
|
|||||||
command_names: tuple[str, ...] = COMMAND_NAMES
|
command_names: tuple[str, ...] = COMMAND_NAMES
|
||||||
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
|
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
|
||||||
capabilities: frozenset[str] = frozenset()
|
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
|
@property
|
||||||
def command_count(self) -> int:
|
def command_count(self) -> int:
|
||||||
@@ -537,7 +549,13 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
|||||||
zero_kind="urdf_axis_chain",
|
zero_kind="urdf_axis_chain",
|
||||||
),
|
),
|
||||||
"thumb_ip": JointSpec(
|
"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"):
|
for finger in ("index", "middle", "ring", "pinky"):
|
||||||
@@ -703,6 +721,12 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
|||||||
"palm_axis_relative_motion_v3",
|
"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
|
import numpy as np
|
||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
from .full_hand import (
|
from .profile import (
|
||||||
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
||||||
G20_RIGHT_19_LAYOUT,
|
G20_RIGHT_19_LAYOUT,
|
||||||
get_hand_calibration_profile,
|
get_hand_calibration_profile,
|
||||||
validate_compact_payload,
|
validate_compact_payload,
|
||||||
)
|
)
|
||||||
from .product import ProductConfig, sha256_file
|
from ...product import ProductConfig, sha256_file
|
||||||
from .storage import atomic_write_json
|
from ...storage import atomic_write_json
|
||||||
from .urdf_zero import (
|
from ...core.urdf import UrdfCorrectionPlan, build_correction_plan
|
||||||
|
from .zero_solver import (
|
||||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
get_zero_calibration_profile,
|
get_zero_calibration_profile,
|
||||||
)
|
)
|
||||||
@@ -202,6 +203,7 @@ def verify_corrected_urdf(
|
|||||||
*,
|
*,
|
||||||
expected_offsets_rad: Mapping[str, float] | None = None,
|
expected_offsets_rad: Mapping[str, float] | None = None,
|
||||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||||
|
correction_plan: UrdfCorrectionPlan | None = None,
|
||||||
) -> tuple[str, ...]:
|
) -> tuple[str, ...]:
|
||||||
"""Prove that only active-joint origin.rpy attributes changed.
|
"""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")
|
source_text = Path(source).read_text(encoding="utf-8")
|
||||||
corrected_text = Path(corrected).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)
|
before = _joint_blocks(source_text)
|
||||||
after = _joint_blocks(corrected_text)
|
after = _joint_blocks(corrected_text)
|
||||||
if set(before) != set(after):
|
if set(before) != set(after):
|
||||||
@@ -243,6 +252,12 @@ def verify_corrected_urdf(
|
|||||||
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
||||||
}
|
}
|
||||||
if endpoint_offsets:
|
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)
|
source_joints = _joint_elements(source)
|
||||||
corrected_joints = _joint_elements(corrected)
|
corrected_joints = _joint_elements(corrected)
|
||||||
for name, offset in endpoint_offsets.items():
|
for name, offset in endpoint_offsets.items():
|
||||||
@@ -745,11 +760,20 @@ def finalize_session_artifacts(
|
|||||||
name: offsets[name]
|
name: offsets[name]
|
||||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
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(
|
changed_joints = verify_corrected_urdf(
|
||||||
config.source_urdf,
|
config.source_urdf,
|
||||||
paths["urdf"],
|
paths["urdf"],
|
||||||
expected_offsets_rad=offsets,
|
expected_offsets_rad=offsets,
|
||||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||||
|
correction_plan=correction_plan,
|
||||||
)
|
)
|
||||||
if standalone_thumb:
|
if standalone_thumb:
|
||||||
clipped_runtime_joints = {}
|
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_msgs.msg import String
|
||||||
from std_srvs.srv import Trigger
|
from std_srvs.srv import Trigger
|
||||||
|
|
||||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
from ...hikrobot_camera import configure_fastdds_large_image_transport
|
||||||
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
|
from ...operator_report import (
|
||||||
from .product import ProductConfig, load_product_config, sha256_file
|
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 .publication import atomic_session_pointer, finalize_session_artifacts
|
||||||
from .storage import atomic_write_json
|
from ...storage import atomic_write_json
|
||||||
|
|
||||||
|
|
||||||
EXIT_PASS = 0
|
EXIT_PASS = 0
|
||||||
@@ -137,7 +141,7 @@ class ProgressConsole:
|
|||||||
def _default_product_config() -> Path:
|
def _default_product_config() -> Path:
|
||||||
try:
|
try:
|
||||||
installed = Path(
|
installed = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("linkerhand_calibration")
|
||||||
) / "config" / "g20_right_product.yaml"
|
) / "config" / "g20_right_product.yaml"
|
||||||
if installed.is_file():
|
if installed.is_file():
|
||||||
return installed
|
return installed
|
||||||
@@ -145,7 +149,7 @@ def _default_product_config() -> Path:
|
|||||||
pass
|
pass
|
||||||
return (
|
return (
|
||||||
Path.cwd()
|
Path.cwd()
|
||||||
/ "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
|
/ "src/linkerhand_calibration/config/g20_right_product.yaml"
|
||||||
).resolve()
|
).resolve()
|
||||||
|
|
||||||
|
|
||||||
@@ -188,7 +192,7 @@ def _launch_command(
|
|||||||
return [
|
return [
|
||||||
"ros2",
|
"ros2",
|
||||||
"launch",
|
"launch",
|
||||||
"g20_thumb_apriltag_calibration",
|
"linkerhand_calibration",
|
||||||
"three_camera_calibration.launch.py",
|
"three_camera_calibration.launch.py",
|
||||||
*(f"{name}:={value}" for name, value in values.items()),
|
*(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.spatial.transform import Rotation
|
||||||
from scipy.stats import t as student_t
|
from scipy.stats import t as student_t
|
||||||
|
|
||||||
from .core import fit_rotation_axis, robust_rotation_summary
|
from ...core import fit_rotation_axis, robust_rotation_summary
|
||||||
from .full_hand import (
|
from ...core.urdf import UrdfCorrectionPlan
|
||||||
|
from .profile import (
|
||||||
G20_RIGHT_19_LAYOUT,
|
G20_RIGHT_19_LAYOUT,
|
||||||
IMAGE_TRAJECTORY_JOINTS,
|
IMAGE_TRAJECTORY_JOINTS,
|
||||||
LEFT_HAND_PROFILE,
|
LEFT_HAND_PROFILE,
|
||||||
@@ -28,8 +29,8 @@ from .full_hand import (
|
|||||||
JointCurveFit,
|
JointCurveFit,
|
||||||
get_hand_calibration_profile,
|
get_hand_calibration_profile,
|
||||||
)
|
)
|
||||||
from .sample_schema import explicit_domain_value
|
from ...sample_schema import explicit_domain_value
|
||||||
from .trajectory import (
|
from ...trajectory import (
|
||||||
_fit_circle_with_axis,
|
_fit_circle_with_axis,
|
||||||
_fit_joint_curve,
|
_fit_joint_curve,
|
||||||
_fit_plane_axis,
|
_fit_plane_axis,
|
||||||
@@ -3850,7 +3851,7 @@ def solve_urdf_zero_offsets(
|
|||||||
if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad:
|
if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad:
|
||||||
cone_range = float(np.ptp(cone_mismatches))
|
cone_range = float(np.ptp(cone_mismatches))
|
||||||
stable_product_bias = bool(
|
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 maximum_systematic_axis_cone_bias_rad is not None
|
||||||
and len(cone_mismatches) >= 4
|
and len(cone_mismatches) >= 4
|
||||||
and maximum_cone_mismatch
|
and maximum_cone_mismatch
|
||||||
@@ -4454,6 +4455,7 @@ def write_zero_corrected_urdf(
|
|||||||
offsets_rad: Mapping[str, float],
|
offsets_rad: Mapping[str, float],
|
||||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||||
timestamp: str | None = None,
|
timestamp: str | None = None,
|
||||||
|
correction_plan: UrdfCorrectionPlan | None = None,
|
||||||
) -> Path:
|
) -> Path:
|
||||||
source = Path(source_urdf).expanduser().resolve()
|
source = Path(source_urdf).expanduser().resolve()
|
||||||
output = Path(output_directory).expanduser().resolve()
|
output = Path(output_directory).expanduser().resolve()
|
||||||
@@ -4475,6 +4477,15 @@ def write_zero_corrected_urdf(
|
|||||||
}
|
}
|
||||||
if not set(endpoint_offsets) <= set(offsets):
|
if not set(endpoint_offsets) <= set(offsets):
|
||||||
raise ValueError("endpoint-anchored offsets must be URDF zero targets")
|
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(
|
if any(
|
||||||
not math.isfinite(value) or abs(value) > math.radians(90.0)
|
not math.isfinite(value) or abs(value) > math.radians(90.0)
|
||||||
for value in offsets.values()
|
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,
|
interpolate_state_u8,
|
||||||
tag_quality_is_valid,
|
tag_quality_is_valid,
|
||||||
)
|
)
|
||||||
from .core import (
|
from .compat.legacy.thumb_core import (
|
||||||
BASELINE_COMMAND,
|
BASELINE_COMMAND,
|
||||||
COMMAND_NAMES,
|
COMMAND_NAMES,
|
||||||
DIRECTION_DECREASING,
|
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} 次异常轮补采)"
|
f"{fit_attempt_limit - 1} 次异常轮补采)"
|
||||||
)
|
)
|
||||||
resume = status.get("resume", {})
|
resume = status.get("resume", {})
|
||||||
if isinstance(resume, Mapping) and resume.get("used"):
|
if isinstance(resume, Mapping):
|
||||||
lines.append(
|
position_policy = str(resume.get("start_position_policy", ""))
|
||||||
"断点:已恢复 "
|
if resume.get("checkpoint_pending"):
|
||||||
f"{int(resume.get('completed_task_count', 0))}/"
|
lines.append(
|
||||||
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
|
"断点:正在建立本次起始位置基准,完成后再决定是否复用旧任务"
|
||||||
"失败任务已丢弃并重新采集"
|
)
|
||||||
)
|
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)
|
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
|
from pathlib import Path
|
||||||
import re
|
import re
|
||||||
from typing import Any, Mapping
|
from typing import Any, Mapping
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
from .compat import (
|
||||||
from .full_hand import (
|
product_profile_key,
|
||||||
G20_RIGHT_19_LAYOUT,
|
resolve_legacy_profile_alias,
|
||||||
HandCalibrationProfile,
|
resolve_renamed_package_path,
|
||||||
get_hand_calibration_profile,
|
|
||||||
)
|
)
|
||||||
from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile
|
from .core import CalibrationProfile, ProfileKey
|
||||||
|
from .core.geometry import camera_info_fingerprint, load_camera_extrinsics
|
||||||
|
from .models import RegisteredProfile, get_default_registry
|
||||||
VIEWS = ("front", "side", "top")
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class ProductCalibrationContract:
|
class ProductCalibrationContract:
|
||||||
"""Declarative boundary between one hand model and the shared engine."""
|
"""Compatibility facade over one typed, locally registered profile."""
|
||||||
|
|
||||||
model: str
|
registered: RegisteredProfile
|
||||||
side: str
|
|
||||||
layout_id: str
|
@property
|
||||||
profile: HandCalibrationProfile
|
def model(self) -> str:
|
||||||
zero_profile: ZeroCalibrationProfile
|
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
|
@property
|
||||||
def required_tag_ids(self) -> frozenset[int]:
|
def required_tag_ids(self) -> frozenset[int]:
|
||||||
return frozenset(
|
return self.typed_profile.vision.tag_ids
|
||||||
int(tag_id)
|
|
||||||
for tags in self.profile.view_tags.values()
|
|
||||||
for tag_id in tags.values()
|
|
||||||
)
|
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def views(self) -> tuple[str, ...]:
|
def views(self) -> tuple[str, ...]:
|
||||||
return tuple(self.profile.view_tags)
|
return self.typed_profile.vision.view_names
|
||||||
|
|
||||||
|
|
||||||
_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
|
|
||||||
|
|
||||||
|
|
||||||
def get_product_calibration_contract(
|
def get_product_calibration_contract(
|
||||||
model: str, side: str, layout_id: str
|
model: str, side: str, layout_id: str, revision: int = 1
|
||||||
) -> ProductCalibrationContract:
|
) -> ProductCalibrationContract:
|
||||||
key = (
|
key = resolve_legacy_profile_alias(
|
||||||
str(model).strip().upper(),
|
ProfileKey(model, side, layout_id, revision)
|
||||||
str(side).strip().lower(),
|
|
||||||
str(layout_id).strip().lower(),
|
|
||||||
)
|
)
|
||||||
try:
|
return ProductCalibrationContract(
|
||||||
return _PRODUCT_CONTRACTS[key]
|
get_default_registry().get(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"
|
|
||||||
),
|
|
||||||
)
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -142,10 +95,7 @@ def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path:
|
|||||||
text = str(value).strip()
|
text = str(value).strip()
|
||||||
if not text:
|
if not text:
|
||||||
raise ValueError(f"{name} is required")
|
raise ValueError(f"{name} is required")
|
||||||
candidate = Path(text).expanduser()
|
return resolve_renamed_package_path(text, workspace)
|
||||||
if not candidate.is_absolute():
|
|
||||||
candidate = workspace / candidate
|
|
||||||
return candidate.resolve()
|
|
||||||
|
|
||||||
|
|
||||||
def _camera_info_fingerprint(path: Path) -> str:
|
def _camera_info_fingerprint(path: Path) -> str:
|
||||||
@@ -190,12 +140,18 @@ def _custom_pnp_tag_sizes_m_by_id(
|
|||||||
) -> dict[int, float]:
|
) -> dict[int, float]:
|
||||||
with path.open("r", encoding="utf-8") as stream:
|
with path.open("r", encoding="utf-8") as stream:
|
||||||
payload = _mapping(yaml.safe_load(stream), str(path))
|
payload = _mapping(yaml.safe_load(stream), str(path))
|
||||||
parameters = _mapping(
|
candidates = [
|
||||||
_mapping(payload.get("g20_calibration"), "g20_calibration").get(
|
_mapping(node, str(name)).get("ros__parameters")
|
||||||
"ros__parameters"
|
for name, node in payload.items()
|
||||||
),
|
if isinstance(node, Mapping)
|
||||||
"ros__parameters",
|
and isinstance(node.get("ros__parameters"), Mapping)
|
||||||
)
|
and "tag_size_m" in node["ros__parameters"]
|
||||||
|
]
|
||||||
|
if len(candidates) != 1:
|
||||||
|
raise ValueError(
|
||||||
|
"calibration config must contain exactly one Tag-size parameter set"
|
||||||
|
)
|
||||||
|
parameters = _mapping(candidates[0], "ros__parameters")
|
||||||
default = float(parameters.get("tag_size_m", 0.0))
|
default = float(parameters.get("tag_size_m", 0.0))
|
||||||
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
|
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
|
||||||
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
|
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
|
||||||
@@ -208,10 +164,37 @@ def _custom_pnp_tag_sizes_m_by_id(
|
|||||||
return result
|
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)
|
@dataclass(frozen=True)
|
||||||
class ProductConfig:
|
class ProductConfig:
|
||||||
path: Path
|
path: Path
|
||||||
workspace: Path
|
workspace: Path
|
||||||
|
schema_version: int
|
||||||
|
profile_key: ProfileKey
|
||||||
|
namespace: str
|
||||||
model: str
|
model: str
|
||||||
side: str
|
side: str
|
||||||
tag_layout: str
|
tag_layout: str
|
||||||
@@ -248,23 +231,22 @@ def load_product_config(
|
|||||||
raise ValueError(f"product config does not exist: {source}")
|
raise ValueError(f"product config does not exist: {source}")
|
||||||
with source.open("r", encoding="utf-8") as stream:
|
with source.open("r", encoding="utf-8") as stream:
|
||||||
raw = _mapping(yaml.safe_load(stream), str(source))
|
raw = _mapping(yaml.safe_load(stream), str(source))
|
||||||
if int(raw.get("schema_version", -1)) != 1:
|
profile_key = product_profile_key(raw)
|
||||||
raise ValueError("product config schema_version must be 1")
|
|
||||||
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
|
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
|
||||||
serial = str(raw.get("serial_number", ""))
|
serial = str(raw.get("serial_number", ""))
|
||||||
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
||||||
raise ValueError("serial_number is invalid")
|
raise ValueError("serial_number is invalid")
|
||||||
model = str(raw.get("model", "")).strip().upper()
|
model = profile_key.model
|
||||||
side = str(raw.get("side", "")).strip().lower()
|
side = profile_key.side
|
||||||
# Keep the deployed G20 schema compatible while making the layout an
|
layout = profile_key.layout
|
||||||
# explicit product choice for all new configurations.
|
contract = get_product_calibration_contract(
|
||||||
layout = str(
|
model, side, layout, profile_key.revision
|
||||||
raw.get(
|
)
|
||||||
"tag_layout",
|
namespace = str(
|
||||||
G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "",
|
raw.get("namespace", contract.typed_profile.namespace)
|
||||||
)
|
).strip()
|
||||||
).strip().lower()
|
if not namespace.startswith("/") or namespace.endswith("/"):
|
||||||
contract = get_product_calibration_contract(model, side, layout)
|
raise ValueError("namespace must be an absolute ROS namespace")
|
||||||
can_interface = str(raw.get("can_interface", "")).strip()
|
can_interface = str(raw.get("can_interface", "")).strip()
|
||||||
if not can_interface:
|
if not can_interface:
|
||||||
raise ValueError("can_interface is required")
|
raise ValueError("can_interface is required")
|
||||||
@@ -317,6 +299,7 @@ def load_product_config(
|
|||||||
actual = sha256_file(candidate)
|
actual = sha256_file(candidate)
|
||||||
if actual != expected:
|
if actual != expected:
|
||||||
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
|
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)
|
expected_tag_ids = set(contract.required_tag_ids)
|
||||||
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
||||||
@@ -335,13 +318,16 @@ def load_product_config(
|
|||||||
|
|
||||||
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
||||||
required_views = tuple(contract.views)
|
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):
|
if set(camera_raw) != set(required_views):
|
||||||
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
||||||
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
vision = contract.typed_profile.vision
|
||||||
|
loaded_extrinsics = load_camera_extrinsics(
|
||||||
|
extrinsics,
|
||||||
|
required_views=required_views,
|
||||||
|
reference_view=vision.extrinsic_reference_view,
|
||||||
|
quality_limits=vision.extrinsics_quality_limits,
|
||||||
|
minimum_capture_counts=vision.minimum_capture_counts,
|
||||||
|
)
|
||||||
cameras: dict[str, dict[str, str]] = {}
|
cameras: dict[str, dict[str, str]] = {}
|
||||||
serials: set[str] = set()
|
serials: set[str] = set()
|
||||||
for view in required_views:
|
for view in required_views:
|
||||||
@@ -382,6 +368,9 @@ def load_product_config(
|
|||||||
return ProductConfig(
|
return ProductConfig(
|
||||||
path=source,
|
path=source,
|
||||||
workspace=root,
|
workspace=root,
|
||||||
|
schema_version=int(raw["schema_version"]),
|
||||||
|
profile_key=profile_key,
|
||||||
|
namespace=namespace,
|
||||||
model=model,
|
model=model,
|
||||||
side=side,
|
side=side,
|
||||||
tag_layout=layout,
|
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