8 Commits

Author SHA1 Message Date
admin 08fe190b3a O6原始urdf 2026-09-02 13:36:16 +08:00
admin f7aeef87a8 L6右手标定 2026-09-02 13:26:32 +08:00
admin 2b7c1f92e7 原始urdf位置修改 2026-09-01 13:57:37 +08:00
admin 1ed36ecdd8 标定代码结构修改 2026-09-01 11:51:28 +08:00
admin 7f84225ba8 refactor: dispatch formal calibration through model profiles 2026-08-31 19:22:51 +08:00
admin ba9f1b25e8 refactor: establish reusable calibration architecture 2026-08-31 18:33:31 +08:00
admin 0d606c2ba2 refactor: rename calibration package 2026-08-31 18:06:15 +08:00
admin 06c050e446 test: restore calibration regression baseline 2026-08-31 17:58:11 +08:00
220 changed files with 13784 additions and 524 deletions
+2
View File
@@ -69,6 +69,8 @@ Thumbs.db
*_mapping_quality.json
# Device-specific robot descriptions derived from local calibration runs
# Includes both full and partial timestamped zero-calibration outputs.
/src/linkerhand_calibration/urdf/*/*_zero_calibrated_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
@@ -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
@@ -230,7 +230,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
}
),
"L6": HandConfig(
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
init_pos=[250] * 6,
preset_actions={
@@ -33,6 +33,10 @@ _CANONICAL_COMMAND_NAMES = {
"thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
],
"L6": [
"thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch",
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
],
}
_CANONICAL_COMMAND_BOUNDS = {
@@ -43,6 +47,7 @@ _CANONICAL_COMMAND_BOUNDS = {
*[(0, 255)] * 5,
],
"O6": [(0, 255)] * 6,
"L6": [(0, 255)] * 6,
}
+12
View File
@@ -0,0 +1,12 @@
from gui_control.config.constants import HAND_CONFIGS
def test_l6_gui_uses_the_sdk_channel_order() -> None:
assert HAND_CONFIGS["L6"].joint_names_en == [
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
]
@@ -1,3 +1,5 @@
from collections import deque
import can
import time, sys
import threading
@@ -57,6 +59,14 @@ class LinkerHandL6Can:
self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
self.is_lock = False
self.version = None
# L6 replies to a six-byte 0x01 position command with an immediate
# byte-for-byte echo on the same CAN ID. A zero-payload 0x01 state
# query also replies on that ID, but with the measured positions.
# Keep the two transactions distinct so command echoes never enter
# the published feedback stream used by calibration.
self._position_echo_lock = threading.Lock()
self._pending_position_echoes = deque(maxlen=32)
self._position_echo_timeout_seconds = 0.02
# Start the receiving thread
self.running = True
self.receive_thread = threading.Thread(target=self.receive_response)
@@ -111,6 +121,11 @@ class LinkerHandL6Can:
frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
data = [frame_property_value] + [int(val) for val in data_list]
msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
if frame_property_value == 0x01 and len(data_list) == 6:
with self._position_echo_lock:
self._pending_position_echoes.append(
(time.monotonic(), tuple(int(value) for value in data_list))
)
try:
self.bus.send(msg)
except can.CanError as e:
@@ -201,7 +216,23 @@ class LinkerHandL6Can:
except:
return
if frame_type == 0x01: # 0x01
self.x01 = list(response_data)
response = tuple(int(value) for value in response_data)
now = time.monotonic()
is_position_echo = False
with self._position_echo_lock:
while (
self._pending_position_echoes
and now - self._pending_position_echoes[0][0]
> self._position_echo_timeout_seconds
):
self._pending_position_echoes.popleft()
for pending in tuple(self._pending_position_echoes):
if pending[1] == response:
self._pending_position_echoes.remove(pending)
is_position_echo = True
break
if not is_position_echo:
self.x01 = list(response)
elif frame_type == 0x02: # 0x02
self.x02 = list(response_data)
elif frame_type == 0x05: # Set speed
@@ -391,7 +422,10 @@ class LinkerHandL6Can:
return self.x35
def get_finger_order(self):
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
# L6 channel 1 is the physical CMC roll actuator. Older SDK releases
# exposed the channel as ``thumb_cmc_yaw`` even though the wire order
# and mechanism have always been roll.
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
def show_fun_table(self):
pass
@@ -372,7 +372,7 @@ class LinkerHandL6RS485:
return [0] * 6
def get_finger_order(self):
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
# --------------------------------------------------
# 便捷方法
@@ -0,0 +1,72 @@
import ast
from collections import deque
from pathlib import Path
import sys
import threading
import time
from types import SimpleNamespace
PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core"
EXPECTED = [
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
]
def _finger_order(path: Path, class_name: str) -> list[str]:
module = ast.parse(path.read_text(encoding="utf-8"))
selected = next(
item
for item in module.body
if isinstance(item, ast.ClassDef) and item.name == class_name
)
method = next(
item
for item in selected.body
if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order"
)
returned = next(item for item in method.body if isinstance(item, ast.Return))
return ast.literal_eval(returned.value)
def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None:
assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED
assert _finger_order(
PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485"
) == EXPECTED
def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
linker_hand_root = PACKAGE.parent
sys.path.insert(0, str(linker_hand_root))
try:
from core.can.linker_hand_l6_can import LinkerHandL6Can
finally:
sys.path.remove(str(linker_hand_root))
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
hand.can_id = 0x27
hand.x01 = [10, 20, 30, 40, 50, 60]
hand._position_echo_lock = threading.Lock()
command = (255, 2, 253, 253, 253, 253)
hand._pending_position_echoes = deque(
[(time.monotonic(), command)], maxlen=32
)
hand._position_echo_timeout_seconds = 0.02
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)))
)
assert hand.x01 == [10, 20, 30, 40, 50, 60]
assert not hand._pending_position_echoes
measured = (250, 3, 252, 252, 252, 252)
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)))
)
assert hand.x01 == list(measured)
@@ -1,17 +1,127 @@
# G20 左右手 AprilTag 标定
# LinkerHand 专业标定包
## L6 右手局部标定(l6_right_8/v1)
本版只发布 `rh_thumb_cmc_pitch`、`rh_thumb_cmc_roll`、
`rh_pinky_mcp_pitch` 的静态零位与动态曲线,以及 `rh_thumb_dip`、
`rh_pinky_dip` 的视觉动态曲线。拇指 DIP 通过线性 mimic;小指 DIP 使用实测
双方向运行曲线和 MuJoCo 二次 equality,因为它的传动比会随屈曲角变化。生成的
修正 URDF 保留 `rh_pinky_dip` 的 `<mimic>`,因此在普通 URDF/RViz 中仍会跟随
MCP 运动;该线性回退精确对齐实测零位和闭合端点。中间行程的准确非线性轨迹由
MuJoCo equality 或下述标定桥提供。根据 L6 四指同机构的实机确认,食指、中指、
无名指的 MCP 零偏、行程、双向反馈曲线及 DIP 耦合从小指迁移;每指自己的
`origin.xyz`、axis、mesh 和惯量保持 CAD,不把迁移结果标成 Tag 实测。四指 MCP
以反馈 255 的展开端作为 CAD lower/zero 锚点;实测行程不会再被强制压回较短的
CAD upper,因此不会向四指 origin 写入系统性的负零偏。
结果指针为 `latest_partial_passed`,不会被当作六路主动关节的完整标定。
拇指 pitch/roll 根据两组已记录六路反馈值的实机/仿真姿态对比,以反馈 255
保持源 CAD joint zero,不叠加端点推断的静态偏置;小指及三指迁移仍以反馈 0
机械闭合姿态对齐源 CAD upper。该策略只改变坐标锚点,不改变视觉实测的双方向
行程曲线。现场姿态对比必须同时记录对应的六路反馈值。
先启动 SDK 和 GUI 做手动检查时使用:
```bash
ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
-p hand_type:=right -p hand_joint:=L6 -p can:=can0 -p topic_prefix:=/l6
ros2 run gui_control gui_control
```
正式一键标定由 runner 自己启动 SDK、三相机、AprilTag 和标定节点,不要同时
运行上面的 SDK/GUI 控制命令:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml
```
只检查 Profile、8 张 16 mm Tag、相机/外参哈希和只读源 URDF:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml --validate-only
```
离线回放与在线发布使用同一拟合/URDF写回路径:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml \
--offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl
```
运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、
roll、index、middle、ring、pinky;旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路
兼容别名读取,新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。
预检和正式扫描均使用 L6 硬件速度 `1` 作为上限,并由 100 Hz 余弦缓入缓出
轨迹把完整 `255↔0` 行程固定为 `6 s`;短行程按距离同比缩短。SDK 会过滤 L6
在同一 CAN ID 回送的位置命令回显,标定只使用状态查询返回的真实反馈。标定
启动时还会检查重复 SDK/GUI 发布者,避免两个进程同时访问同一只手。
标定完成后,用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节
`JointState`:
```bash
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
hand_type:=right \
calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json
```
schema v6 默认订阅 `/l6/cb_right_hand_state`,发布
`/sim/mujoco/l6/right/joint_state`。标准 URDF 的 `<mimic>` 本身只支持线性关系,
所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似;需要实测轨迹时使用该桥
或修正 URDF 内的 MuJoCo equality。
## G20右手正式一键标定
固定三相机和19张Tag安装完成后,用户只运行:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
ros2 run linkerhand_calibration calibrate_hand
```
旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名;
旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。
构建和正式调用统一为:
```bash
colcon build --packages-select linkerhand_calibration
ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml>
```
## 代码边界
- `core/`:无 ROS、无具体型号,包含领域类型、PnP/旋转数学、拟合接口、统一样本
契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。
- `runtime/`:通用会话状态机与注册 Profile 分发;ROS 消息和硬件适配只能位于
`runtime/nodes`、`runtime/adapters`。
- `models/g20/`:G20 right-19、legacy-11、运动、零位、产物和中文诊断策略。
后续型号或左右手作为新的独立 Profile 加入 `models/`,不在通用层增加分支。
- `compat/`:v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留
一版最小转发 shim,不包含算法副本。
产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、
Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改;v2 配置使用
`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明,通用层
不要求 `front/side/top`,也不假设固定 20 个命令。
仓库中存在已审定硬件会话时,可执行只读金标准检查(不会覆盖任何产物):
```bash
ros2 run linkerhand_calibration validate_g20_goldens \
calibration_output/G20_RIGHT_001
```
它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话;完整
整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。
完全独立地只标定大拇指4项任务时,使用:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
ros2 run linkerhand_calibration calibrate_hand \
--scope thumb
```
@@ -23,7 +133,7 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定,才额外使用:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
ros2 run linkerhand_calibration calibrate_hand \
--scope thumb \
--base-session calibration_output/G20_RIGHT_001/latest_passed
```
@@ -35,7 +145,7 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
只重新采集12项四指任务并合成完整整手URDF:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
ros2 run linkerhand_calibration calibrate_hand \
--scope fingers \
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
```
@@ -114,7 +224,7 @@ schema v4 JSON,不再生成schema v5运行文件。
`tag_layout:=legacy_11`,两套配置和结果schema互不覆盖:
```bash
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
hand_type:=right \
tag_layout:=g20_right_19 \
serial_number:=G20_RIGHT_001 \
@@ -271,7 +381,7 @@ ID 9 必须在拇指横摆的完整行程中持续可见。
```bash
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_extrinsics.launch.py \
output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
@@ -312,12 +422,11 @@ ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
先使用禁止运动模式检查三个机位、外参、内参和标签:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_calibration.launch.py \
hand_type:=left \
serial_number:=G20_LEFT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
commands_enabled:=false
```
@@ -335,19 +444,20 @@ ros2 run image_view image_view --ros-args \
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后,重启正式流程:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_calibration.launch.py \
hand_type:=left \
serial_number:=G20_LEFT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
can_interface:=can0
```
右手使用同一入口;默认自动选择右手SDK话题和原始URDF:
原始URDF及其mesh随`linkerhand_calibration`安装,默认根据`hand_type`自动选择。
如需调试其他CAD版本,仍可通过`source_urdf_path:=<绝对路径>`显式覆盖。
右手使用同一入口,并自动选择右手SDK话题和原始URDF:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
three_camera_calibration.launch.py \
hand_type:=right \
serial_number:=G20_RIGHT_001 \
@@ -526,7 +636,7 @@ calibration_output/G20_RIGHT_001/<时间戳>/
命令。`--output-tag` 为新产物增加安全后缀,已有JSON、URDF和验证报告不会被覆盖:
```bash
python3 -m g20_thumb_apriltag_calibration.offline_replay \
python3 -m linkerhand_calibration.offline_replay \
calibration_output/G20_RIGHT_001/20260811_120146 \
--output-tag AXIS_FRAME_V3 \
--write
@@ -543,7 +653,7 @@ python3 -m g20_thumb_apriltag_calibration.offline_replay \
`JointState`(包括5个被动关节):
```bash
ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
hand_type:=right \
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
```
@@ -615,7 +725,7 @@ sudo apt-get install -y \
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install \
--packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
--packages-select linker_hand_ros2_sdk linkerhand_calibration
source install/setup.bash
```
@@ -642,7 +752,7 @@ source install/setup.bash
先单独启动相机(不会连接机械手,也不会发送关节命令):
```bash
ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
ros2 run linkerhand_calibration hikrobot_camera_node --ros-args \
--remap __ns:=/camera/camera/color \
-p serial_number:=DB2163742 \
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
@@ -680,7 +790,7 @@ ros2 run camera_calibration cameracalibrator \
但标定节点不会发送位置运动命令,也不会允许解锁全行程扫描:
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
commands_enabled:=false
@@ -691,7 +801,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
速度,并使用单终点连续运动:
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
can_interface:=can0 \
@@ -804,7 +914,7 @@ calibration_output/<序列号>/<时间戳>/
恢复时必须显式复用原目录,否则会创建新会话:
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
```
@@ -884,7 +994,7 @@ for name, joint in data["joints"].items():
URDF:
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
front_cmc_pitch_zero.launch.py \
serial_number:=G20_LEFT_001
```
@@ -961,7 +1071,7 @@ zero_angles.table_projected_zero_rad
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量,不读取T3标签朝向。
```bash
ros2 launch g20_thumb_apriltag_calibration \
ros2 launch linkerhand_calibration \
front_cmc_roll_calibration.launch.py \
serial_number:=G20_LEFT_001
```
@@ -21,7 +21,7 @@ cameras:
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
artifacts:
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
source_urdf: package://linkerhand_calibration/urdf/g20_right/linkerhand_g20_right.urdf
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
@@ -0,0 +1,59 @@
/l6_calibration/front/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [0, 1, 2]
frames: [front_base, thumb_pitch, thumb_dip]
sizes: [0.016, 0.016, 0.016]
/l6_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [3, 4, 5]
frames: [side_base, pinky_pitch, pinky_dip]
sizes: [0.016, 0.016, 0.016]
/l6_calibration/top/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [6, 7]
frames: [top_base, thumb_roll]
sizes: [0.016, 0.016]
@@ -0,0 +1,37 @@
schema_version: 2
profile_id: L6/right/l6_right_8/v1
model: L6
side: right
tag_layout: l6_right_8
namespace: /l6_calibration
serial_number: L6_RIGHT_001
can_interface: can0
output_root: calibration_output
cameras:
front:
serial_number: DB2163742
camera_name: hikrobot_front_DB2163742
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
side:
serial_number: DB2163749
camera_name: hikrobot_side_DB2163749
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
top:
serial_number: DB2163739
camera_name: hikrobot_top_DB2163739
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
artifacts:
source_urdf: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf
source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f
tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
release:
required_independent_passes: 1
static_repeatability_deg: 1.0
@@ -0,0 +1,67 @@
l6_calibration:
ros__parameters:
command_topic: /l6/cb_right_hand_control_cmd
state_topic: /l6/cb_right_hand_state
setting_topic: /l6/cb_hand_setting_cmd
front_camera_info_topic: /l6_calibration/front/camera/camera_info
front_detections_topic: /l6_calibration/front/apriltag/detections
side_camera_info_topic: /l6_calibration/side/camera/camera_info
side_detections_topic: /l6_calibration/side/apriltag/detections
top_camera_info_topic: /l6_calibration/top/camera/camera_info
top_detections_topic: /l6_calibration/top/apriltag/detections
baseline_command_u8: [255, 255, 255, 255, 255, 255]
# L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10,
# which left fewer than 32 useful feedback bins. Speed 1 is still only a
# firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s.
# A 100 Hz cosine trajectory therefore sets the actual, model-level pace.
preflight_speed_u8: 1
formal_speed_u8: 1
speed_settle_seconds: 0.2
command_trajectory_full_range_seconds: 6.0
torque_u8: 80
repetitions: 4
preflight_checkpoints_u8: [255, 127, 0]
tag_size_m: 0.016
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
minimum_detection_rate: 0.95
# Per-Tag quality remains >=95%. With three independently detected Tags,
# the fully joined frame rate may be 0.95^3 ~= 85.7%.
minimum_joint_frame_rate: 0.85
minimum_feedback_hz: 25.0
maximum_state_image_skew_ms: 50.0
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
pnp_maximum_reprojection_error_px: 1.5
pnp_reprojection_tie_px: 1.5
pnp_maximum_pose_jump_deg: 35.0
pnp_maximum_translation_jump_m: 0.04
pnp_maximum_tag_tilt_deg: 75.0
pnp_tracker_reset_seconds: 5.0
minimum_sweep_frames: 40
minimum_state_span_u8: 240.0
minimum_sweep_bins: 32
maximum_bin_gap: 16
maximum_monotonic_correction_deg: 2.0
passive_maximum_monotonic_correction_deg: 3.0
maximum_validation_mae_deg: 1.0
maximum_validation_p95_deg: 2.0
maximum_validation_error_deg: 3.0
mimic_minimum_multiplier: 0.5
mimic_maximum_multiplier: 1.5
mimic_maximum_cycle_range: 0.03
mimic_maximum_residual_p95_deg: 2.0
endpoint_tolerance_u8: 2.0
endpoint_hold_seconds: 1.0
motor_stall_timeout_seconds: 2.0
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
automatic_sweep_retry_limit: 2
non_target_motion_tolerance_u8: 3.0
fixed_base_maximum_corner_drift_px: 2.0
fixed_base_movement_confirmation_frames: 5
@@ -0,0 +1,21 @@
"""One-release compatibility surface for the former Python package name.
New code must import :mod:`linkerhand_calibration`. Only the documented
configuration loader is re-exported here; calibration algorithms continue to
have a single implementation in the renamed package.
"""
from __future__ import annotations
import warnings
warnings.warn(
"g20_thumb_apriltag_calibration is deprecated; "
"import linkerhand_calibration instead",
DeprecationWarning,
stacklevel=2,
)
from linkerhand_calibration.product import ProductConfig, load_product_config
__all__ = ["ProductConfig", "load_product_config"]
@@ -0,0 +1,9 @@
"""Deprecated forwarding entry point for the runtime joint-state bridge."""
from linkerhand_calibration.calibrated_joint_state_bridge import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -0,0 +1,9 @@
"""Deprecated forwarding entry point for offline replay."""
from linkerhand_calibration.offline_replay import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -0,0 +1,9 @@
"""Deprecated forwarding entry point for the former Python package."""
from linkerhand_calibration.one_command import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -1,4 +1,4 @@
"""Publish calibrated G20 URDF angles from raw command/feedback u8 values."""
"""Publish profile-calibrated URDF angles from raw command/feedback u8 values."""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
@@ -14,10 +14,10 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("input_topic", default_value=""),
DeclareLaunchArgument("output_topic", default_value=""),
Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="calibrated_joint_state_bridge",
name=[
"g20_calibrated_joint_state_bridge_",
"calibrated_joint_state_bridge_",
LaunchConfiguration("hand_type"),
],
output="screen",
@@ -48,7 +48,7 @@ def _launch_stack(context):
tag_config = LaunchConfiguration("tag_config").perform(context)
zero_config = LaunchConfiguration("zero_config").perform(context)
camera = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -158,7 +158,7 @@ def _launch_stack(context):
)
zero_node = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="cmc_pitch_zero_node",
name="g20_thumb_cmc_pitch_zero",
output="screen",
@@ -203,7 +203,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
return LaunchDescription(
[
@@ -50,7 +50,7 @@ def _launch_stack(context):
"calibration_config"
).perform(context)
camera = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -160,7 +160,7 @@ def _launch_stack(context):
)
calibration_node = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="cmc_roll_calibration_node",
name="g20_thumb_cmc_roll_calibration",
output="screen",
@@ -205,7 +205,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
return LaunchDescription(
[
@@ -82,7 +82,7 @@ def _launch_stack(context):
)
camera = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -234,7 +234,7 @@ def _launch_stack(context):
)
calibration = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="calibration_node",
name="g20_thumb_calibration",
output="screen",
@@ -315,7 +315,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
default_output = str(Path.cwd() / "calibration_output")
return LaunchDescription(
@@ -1,4 +1,4 @@
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
"""Launch three Hikrobot views and one registered hand calibration owner."""
from __future__ import annotations
@@ -26,20 +26,27 @@ from launch_ros.parameter_descriptions import ParameterValue
VIEWS = ("front", "side", "top")
def _default_source_urdf(hand_type: str) -> Path:
relative = Path(
"assets/robots/hands/linker_hand"
) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
def _default_source_urdf(model: str, hand_type: str) -> Path:
relative = (
Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf"
if model.upper() == "L6" and hand_type == "right"
else Path("urdf")
/ f"{model.lower()}_{hand_type}"
/ f"linkerhand_{model.lower()}_{hand_type}.urdf"
)
package_source_or_share = Path(__file__).resolve().parents[1] / relative
try:
installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
installed = (
Path(get_package_share_directory("linkerhand_calibration"))
/ relative
)
except Exception:
installed = workspace
return workspace if workspace.is_file() else installed
installed = package_source_or_share
return installed if installed.is_file() else package_source_or_share
def _launch_stack(context):
from g20_thumb_apriltag_calibration.product import (
from linkerhand_calibration.product import (
get_product_calibration_contract,
)
@@ -56,7 +63,7 @@ def _launch_stack(context):
raise RuntimeError(str(error)) from error
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
tag_config = (
Path(requested_tag_config).expanduser().resolve()
@@ -66,6 +73,8 @@ def _launch_stack(context):
/ (
"three_camera_tags_g20_right_19.yaml"
if tag_layout == "g20_right_19"
else "l6_right_8_tags.yaml"
if tag_layout == "l6_right_8"
else "three_camera_tags_g20_right_15.yaml"
if tag_layout == "g20_right_15"
else "three_camera_tags.yaml"
@@ -81,17 +90,17 @@ def _launch_stack(context):
source_urdf = (
Path(requested_source).expanduser().resolve()
if requested_source
else _default_source_urdf(hand_type).resolve()
else _default_source_urdf(model, hand_type).resolve()
)
if not source_urdf.is_file():
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
expected_source_hash = LaunchConfiguration(
"source_urdf_expected_sha256"
).perform(context).strip().lower()
if contract.profile.supports("urdf_zero_publication"):
if contract.typed_profile.artifacts.publish_corrected_urdf:
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
raise RuntimeError(
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
"this profile requires source_urdf_expected_sha256 confirmed "
"by the CAD/hardware owner"
)
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
@@ -137,19 +146,20 @@ def _launch_stack(context):
raw_topics = []
info_topics = []
detection_topics = []
calibration_namespace = contract.typed_profile.namespace
for view in VIEWS:
namespace = f"/g20_calibration/{view}/camera"
namespace = f"{calibration_namespace}/{view}/camera"
raw_topic = f"{namespace}/image_raw"
info_topic = f"{namespace}/camera_info"
rect_topic = f"{namespace}/image_rect"
detector_namespace = f"/g20_calibration/{view}/apriltag"
detector_namespace = f"{calibration_namespace}/{view}/apriltag"
detection_topic = f"{detector_namespace}/detections"
raw_topics.append(raw_topic)
info_topics.append(info_topic)
detection_topics.append(detection_topic)
cameras.append(
Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace=namespace,
@@ -165,7 +175,9 @@ def _launch_stack(context):
"camera_name": LaunchConfiguration(
f"{view}_camera_name"
),
"frame_id": f"g20_calibration_{view}_optical_frame",
"frame_id": (
f"{model.lower()}_calibration_{view}_optical_frame"
),
"image_width": 1624,
"image_height": 1240,
"frame_rate": ParameterValue(
@@ -228,7 +240,7 @@ def _launch_stack(context):
)
vision = ComposableNodeContainer(
name="g20_three_camera_vision",
name=f"{model.lower()}_three_camera_vision",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
@@ -260,10 +272,10 @@ def _launch_stack(context):
# G20 velocity read sends another five synchronous CAN
# queries, so keep it off the trajectory-critical path.
"velocity_poll_rate": 1.0,
# Calibration sends one endpoint command per sweep. Keep
# polling the real motor state during the following motion;
# otherwise the SDK republishes a stale state for 0.2 s and
# creates 17-27 command-unit holes in the trajectory bins.
# G20 sends an endpoint and L6 streams a bounded trajectory.
# Keep polling the real motor state during either command path;
# otherwise the SDK republishes stale state and creates large
# command-unit holes in the trajectory bins.
"defer_state_reads_while_commanding": False,
"repeat_position_commands": False,
"is_touch": False,
@@ -271,11 +283,15 @@ def _launch_stack(context):
],
)
calibration = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="three_camera_calibration_node",
name="g20_calibration",
name=f"{model.lower()}_calibration",
output="screen",
emulate_tty=True,
arguments=[
"--profile-id",
contract.typed_profile.key.profile_id,
],
parameters=[
LaunchConfiguration("calibration_config"),
{
@@ -294,7 +310,7 @@ def _launch_stack(context):
# cb_<side>_hand_info has a subscriber. Calibration only used
# that topic to display a speed diagnostic, while those reads
# created 17-33 command-unit holes in position trajectories.
"info_topic": "/g20_calibration/disabled_hand_info",
"info_topic": f"{calibration_namespace}/disabled_hand_info",
"command_topic": command_topic,
"state_topic": state_topic,
"camera_extrinsics_file": LaunchConfiguration(
@@ -304,6 +320,15 @@ def _launch_stack(context):
"source_urdf_expected_sha256": LaunchConfiguration(
"source_urdf_expected_sha256"
),
"camera_extrinsics_expected_sha256": LaunchConfiguration(
"camera_extrinsics_expected_sha256"
),
"calibration_config_expected_sha256": LaunchConfiguration(
"calibration_config_expected_sha256"
),
"tag_config_expected_sha256": LaunchConfiguration(
"tag_config_expected_sha256"
),
"corrected_urdf_output_dir": LaunchConfiguration(
"corrected_urdf_output_dir"
),
@@ -358,14 +383,14 @@ def _launch_stack(context):
command_topic,
state_topic,
info_topic,
"/g20_calibration/status",
f"{calibration_namespace}/status",
],
output="screen",
)
return [
LogInfo(
msg=(
f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; "
f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; "
f"source_urdf={source_urdf}"
)
),
@@ -386,7 +411,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
info_root = Path.home() / ".ros" / "camera_info"
return LaunchDescription(
@@ -473,6 +498,15 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"source_urdf_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"camera_extrinsics_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"calibration_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"tag_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"corrected_urdf_output_dir", default_value=""
),
@@ -26,7 +26,7 @@ def _launch(context):
namespace = f"/g20_extrinsics/{view}/camera"
cameras.append(
Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace=namespace,
@@ -85,7 +85,7 @@ def _launch(context):
output="screen",
)
solver = Node(
package="g20_thumb_apriltag_calibration",
package="linkerhand_calibration",
executable="three_camera_extrinsics_node",
name="g20_camera_extrinsics",
output="screen",
@@ -138,7 +138,7 @@ def _launch(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
)
camera_info = Path.home() / ".ros" / "camera_info"
return LaunchDescription(
@@ -0,0 +1,5 @@
"""Profile-driven LinkerHand calibration and validated URDF correction."""
from .core import CalibrationProfile, ProfileKey
__all__ = ["CalibrationProfile", "ProfileKey"]
@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
import numpy as np
from .core import PAIR_NAMES, robust_rotation_summary
from .core import robust_rotation_summary
from .pnp import SquareTagPose
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
"t3_t4": ("t3", "t4"),
"t4_t5": ("t4", "t5"),
}
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
@dataclass(frozen=True)
@@ -116,7 +117,7 @@ def interpolate_state_u8(
*,
maximum_skew_ns: int,
) -> tuple[tuple[float, ...], int] | None:
"""Interpolate the 20-D hand state at an image timestamp.
"""Interpolate a profile-sized hand state at an image timestamp.
The SDK publishes state independently from the camera. Continuous
calibration must therefore use the image timestamp instead of whichever
@@ -150,7 +151,11 @@ def interpolate_state_u8(
fraction = before_gap / denominator
before_values = np.asarray(before.position_u8, dtype=float)
after_values = np.asarray(after.position_u8, dtype=float)
if before_values.shape != (20,) or after_values.shape != (20,):
if (
before_values.ndim != 1
or before_values.size == 0
or after_values.shape != before_values.shape
):
return None
interpolated = before_values + fraction * (after_values - before_values)
return (
@@ -1,4 +1,4 @@
"""Map G20 u8 feedback to URDF joint angles using one calibration JSON.
"""Map model SDK u8 feedback to URDF joint angles using one calibration JSON.
The static encoder-zero corrections in ``zero_angles`` are already baked into
the corrected URDF joint origins. This bridge therefore publishes only the
@@ -26,6 +26,7 @@ from .full_hand import (
infer_compact_payload_layout,
validate_compact_payload,
)
from .models.l6.artifacts import validate_l6_runtime_payload
G20_COMMAND_NAMES: tuple[str, ...] = (
@@ -80,12 +81,16 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = (
class CalibratedCommandMapper:
"""Validated, side-specific lookup from G20 u8 values to URDF radians."""
"""Validated, profile-specific lookup from SDK u8 values to URDF radians."""
def __init__(
self, payload: Mapping[str, Any], *, expected_side: str | None = None
) -> None:
validate_compact_payload(payload)
schema_version = int(payload["schema_version"])
if schema_version == 6:
validate_l6_runtime_payload(payload)
else:
validate_compact_payload(payload)
side = str(payload["side"]).lower()
if expected_side is not None and side != str(expected_side).lower():
raise ValueError(
@@ -95,12 +100,18 @@ class CalibratedCommandMapper:
quality = payload["quality"]
if quality.get("passed") is not True:
raise ValueError("calibration quality.passed must be true")
layout_id = infer_compact_payload_layout(payload)
profile = get_hand_calibration_profile(side, layout_id)
layout_id = (
str(payload["layout_id"])
if schema_version == 6
else infer_compact_payload_layout(payload)
)
self.side = side
self.layout_id = layout_id
self.model = str(payload["model"]).upper()
self.profile_id = str(
payload.get("profile_id", f"G20/{side}/{layout_id}/v1")
)
self.serial_number = str(payload["serial_number"])
schema_version = int(payload["schema_version"])
self.input_domain = str(
payload.get(
"curve_input_domain",
@@ -109,16 +120,29 @@ class CalibratedCommandMapper:
)
if self.input_domain not in {"command_u8", "feedback_u8"}:
raise ValueError("calibration curve_input_domain is invalid")
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in G20_URDF_JOINT_NAMES
}
if schema_version == 6:
self.command_names = tuple(str(value) for value in payload["command_names"])
self.urdf_joint_names = tuple(str(name) for name in payload["joints"])
self._motor_by_joint = {
name: int(payload["joints"][name]["motor_index"])
for name in self.urdf_joint_names
}
self.feedback_name_aliases = {"thumb_cmc_yaw": "thumb_cmc_roll"}
else:
profile = get_hand_calibration_profile(side, layout_id)
self.command_names = G20_COMMAND_NAMES
self.urdf_joint_names = G20_URDF_JOINT_NAMES
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in self.urdf_joint_names
}
self.feedback_name_aliases = {}
self._curves = {
name: tuple(
float(value)
for value in payload["joints"][name]["angle_rad"]
)
for name in G20_URDF_JOINT_NAMES
for name in self.urdf_joint_names
}
self._decreasing_curves = {
name: tuple(
@@ -127,7 +151,7 @@ class CalibratedCommandMapper:
"decreasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
for name in self.urdf_joint_names
}
self._increasing_curves = {
name: tuple(
@@ -136,7 +160,7 @@ class CalibratedCommandMapper:
"increasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
for name in self.urdf_joint_names
}
self._previous_by_motor: dict[int, float] = {}
self._direction_by_motor: dict[int, str] = {}
@@ -161,16 +185,21 @@ class CalibratedCommandMapper:
if len(set(names)) != len(names):
raise ValueError("JointState names must be unique")
by_name = dict(zip((str(name) for name in names), values))
missing = [name for name in G20_COMMAND_NAMES if name not in by_name]
for alias, canonical in self.feedback_name_aliases.items():
if alias in by_name and canonical not in by_name:
by_name[canonical] = by_name[alias]
missing = [name for name in self.command_names if name not in by_name]
if missing:
raise ValueError(
"G20 command is missing named channels: " + ",".join(missing)
f"{self.model} feedback is missing named channels: "
+ ",".join(missing)
)
command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
command = tuple(by_name[name] for name in self.command_names)
else:
if len(values) != len(G20_COMMAND_NAMES):
if len(values) != len(self.command_names):
raise ValueError(
"unnamed G20 command must contain exactly 20 positions"
f"unnamed {self.model} feedback must contain exactly "
f"{len(self.command_names)} positions"
)
command = values
indices = tuple(self._command_index(value) for value in command)
@@ -185,7 +214,7 @@ class CalibratedCommandMapper:
direction = "decreasing"
direction_by_motor[motor] = direction
result: list[float] = []
for name in G20_URDF_JOINT_NAMES:
for name in self.urdf_joint_names:
motor = self._motor_by_joint[name]
direction = direction_by_motor[motor]
curves = (
@@ -214,20 +243,22 @@ def load_calibrated_command_mapper(
return CalibratedCommandMapper(payload, expected_side=expected_side)
def default_input_topic(hand_type: str, input_domain: str) -> str:
def default_input_topic(
hand_type: str, input_domain: str, model: str = "G20"
) -> str:
side = str(hand_type).lower()
if side not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
if input_domain == "feedback_u8":
return f"/g20/cb_{side}_hand_state"
return f"/{str(model).lower()}/cb_{side}_hand_state"
if input_domain == "command_u8":
return f"/g20/cb_{side}_hand_control_cmd"
return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
raise ValueError("calibration curve_input_domain is invalid")
class CalibratedJointStateBridge(Node):
def __init__(self) -> None:
super().__init__("g20_calibrated_joint_state_bridge")
super().__init__("calibrated_joint_state_bridge")
self.declare_parameter("hand_type", "right")
self.declare_parameter("calibration_file", "")
self.declare_parameter("input_topic", "")
@@ -245,10 +276,11 @@ class CalibratedJointStateBridge(Node):
input_topic = str(self.get_parameter("input_topic").value).strip()
output_topic = str(self.get_parameter("output_topic").value).strip()
self.input_topic = input_topic or default_input_topic(
hand_type, self.mapper.input_domain
hand_type, self.mapper.input_domain, self.mapper.model
)
self.output_topic = (
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
output_topic
or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state"
)
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
self.subscription = self.create_subscription(
@@ -256,7 +288,8 @@ class CalibratedJointStateBridge(Node):
)
self._last_error = ""
self.get_logger().info(
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
f"loaded {self.mapper.profile_id} calibration for "
f"{self.mapper.serial_number}: "
f"{self.input_topic} ({self.mapper.input_domain}) -> "
f"{self.output_topic}"
)
@@ -273,7 +306,7 @@ class CalibratedJointStateBridge(Node):
self._last_error = ""
result = JointState()
result.header = command.header
result.name = list(G20_URDF_JOINT_NAMES)
result.name = list(self.mapper.urdf_joint_names)
result.position = list(positions)
self.publisher.publish(result)
@@ -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,52 @@
"""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_package_uri(value: str, workspace: Path) -> Path | None:
prefix = "package://"
if not value.startswith(prefix):
return None
package_name, separator, relative = value[len(prefix) :].partition("/")
if not separator or not package_name or not relative:
raise ValueError(f"invalid ROS package resource path: {value}")
workspace_candidate = (workspace / "src" / package_name / relative).resolve()
if workspace_candidate.exists():
return workspace_candidate
try:
from ament_index_python.packages import get_package_share_directory
package_share = Path(get_package_share_directory(package_name))
except Exception:
return workspace_candidate
return (package_share / relative).resolve()
def resolve_renamed_package_path(value: str | Path, workspace: Path) -> Path:
"""Resolve workspace paths, ROS package resources and the former prefix.
Deployed v1 product YAML files are kept byte-for-byte stable because the
artifact paths participate in operational review. Existing paths always
win; package URIs prefer a source-workspace copy, and the rename mapping
is used only when the literal legacy path no longer exists.
"""
text = str(value).strip()
package_resource = _resolve_package_uri(text, workspace)
if package_resource is not None:
return package_resource
raw = Path(text).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,390 @@
"""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()
# Calibration names are allowed to stay model-neutral while the source
# URDF keeps any vendor/side prefixes (for example ``rh_``).
urdf_joint_by_joint: Mapping[str, str] = field(default_factory=dict)
# Older SDKs occasionally published a wrong label for a physically stable
# channel. Aliases are accepted only at the declared channel index.
feedback_name_aliases: Mapping[str, str] = field(default_factory=dict)
# SDK speed commands are not necessarily one value per position channel.
# This mapping makes that protocol detail explicit in a profile.
speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict)
@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
start_u8: int = 255
end_u8: int = 0
preflight_speed_u8: int | None = None
formal_speed_u8: int | None = None
@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]
# ``upper_at_end`` means TaskSpec.end_u8 is the trusted source-URDF upper
# physical endpoint. The measured travel then defines the electrical
# zero and corrected [0, travel] coordinate range.
endpoint_anchor_by_joint: Mapping[str, str] = field(default_factory=dict)
fitted_mimic_joints: frozenset[str] = frozenset()
# Passive coupling is not necessarily representable by the linear URDF
# ``mimic`` element. Profiles must opt in explicitly before a nonlinear
# runtime/MuJoCo relation may be published.
coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict)
@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
# Per-URDF-joint provenance used by partial calibration artifacts.
# Known values are: measured_static_dynamic, measured_dynamic_cad_static,
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
# and mimic_nominal.
joint_coverage: Mapping[str, str] = field(default_factory=dict)
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")
if command.urdf_joint_by_joint:
if not set(command.command_index_by_joint).issubset(
command.urdf_joint_by_joint
):
errors.append("every commanded joint must map to a URDF joint")
urdf_names = tuple(command.urdf_joint_by_joint.values())
if len(set(urdf_names)) != len(urdf_names):
errors.append("URDF joint mappings must be unique")
if any(
index not in indices or slot < 0
for index, slot in command.speed_slot_by_command_index.items()
):
errors.append("speed-slot mapping is invalid")
if any(
not str(alias).strip() or canonical not in command.names
for alias, canonical in command.feedback_name_aliases.items()
):
errors.append("feedback name alias is not part of the command schema")
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")
if not 0 <= task.start_u8 <= 255 or not 0 <= task.end_u8 <= 255:
errors.append(f"task {task.key} sweep endpoint is out of range")
if task.start_u8 == task.end_u8:
errors.append(f"task {task.key} sweep endpoints must differ")
for speed in (task.preflight_speed_u8, task.formal_speed_u8):
if speed is not None and not 0 <= speed <= 255:
errors.append(f"task {task.key} speed 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")
if not set(zero.endpoint_anchor_by_joint).issubset(zero.active_joints):
errors.append("endpoint anchors must target active joints")
if not set(zero.endpoint_anchor_by_joint.values()).issubset(
{
"upper_at_end",
"lower_at_start",
"zero_at_start",
"cad_range_center",
}
):
errors.append("endpoint anchor policy is unsupported")
if not zero.fitted_mimic_joints.issubset(zero.passive_joints):
errors.append("fitted mimic targets must be passive joints")
if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint):
errors.append("fitted mimic target has no source mapping")
if not set(zero.coupling_model_by_joint).issubset(
zero.mimic_source_by_joint
):
errors.append("coupling model target has no source mapping")
if not set(zero.coupling_model_by_joint.values()).issubset(
{"linear_mimic", "quadratic_runtime"}
):
errors.append("coupling model policy is unsupported")
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 profile.joint_coverage:
valid_coverage = {
"measured_static_dynamic",
"measured_dynamic_cad_static",
"transferred_static_dynamic",
"transferred_dynamic_cad_static",
"cad_nominal",
"mimic_nominal",
}
if set(profile.joint_coverage) != all_joints:
errors.append("joint coverage must describe every profile joint")
if not set(profile.joint_coverage.values()).issubset(valid_coverage):
errors.append("joint coverage contains an unsupported status")
if errors:
raise ProfileValidationError("; ".join(errors))
@@ -0,0 +1,27 @@
"""Normalized records shared by online evaluation and offline replay."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Mapping
@dataclass(frozen=True)
class SampleRecord:
task_key: str
measurement: str
view: str
cycle: int
direction: str
command_u8: int
timestamp_ns: int
values: Mapping[str, Any]
quality: Mapping[str, float] = field(default_factory=dict)
def __post_init__(self) -> None:
if not self.task_key or not self.measurement or not self.view:
raise ValueError("sample task, measurement, and view are required")
if self.cycle < 0 or not 0 <= self.command_u8 <= 255:
raise ValueError("sample cycle or command is out of range")
if self.timestamp_ns < 0:
raise ValueError("sample timestamp must be non-negative")
@@ -0,0 +1,19 @@
"""Shared task direction vocabulary."""
DIRECTION_DECREASING = "decreasing"
DIRECTION_INCREASING = "increasing"
DIRECTIONS: tuple[str, ...] = (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
)
PHASE_ROOT = "root"
PHASE_TIP = "tip"
__all__ = [
"DIRECTION_DECREASING",
"DIRECTION_INCREASING",
"DIRECTIONS",
"PHASE_ROOT",
"PHASE_TIP",
]
@@ -0,0 +1,5 @@
"""Pure curve and axis fitting."""
from .curve import FitResult, isotonic_nonincreasing
__all__ = ["FitResult", "isotonic_nonincreasing"]
@@ -0,0 +1,70 @@
"""Model-independent curve fitting result and monotonic projection."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Sequence
import numpy as np
from ..geometry import delta_rotation_vector
def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
"""Unweighted PAVA projection onto non-increasing values."""
original = np.asarray(values, dtype=float)
if original.ndim != 1 or not np.all(np.isfinite(original)):
raise ValueError("values must be a finite vector")
negated = -original
levels: list[float] = []
weights: list[int] = []
starts: list[int] = []
for index, value in enumerate(negated):
levels.append(float(value))
weights.append(1)
starts.append(index)
while len(levels) >= 2 and levels[-2] > levels[-1]:
total_weight = weights[-2] + weights[-1]
merged = (
levels[-2] * weights[-2] + levels[-1] * weights[-1]
) / total_weight
levels[-2:] = [merged]
weights[-2:] = [total_weight]
starts.pop()
projected = np.empty_like(original)
for block_index, (level, start) in enumerate(zip(levels, starts)):
end = (
starts[block_index + 1]
if block_index + 1 < len(starts)
else len(original)
)
projected[start:end] = -level
return projected
@dataclass(frozen=True)
class FitResult:
joints: dict[str, dict[str, Any]]
axes: dict[str, tuple[float, float, float]]
references: dict[str, tuple[float, float, float, float]]
ip_coupling: dict[str, float]
max_monotonic_correction_rad: float
max_hysteresis_rad: float
measurement_mode: str = "rotation"
trajectory_models: dict[str, Any] = field(default_factory=dict)
trajectory_quality: dict[str, Any] = field(default_factory=dict)
def measure_from_reference(
self,
joint_name: str,
observed_quaternion_xyzw: Sequence[float],
reference_quaternion_xyzw: Sequence[float] | None = None,
) -> float:
reference = (
reference_quaternion_xyzw
if reference_quaternion_xyzw is not None
else self.references[joint_name]
)
vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
axis = np.asarray(self.axes[joint_name], dtype=float)
return float(vector @ axis)
@@ -0,0 +1,41 @@
"""Pure geometry used by online and offline calibration."""
from .extrinsics import (
CameraCalibrationIdentity,
CameraExtrinsics,
camera_info_fingerprint,
load_camera_extrinsics,
matrix_payload,
transform_matrix,
validate_camera_extrinsics_payload,
)
from .rotation import (
delta_rotation_vector,
fit_rotation_axis,
image_plane_tag_quaternion_xyzw,
normalize_quaternion_xyzw,
relative_quaternion_xyzw,
robust_rotation_summary,
rotation_inlier_fraction,
rotation_rms_rad,
rotation_spread_rad,
)
__all__ = [
"CameraCalibrationIdentity",
"CameraExtrinsics",
"camera_info_fingerprint",
"delta_rotation_vector",
"fit_rotation_axis",
"image_plane_tag_quaternion_xyzw",
"load_camera_extrinsics",
"matrix_payload",
"normalize_quaternion_xyzw",
"relative_quaternion_xyzw",
"robust_rotation_summary",
"rotation_inlier_fraction",
"rotation_rms_rad",
"rotation_spread_rad",
"transform_matrix",
"validate_camera_extrinsics_payload",
]
@@ -13,9 +13,6 @@ import yaml
from scipy.spatial.transform import Rotation
VIEWS: tuple[str, ...] = ("front", "side", "top")
def camera_info_fingerprint(
*,
width: int,
@@ -81,66 +78,61 @@ class CameraCalibrationIdentity:
@dataclass(frozen=True)
class ThreeCameraExtrinsics:
"""Transforms points from each camera optical frame into front optical."""
class CameraExtrinsics:
"""Transforms every declared camera into one Profile-selected reference."""
cameras: Mapping[str, CameraCalibrationIdentity]
front_from_view: Mapping[str, np.ndarray]
reference_view: str
reference_from_view: Mapping[str, np.ndarray]
quality: Mapping[str, float]
def transform(self, view: str) -> np.ndarray:
if view not in self.front_from_view:
if view not in self.reference_from_view:
raise KeyError(f"extrinsics do not contain view {view}")
return np.asarray(self.front_from_view[view], dtype=float).copy()
def camera_matches(
self,
view: str,
*,
serial_number: str,
width: int,
height: int,
intrinsics_sha256: str,
) -> bool:
expected = self.cameras.get(view)
return bool(
expected is not None
and expected.serial_number == str(serial_number)
and expected.width == int(width)
and expected.height == int(height)
and expected.intrinsics_sha256 == str(intrinsics_sha256)
)
return np.asarray(self.reference_from_view[view], dtype=float).copy()
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
def validate_camera_extrinsics_payload(
payload: Mapping[str, Any],
*,
required_views: Sequence[str],
reference_view: str,
quality_limits: Mapping[str, float] | None = None,
minimum_capture_counts: Mapping[str, int] | None = None,
) -> None:
if int(payload.get("schema_version", -1)) != 1:
raise ValueError("camera extrinsics schema_version must be 1")
if payload.get("reference_view") != "front":
raise ValueError("camera extrinsics reference_view must be front")
reference = str(reference_view)
if payload.get("reference_view") != reference:
raise ValueError(
"camera extrinsics reference_view differs from the Profile"
)
cameras = payload.get("cameras")
transforms = payload.get("front_from_view")
transforms = payload.get(f"{reference}_from_view")
quality = payload.get("quality")
if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
raise ValueError("camera extrinsics must contain front/side/top cameras")
if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
raise ValueError("camera extrinsics must contain all three transforms")
views = tuple(str(view) for view in required_views)
if not views or len(set(views)) != len(views):
raise ValueError("required extrinsic views must be non-empty and unique")
if reference not in views:
raise ValueError("extrinsic reference view is not required")
if not isinstance(cameras, Mapping) or set(cameras) != set(views):
raise ValueError("camera extrinsics differ from the Profile views")
if not isinstance(transforms, Mapping) or set(transforms) != set(views):
raise ValueError("camera transforms differ from the Profile views")
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
raise ValueError("camera extrinsics quality is not passed")
quality_limits = {
"reprojection_rms_px": 1.2,
"maximum_rotation_repeatability_deg": 0.3,
"maximum_translation_repeatability_m": 0.0015,
}
for key, limit in quality_limits.items():
for key, limit in dict(quality_limits or {}).items():
value = float(quality.get(key, float("inf")))
if not np.isfinite(value) or value > limit:
raise ValueError(
f"camera extrinsics {key}={value} exceeds {limit}"
)
for key in ("front_side_captures", "front_top_captures"):
if int(quality.get(key, 0)) < 15:
raise ValueError(f"camera extrinsics {key} must be at least 15")
for view in VIEWS:
for key, minimum in dict(minimum_capture_counts or {}).items():
if int(quality.get(key, 0)) < int(minimum):
raise ValueError(
f"camera extrinsics {key} must be at least {minimum}"
)
for view in views:
identity = cameras[view]
if not isinstance(identity, Mapping):
raise ValueError(f"{view} camera identity must be an object")
@@ -158,14 +150,23 @@ def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
transform.get("translation_xyz_m", ()),
transform.get("quaternion_xyzw", ()),
)
if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
raise ValueError("front_from_view.front must be identity")
serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
if len(set(serials)) != len(VIEWS):
if view == reference and not np.allclose(
matrix, np.eye(4), atol=1.0e-9
):
raise ValueError("reference-view transform must be identity")
serials = [str(cameras[view]["serial_number"]) for view in views]
if len(set(serials)) != len(views):
raise ValueError("camera extrinsics serial numbers must be unique")
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
def load_camera_extrinsics(
path: str | Path,
*,
required_views: Sequence[str],
reference_view: str,
quality_limits: Mapping[str, float] | None = None,
minimum_capture_counts: Mapping[str, int] | None = None,
) -> CameraExtrinsics:
source = Path(path).expanduser().resolve()
if not source.is_file():
raise ValueError(f"camera extrinsics file does not exist: {source}")
@@ -173,7 +174,15 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
payload = yaml.safe_load(stream)
if not isinstance(payload, Mapping):
raise ValueError("camera extrinsics file must contain an object")
validate_extrinsics_payload(payload)
validate_camera_extrinsics_payload(
payload,
required_views=required_views,
reference_view=reference_view,
quality_limits=quality_limits,
minimum_capture_counts=minimum_capture_counts,
)
views = tuple(str(view) for view in required_views)
transform_key = f"{reference_view}_from_view"
cameras = {
view: CameraCalibrationIdentity(
serial_number=str(payload["cameras"][view]["serial_number"]),
@@ -183,45 +192,21 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
payload["cameras"][view]["intrinsics_sha256"]
),
)
for view in VIEWS
for view in views
}
transforms = {
view: transform_matrix(
payload["front_from_view"][view]["translation_xyz_m"],
payload["front_from_view"][view]["quaternion_xyzw"],
payload[transform_key][view]["translation_xyz_m"],
payload[transform_key][view]["quaternion_xyzw"],
)
for view in VIEWS
for view in views
}
return ThreeCameraExtrinsics(
return CameraExtrinsics(
cameras=cameras,
front_from_view=transforms,
reference_view=str(reference_view),
reference_from_view=transforms,
quality={
str(key): float(value) if isinstance(value, (int, float)) else value
for key, value in payload["quality"].items()
},
)
def dump_three_camera_extrinsics(
path: str | Path,
*,
cameras: Mapping[str, Mapping[str, Any]],
front_from_view: Mapping[str, Sequence[Sequence[float]]],
quality: Mapping[str, Any],
) -> None:
payload = {
"schema_version": 1,
"reference_view": "front",
"cameras": {view: dict(cameras[view]) for view in VIEWS},
"front_from_view": {
view: matrix_payload(front_from_view[view]) for view in VIEWS
},
"quality": dict(quality),
}
validate_extrinsics_payload(payload)
destination = Path(path).expanduser().resolve()
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_suffix(destination.suffix + ".tmp")
with temporary.open("w", encoding="utf-8") as stream:
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
temporary.replace(destination)
@@ -0,0 +1,157 @@
"""Pure quaternion summaries and rotation-axis fitting."""
from __future__ import annotations
import math
from typing import Sequence
import numpy as np
from scipy.spatial.transform import Rotation
def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
quaternion = np.asarray(values, dtype=float)
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
raise ValueError("quaternion must contain four finite xyzw values")
norm = float(np.linalg.norm(quaternion))
if norm < 1e-12:
raise ValueError("quaternion norm is zero")
return quaternion / norm
def relative_quaternion_xyzw(
parent_camera_quaternion: Sequence[float],
child_camera_quaternion: Sequence[float],
) -> tuple[float, float, float, float]:
"""Compute parent-to-child orientation from two camera-to-Tag rotations."""
parent = Rotation.from_quat(
normalize_quaternion_xyzw(parent_camera_quaternion)
)
child = Rotation.from_quat(
normalize_quaternion_xyzw(child_camera_quaternion)
)
quaternion = (parent.inv() * child).as_quat()
return tuple(float(value) for value in quaternion)
def image_plane_tag_quaternion_xyzw(
corners_xy: Sequence[Sequence[float]],
) -> tuple[float, float, float, float]:
"""Estimate Tag orientation about the optical axis from ordered corners."""
corners = np.asarray(corners_xy, dtype=float)
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
raise ValueError("corners_xy must contain four finite xy points")
x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
if float(np.linalg.norm(x_axis)) < 1e-9:
raise ValueError("tag x-axis is degenerate")
angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
return tuple(float(value) for value in quaternion)
def robust_rotation_summary(
quaternions_xyzw: Sequence[Sequence[float]],
) -> tuple[tuple[float, float, float, float], float]:
"""Return a robust orientation and maximum angular residual in radians."""
if not quaternions_xyzw:
raise ValueError("at least one quaternion is required")
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
reference = rotations[0]
delta_vectors = (reference.inv() * rotations).as_rotvec()
median_delta = np.median(delta_vectors, axis=0)
robust = reference * Rotation.from_rotvec(median_delta)
residuals = (robust.inv() * rotations).magnitude()
maximum = float(np.max(residuals)) if residuals.size else 0.0
return tuple(float(value) for value in robust.as_quat()), maximum
def rotation_spread_rad(
quaternions_xyzw: Sequence[Sequence[float]],
) -> float:
"""Return the maximum geodesic residual around a robust orientation."""
_, spread = robust_rotation_summary(quaternions_xyzw)
return spread
def rotation_rms_rad(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float | None = None,
) -> float:
"""Return RMS geodesic noise around a robust orientation."""
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
if outlier_threshold_rad is not None:
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
residuals = residuals[residuals <= threshold]
if residuals.size == 0:
return float("inf")
return float(np.sqrt(np.mean(np.square(residuals))))
def rotation_inlier_fraction(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float,
) -> float:
"""Return the fraction close to the robust orientation."""
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
return float(np.mean(residuals <= threshold))
def delta_rotation_vector(
reference_xyzw: Sequence[float],
observed_xyzw: Sequence[float],
) -> np.ndarray:
reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
return (reference.inv() * observed).as_rotvec()
def fit_rotation_axis(
vectors: Sequence[Sequence[float]],
commands: Sequence[int],
) -> np.ndarray:
"""Fit and orient the single rotational axis used by one command sweep."""
matrix = np.asarray(vectors, dtype=float)
command_values = np.asarray(commands, dtype=int)
if matrix.ndim != 2 or matrix.shape[1] != 3:
raise ValueError("vectors must have shape (N, 3)")
if command_values.shape != (matrix.shape[0],):
raise ValueError("commands must match vectors")
useful = np.linalg.norm(matrix, axis=1) > 1e-6
if int(np.count_nonzero(useful)) < 3:
raise ValueError("insufficient non-zero rotations to fit an axis")
_, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
axis = vh[0]
projections = matrix @ axis
low = projections[command_values <= 16]
high = projections[command_values >= 239]
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
axis = -axis
return axis / np.linalg.norm(axis)
@@ -0,0 +1,15 @@
"""Task acceptance and final-session solver contracts."""
from .interfaces import (
SessionSolution,
SessionSolver,
TaskEvaluation,
TaskEvaluator,
)
__all__ = [
"SessionSolution",
"SessionSolver",
"TaskEvaluation",
"TaskEvaluator",
]
@@ -0,0 +1,42 @@
"""Shared evaluator and final-solver interfaces."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Mapping, Protocol, Sequence
from ..domain import CalibrationProfile, SampleRecord, TaskSpec
@dataclass(frozen=True)
class TaskEvaluation:
accepted: bool
failures: tuple[Mapping[str, Any], ...] = ()
rescan_measurements: frozenset[str] = frozenset()
rescan_cycles: frozenset[int] = frozenset()
@dataclass(frozen=True)
class SessionSolution:
passed: bool
calibration: Mapping[str, Any]
zero_offsets_rad: Mapping[str, float]
failures: tuple[Mapping[str, Any], ...] = ()
diagnostics: Mapping[str, Any] = field(default_factory=dict)
class TaskEvaluator(Protocol):
def evaluate_task(
self,
profile: CalibrationProfile,
task: TaskSpec,
samples: Sequence[SampleRecord],
) -> TaskEvaluation: ...
class SessionSolver(Protocol):
def solve_session(
self,
profile: CalibrationProfile,
samples: Sequence[SampleRecord],
) -> SessionSolution: ...
@@ -0,0 +1,5 @@
"""URDF correction authorization and validation types."""
from .plan import UrdfCorrectionPlan, build_correction_plan
__all__ = ["UrdfCorrectionPlan", "build_correction_plan"]
@@ -0,0 +1,104 @@
"""One authorization plan shared by URDF writers and validators."""
from __future__ import annotations
from dataclasses import dataclass, field
import hashlib
from pathlib import Path
from typing import Mapping
from ..domain import CalibrationProfile
@dataclass(frozen=True)
class UrdfCorrectionPlan:
source_sha256: str
allowed_active_joints: frozenset[str]
endpoint_limit_joints: frozenset[str]
mimic_source_by_joint: Mapping[str, str]
frozen_joints: frozenset[str]
frozen_offsets_rad: Mapping[str, float] = field(default_factory=dict)
forbid_calibrated_source: bool = True
forbid_overwrite: bool = True
preserve_passive_joints: bool = True
def __post_init__(self) -> None:
if len(self.source_sha256) != 64 or any(
character not in "0123456789abcdef"
for character in self.source_sha256.lower()
):
raise ValueError("source URDF SHA-256 is invalid")
if self.allowed_active_joints & self.frozen_joints:
raise ValueError("allowed and frozen URDF joints overlap")
applied = self.allowed_active_joints | set(self.frozen_offsets_rad)
if not set(self.frozen_offsets_rad).issubset(self.frozen_joints):
raise ValueError("frozen offsets must belong to frozen joints")
if not self.endpoint_limit_joints.issubset(applied):
raise ValueError("endpoint limit joint is not an applied active joint")
if set(self.mimic_source_by_joint) & self.allowed_active_joints:
raise ValueError("dependent mimic joints cannot be active edit targets")
def authorize_offsets(self, offsets_rad: Mapping[str, float]) -> None:
required = self.allowed_active_joints | set(self.frozen_offsets_rad)
unexpected = set(offsets_rad) - required
if unexpected:
raise ValueError(
"URDF correction contains unauthorized joints: "
+ ", ".join(sorted(unexpected))
)
missing = required - set(offsets_rad)
if missing:
raise ValueError(
"URDF correction is missing active joints: "
+ ", ".join(sorted(missing))
)
changed_frozen = {
name
for name, expected in self.frozen_offsets_rad.items()
if abs(float(offsets_rad[name]) - float(expected)) > 1.0e-12
}
if changed_frozen:
raise ValueError(
"URDF correction changed frozen offsets: "
+ ", ".join(sorted(changed_frozen))
)
def verify_source(self, source_urdf: str | Path) -> None:
digest = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest()
if digest != self.source_sha256.lower():
raise ValueError("source URDF SHA-256 differs from correction plan")
def build_correction_plan(
profile: CalibrationProfile,
*,
source_sha256: str,
scope: str,
frozen_offsets_rad: Mapping[str, float] | None = None,
) -> UrdfCorrectionPlan:
"""Build one scope-aware edit authorization from typed policies."""
selected = profile.scope.selected_joints(scope)
frozen = profile.scope.frozen_joints[str(scope)]
expected_frozen = {
str(name): float(value)
for name, value in dict(frozen_offsets_rad or {}).items()
}
if set(expected_frozen) != set(frozen):
missing = set(frozen) - set(expected_frozen)
extra = set(expected_frozen) - set(frozen)
raise ValueError(
"frozen URDF offset state differs from scope policy: "
f"missing={','.join(sorted(missing)) or '-'};"
f"extra={','.join(sorted(extra)) or '-'}"
)
applied = selected | frozen
return UrdfCorrectionPlan(
source_sha256=source_sha256,
allowed_active_joints=selected,
endpoint_limit_joints=(
profile.zero.mechanical_endpoint_joints & applied
),
mimic_source_by_joint=profile.zero.mimic_source_by_joint,
frozen_joints=frozen | profile.zero.cad_frozen_joints,
frozen_offsets_rad=expected_frozen,
)
@@ -0,0 +1,130 @@
"""Three-view compatibility policy over generic camera extrinsics."""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Any, Mapping, Sequence
import numpy as np
import yaml
from .core.geometry.extrinsics import (
CameraCalibrationIdentity,
CameraExtrinsics,
camera_info_fingerprint,
load_camera_extrinsics,
matrix_payload,
transform_matrix,
validate_camera_extrinsics_payload,
)
VIEWS: tuple[str, ...] = ("front", "side", "top")
_QUALITY_LIMITS = {
"reprojection_rms_px": 1.2,
"maximum_rotation_repeatability_deg": 0.3,
"maximum_translation_repeatability_m": 0.0015,
}
_MINIMUM_CAPTURE_COUNTS = {
"front_side_captures": 15,
"front_top_captures": 15,
}
@dataclass(frozen=True)
class ThreeCameraExtrinsics:
"""Transforms points from each camera optical frame into front optical."""
cameras: Mapping[str, CameraCalibrationIdentity]
front_from_view: Mapping[str, np.ndarray]
quality: Mapping[str, float]
def transform(self, view: str) -> np.ndarray:
if view not in self.front_from_view:
raise KeyError(f"extrinsics do not contain view {view}")
return np.asarray(self.front_from_view[view], dtype=float).copy()
def camera_matches(
self,
view: str,
*,
serial_number: str,
width: int,
height: int,
intrinsics_sha256: str,
) -> bool:
expected = self.cameras.get(view)
return bool(
expected is not None
and expected.serial_number == str(serial_number)
and expected.width == int(width)
and expected.height == int(height)
and expected.intrinsics_sha256 == str(intrinsics_sha256)
)
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
validate_camera_extrinsics_payload(
payload,
required_views=VIEWS,
reference_view="front",
quality_limits=_QUALITY_LIMITS,
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
)
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
generic = load_camera_extrinsics(
path,
required_views=VIEWS,
reference_view="front",
quality_limits=_QUALITY_LIMITS,
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
)
return ThreeCameraExtrinsics(
cameras=generic.cameras,
front_from_view=generic.reference_from_view,
quality=generic.quality,
)
def dump_three_camera_extrinsics(
path: str | Path,
*,
cameras: Mapping[str, Mapping[str, Any]],
front_from_view: Mapping[str, Sequence[Sequence[float]]],
quality: Mapping[str, Any],
) -> None:
payload = {
"schema_version": 1,
"reference_view": "front",
"cameras": {view: dict(cameras[view]) for view in VIEWS},
"front_from_view": {
view: matrix_payload(front_from_view[view]) for view in VIEWS
},
"quality": dict(quality),
}
validate_extrinsics_payload(payload)
destination = Path(path).expanduser().resolve()
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_suffix(destination.suffix + ".tmp")
with temporary.open("w", encoding="utf-8") as stream:
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
temporary.replace(destination)
__all__ = [
"CameraCalibrationIdentity",
"CameraExtrinsics",
"ThreeCameraExtrinsics",
"VIEWS",
"camera_info_fingerprint",
"dump_three_camera_extrinsics",
"load_camera_extrinsics",
"load_three_camera_extrinsics",
"matrix_payload",
"transform_matrix",
"validate_camera_extrinsics_payload",
"validate_extrinsics_payload",
]
@@ -0,0 +1,7 @@
"""One-release module alias for the relocated model profile implementation."""
import sys
from .models.g20 import profile as _implementation
sys.modules[__name__] = _implementation
@@ -203,7 +203,7 @@ def configure_fastdds_large_image_transport() -> Path:
from ament_index_python.packages import get_package_share_directory
profile = (
Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
Path(get_package_share_directory("linkerhand_calibration"))
/ "config"
/ "fastdds_large_images.xml"
)
@@ -0,0 +1,15 @@
"""Model- and side-specific calibration policies."""
from .registry import (
EngineBindings,
ProfileRegistry,
RegisteredProfile,
get_default_registry,
)
__all__ = [
"EngineBindings",
"ProfileRegistry",
"RegisteredProfile",
"get_default_registry",
]
@@ -0,0 +1,15 @@
"""Registered profiles for this hand family."""
from ..registry import ProfileRegistry
def register_profiles(registry: ProfileRegistry) -> None:
from .legacy_11 import build_left_profile, build_right_profile
from .right_19 import build_profile
registry.register(build_profile())
registry.register(build_left_profile())
registry.register(build_right_profile())
__all__ = ["register_profiles"]
@@ -0,0 +1,219 @@
"""Adapt reviewed family profiles to the shared typed contract."""
from __future__ import annotations
from ...core import (
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
)
from .profile import MIMIC_DERIVED_FINGER_DIPS
from ..registry import EngineBindings, RegisteredProfile
from .artifacts import build_artifact_policy
from .motion import (
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
)
_HARD_THRESHOLD_KEYS = frozenset(
{
"minimum_detection_rate",
"maximum_reprojection_error_px",
"maximum_axis_cycle_difference_rad",
"maximum_pose_line_rms_m",
"maximum_hysteresis_rad",
"maximum_validation_error_rad",
}
)
def _run_cli(args: list[str] | None = None) -> None:
from .runner import main
main(args)
def _run_node(args: list[str] | None = None) -> None:
from .node import main
main(args)
def adapt_profile(
*,
key: ProfileKey,
namespace: str,
hand_profile,
zero_profile,
mechanical_endpoint_joints: frozenset[str] = frozenset(),
post_solve_endpoint_joints: frozenset[str] = frozenset(),
) -> RegisteredProfile:
fixed_by_view = {
view: frozenset(roles)
for view, roles in hand_profile.preflight_view_roles.items()
}
views = tuple(
ViewSpec(
name=view,
tags=tuple(
TagSpec(
role=role,
tag_id=int(tag_id),
fixed_reference=role in fixed_by_view.get(view, frozenset()),
)
for role, tag_id in roles.items()
),
)
for view, roles in hand_profile.view_tags.items()
)
record_specs = hand_profile.record_specs
command_index_by_joint = {
name: int(spec.motor_index) for name, spec in record_specs.items()
}
command_index_by_joint.update(
{
name: int(spec.motor_index)
for name, spec in hand_profile.joint_specs.items()
}
)
measurements = {
name: MeasurementSpec(
joint=name,
kind=str(spec.zero_kind or "curve"),
view=spec.view,
parent_role=spec.parent_role,
child_role=spec.child_role,
validation_source=(hand_profile.axis_validation_sources or {}).get(
name
),
pose_axis_line_required=bool(
spec.pose_axis_line_required
),
)
for name, spec in record_specs.items()
}
tasks = tuple(
TaskSpec(
key=spec.key,
view=spec.view,
command_index=int(spec.motor_index),
joints=tuple(spec.joints),
auxiliary_commands=tuple(spec.auxiliary_commands),
validation_only=bool(spec.validation_only),
)
for spec in hand_profile.sweep_specs
)
active = frozenset(hand_profile.active_joints)
passive = frozenset(hand_profile.passive_joints)
thumb = frozenset(name for name in active if name.startswith("thumb_"))
fingers = active - thumb
typed = CalibrationProfile(
key=key,
namespace=namespace,
command=CommandLayout(
names=tuple(hand_profile.command_names),
baseline_u8=tuple(int(value) for value in hand_profile.baseline_command),
command_index_by_joint=command_index_by_joint,
disabled_indices=frozenset(
index
for index, name in enumerate(hand_profile.command_names)
if name.startswith("reserved_")
),
),
vision=VisionRigSpec(
views=views,
common_frame="calibration_common",
extrinsic_reference_view=views[0].name,
extrinsics_quality_limits={
"reprojection_rms_px": 1.2,
"maximum_rotation_repeatability_deg": 0.3,
"maximum_translation_repeatability_m": 0.0015,
},
minimum_capture_counts={
"front_side_captures": 15,
"front_top_captures": 15,
},
),
motion=MotionPolicy(
tasks=tasks,
precheck_sweeps=bool(hand_profile.precheck_sweeps),
steady_command_checkpoints=bool(
hand_profile.steady_command_checkpoints
),
),
measurement=MeasurementPolicy(
measurements=measurements,
cross_view_sources=dict(
hand_profile.axis_validation_sources or {}
),
image_curve_joints=frozenset(
hand_profile.image_trajectory_joints
),
directional_zero=bool(hand_profile.directional_zero),
cross_view_roll_curve=bool(hand_profile.cross_view_roll_curve),
stable_cross_view_cone_bias=bool(
hand_profile.stable_cross_view_cone_bias
),
),
zero=ZeroSolvePolicy(
active_joints=active,
passive_joints=passive,
direct_zero_joints=tuple(zero_profile.direct_zero_joints),
axis_joints=tuple(zero_profile.axis_joints),
mechanical_endpoint_joints=mechanical_endpoint_joints,
post_solve_endpoint_joints=post_solve_endpoint_joints,
mimic_source_by_joint={
target: source
for target, source in MIMIC_DERIVED_FINGER_DIPS.items()
if target in passive and source in active
},
cad_frozen_joints=frozenset(
passive - set(zero_profile.static_output_zero_offsets_rad)
),
),
quality=QualityPolicy(
training_cycles=(0, 1, 2),
holdout_cycle=3 if key.layout != "legacy_11" else None,
hard_threshold_keys=_HARD_THRESHOLD_KEYS,
isolated_holdout=bool(hand_profile.isolated_holdout),
),
scope=ScopePolicy(
calibrate_joints={
"full": active,
"thumb": thumb,
"fingers": fingers,
},
frozen_joints={
"full": frozenset(),
"thumb": fingers,
"fingers": thumb,
},
),
artifacts=build_artifact_policy(
frozenset(hand_profile.capabilities)
),
)
return RegisteredProfile(
profile=typed,
engine=EngineBindings(
hand_profile=hand_profile,
zero_profile=zero_profile,
motion_command=build_calibration_motion_command,
preparation_waypoints=build_calibration_preparation_waypoints,
return_waypoints=build_calibration_return_waypoints,
cli_main=_run_cli,
node_main=_run_node,
),
)
@@ -0,0 +1,30 @@
"""Runtime JSON and corrected-URDF naming policy."""
from ...core import ArtifactPolicy
def build_artifact_policy(
compatibility_tokens: frozenset[str],
) -> ArtifactPolicy:
return ArtifactPolicy(
output_schema_version=4,
calibration_filename="g20_{side}_{serial_number}_calibration.json",
corrected_urdf_filename=(
"linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf"
),
protected_input_fields=frozenset(
{
"source_urdf_sha256",
"camera_extrinsics_sha256",
"calibration_config_sha256",
"tag_config_sha256",
}
),
session_compatibility_tokens=frozenset(compatibility_tokens),
publish_corrected_urdf=(
"urdf_zero_publication" in compatibility_tokens
),
)
__all__ = ["build_artifact_policy"]
@@ -0,0 +1,26 @@
"""Reviewed command-channel layouts for this hand family."""
G20_COMMAND_NAMES: tuple[str, ...] = (
"thumb_cmc_pitch",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
"thumb_cmc_roll",
"index_mcp_roll",
"middle_mcp_roll",
"ring_mcp_roll",
"pinky_mcp_roll",
"thumb_cmc_yaw",
"reserved_11",
"reserved_12",
"reserved_13",
"reserved_14",
"thumb_mcp",
"index_pip",
"middle_pip",
"ring_pip",
"pinky_pip",
)
__all__ = ["G20_COMMAND_NAMES"]
@@ -0,0 +1,144 @@
"""Read-only regression checks for the reviewed hardware sessions."""
from __future__ import annotations
import argparse
import hashlib
import json
from pathlib import Path
from typing import Any
_PASS_SESSIONS = {
"20260830_181154": "full",
"20260831_141123": "thumb",
"20260831_163843": "thumb",
}
_FAIL_SESSIONS = {
"20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"),
"20260831_142322": ("VAL-QUALITY-501", "zero_model_validation_failed"),
}
def _sha256_file(path: Path) -> str:
digest = hashlib.sha256()
with path.open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _summary(session: Path) -> dict[str, Any]:
value = json.loads(
(session / "calibration_summary_zh.json").read_text(encoding="utf-8")
)
if not isinstance(value, dict):
raise ValueError(f"invalid golden summary: {session}")
return value
def validate_golden_sessions(
serial_root: str | Path,
*,
replay_full_session: bool = True,
) -> dict[str, Any]:
"""Prove PASS/FAIL, hashes, partial freezes, and failure non-publication."""
root = Path(serial_root).expanduser().resolve()
results: dict[str, Any] = {}
for session_id, scope in _PASS_SESSIONS.items():
session = root / session_id
summary = _summary(session)
if summary.get("result") != "PASS":
raise ValueError(f"golden PASS changed: {session_id}")
if summary.get("calibration_scope") != scope:
raise ValueError(f"golden scope changed: {session_id}")
artifacts = summary["artifacts"]
hashes = summary["hashes"]
calibration_json = session / artifacts["json"]
corrected_urdf = session / artifacts["urdf"]
actual_json_hash = _sha256_file(calibration_json)
actual_urdf_hash = _sha256_file(corrected_urdf)
if actual_json_hash != hashes["calibration_json_sha256"]:
raise ValueError(f"golden JSON hash changed: {session_id}")
if actual_urdf_hash != hashes["corrected_urdf_sha256"]:
raise ValueError(f"golden URDF hash changed: {session_id}")
if session_id == "20260831_141123" and len(
summary.get("preserved_certified_zero_joints", ())
) != 12:
raise ValueError("merged thumb no longer freezes all finger zeros")
if session_id == "20260831_163843":
payload = json.loads(calibration_json.read_text(encoding="utf-8"))
if payload.get("artifact_type") != (
"g20_right_standalone_thumb_calibration"
):
raise ValueError("standalone thumb artifact type changed")
results[session_id] = {
"result": "PASS",
"json_sha256": actual_json_hash,
"urdf_sha256": actual_urdf_hash,
}
for session_id, (error_code, reason) in _FAIL_SESSIONS.items():
session = root / session_id
summary = _summary(session)
if (
summary.get("result") != "FAIL"
or summary.get("error_code") != error_code
or summary.get("reason") != reason
):
raise ValueError(f"golden failure decision changed: {session_id}")
results[session_id] = {
"result": "FAIL",
"error_code": error_code,
"reason": reason,
}
failure_ids = set(_FAIL_SESSIONS)
for pointer_name in ("latest_passed", "latest_thumb_passed"):
pointer = root / pointer_name
if pointer.exists() and pointer.resolve().name in failure_ids:
raise ValueError(f"failure session was published through {pointer_name}")
if replay_full_session:
from .offline_replay import replay_session
session_id = "20260830_181154"
replay = replay_session(root / session_id, write_outputs=False)
expected = results[session_id]
if replay["computed_final_json_sha256"] != expected["json_sha256"]:
raise ValueError(
"full-session replay JSON is not byte-identical: "
f"computed={replay['computed_final_json_sha256']} "
f"expected={expected['json_sha256']}"
)
if replay["corrected_urdf_sha256"] != expected["urdf_sha256"]:
raise ValueError(
"full-session replay URDF is not byte-identical: "
f"computed={replay['corrected_urdf_sha256']} "
f"expected={expected['urdf_sha256']}"
)
results[session_id]["offline_replay"] = "byte_identical"
return results
def main(args: list[str] | None = None) -> None:
parser = argparse.ArgumentParser(
description="Validate the five reviewed calibration sessions"
)
parser.add_argument("serial_root")
parser.add_argument("--no-replay", action="store_true")
selected = parser.parse_args(args)
print(
json.dumps(
validate_golden_sessions(
selected.serial_root,
replay_full_session=not selected.no_replay,
),
ensure_ascii=False,
indent=2,
sort_keys=True,
)
)
if __name__ == "__main__":
main()
@@ -0,0 +1,35 @@
"""One-release typed wrappers for the legacy 11-Tag layouts."""
from ...core import ProfileKey
from .profile import get_hand_calibration_profile
from ...urdf_zero import get_zero_calibration_profile
from ..registry import RegisteredProfile
from ._adapter import adapt_profile
LEFT_KEY = ProfileKey("G20", "left", "legacy_11", 1)
RIGHT_KEY = ProfileKey("G20", "right", "legacy_11", 1)
def build_left_profile() -> RegisteredProfile:
hand = get_hand_calibration_profile(LEFT_KEY.side, LEFT_KEY.layout)
return adapt_profile(
key=LEFT_KEY,
namespace="/g20_calibration",
hand_profile=hand,
zero_profile=get_zero_calibration_profile(
LEFT_KEY.side, LEFT_KEY.layout
),
)
def build_right_profile() -> RegisteredProfile:
hand = get_hand_calibration_profile(RIGHT_KEY.side, RIGHT_KEY.layout)
return adapt_profile(
key=RIGHT_KEY,
namespace="/g20_calibration",
hand_profile=hand,
zero_profile=get_zero_calibration_profile(
RIGHT_KEY.side, RIGHT_KEY.layout
),
)
@@ -0,0 +1,15 @@
"""Reviewed motion and safe-waypoint strategy exports."""
from .profile import (
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
build_calibration_speed_profile,
)
__all__ = [
"build_calibration_motion_command",
"build_calibration_preparation_waypoints",
"build_calibration_return_waypoints",
"build_calibration_speed_profile",
]
@@ -26,7 +26,7 @@ from sensor_msgs.msg import CameraInfo, JointState
from std_msgs.msg import String
from std_srvs.srv import Trigger
from .acquisition import (
from ...acquisition import (
StateSample,
TagQuality,
interpolate_state_u8,
@@ -34,20 +34,20 @@ from .acquisition import (
tag_quality_is_valid,
update_pnp_reset_watchdog,
)
from .core import (
COMMAND_NAMES,
from ...core import (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
robust_rotation_summary,
)
from .extrinsics import (
from ...core.urdf import build_correction_plan
from ...extrinsics import (
ThreeCameraExtrinsics,
camera_info_fingerprint,
load_three_camera_extrinsics,
matrix_payload,
transform_matrix,
)
from .full_hand import (
from .profile import (
G20_COMBINATION_REQUIRED_TARGET_KEYS,
G20_REFERENCE_THUMB_CMC_JOINTS,
G20_RIGHT_19_LAYOUT,
@@ -73,23 +73,24 @@ from .full_hand import (
fit_joint_image_curve,
get_hand_calibration_profile,
)
from .hikrobot_camera import configure_fastdds_large_image_transport
from .pnp import (
from ...hikrobot_camera import configure_fastdds_large_image_transport
from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
from ...pnp import (
SquareTagGroupPoseTracker,
SquareTagPose,
SquareTagPoseTracker,
)
from .product import get_product_calibration_contract
from .sample_schema import (
from ...product import get_product_calibration_contract
from ...sample_schema import (
SampleDataContractError,
canonical_sample_record,
fitting_sample_record,
fitting_sample_records,
validate_sample_records,
)
from .storage import append_jsonl, append_jsonl_many, atomic_write_json
from .three_camera_diagnostics import render_three_camera_status_text_zh
from .urdf_zero import (
from ...storage import append_jsonl, append_jsonl_many, atomic_write_json
from .reporting_zh import render_three_camera_status_text_zh
from .zero_solver import (
LEFT_ZERO_PROFILE,
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
@@ -318,13 +319,19 @@ def _palm_axis_observer_schema(
def _palm_axis_resume_policy(
profile: HandCalibrationProfile,
session_start: Mapping[str, Any],
compatibility_tokens: frozenset[str] | None = None,
) -> tuple[bool, tuple[str, ...]]:
"""Validate optional palm-axis checkpoint data when a model uses it."""
capability = "palm_axis_relative_motion_v3"
previous = {str(value) for value in session_start.get("capabilities", [])}
if not profile.palm_axis_observers and capability not in profile.capabilities:
current = (
frozenset(profile.capabilities)
if compatibility_tokens is None
else frozenset(compatibility_tokens)
)
if not profile.palm_axis_observers and capability not in current:
return True, ()
required_previous = set(profile.capabilities) - {capability}
required_previous = set(current) - {capability}
if not required_previous.issubset(previous):
raise ValueError("resume checkpoint lacks required capabilities")
if capability in previous:
@@ -741,7 +748,7 @@ def _requires_pnp_tracker_reset_for_sweep(
return False
if is_fit_retry:
return True
if profile.supports("precheck_sweeps"):
if profile.precheck_sweeps:
return bool(item.precheck)
return not item.precheck and item.cycle == 0
@@ -813,6 +820,52 @@ def _maximum_corner_drift_px(
)
def _resume_fixed_base_position_compatibility(
rows: Sequence[Mapping[str, Any]],
current_corners_by_view: Mapping[
str, Sequence[Sequence[float]] | None
],
maximum_corner_drift_px: float,
) -> tuple[dict[str, float], tuple[str, ...], tuple[str, ...]]:
"""Compare the previous and current session-start palm references.
Calibration measurements are invariant to one rigid hand placement, but
samples expressed in two independently established common frames must
never be combined. Fixed palm-Tag corners provide a camera-native check
before any durable task is imported.
"""
previous_corners_by_view: dict[str, Any] = {}
for row in rows:
if str(row.get("kind", "")) != "fixed_base_reference_locked":
continue
view = str(row.get("view", ""))
if view in current_corners_by_view:
# Keep the last lock in case a future compatible schema records a
# deliberate pre-scan relock in the same raw stream.
previous_corners_by_view[view] = row.get("corner_reference_xy")
drift_by_view_px: dict[str, float] = {}
changed_views: list[str] = []
unverifiable_views: list[str] = []
for view, current in current_corners_by_view.items():
previous = previous_corners_by_view.get(str(view))
if previous is None or current is None:
unverifiable_views.append(str(view))
continue
drift = _maximum_corner_drift_px(previous, current)
if not math.isfinite(drift):
unverifiable_views.append(str(view))
continue
drift_by_view_px[str(view)] = drift
if drift > float(maximum_corner_drift_px):
changed_views.append(str(view))
return (
drift_by_view_px,
tuple(sorted(changed_views)),
tuple(sorted(unverifiable_views)),
)
def _selected_pose_qualities(
selected: Mapping[str, SquareTagPose],
live_qualities: Mapping[str, TagQuality],
@@ -1421,6 +1474,19 @@ def _unresolved_fit_failure_tasks(
# failures that mixed velocity lag or firmware tracking
# deadband with mechanical hysteresis are safe to revalidate.
return True
if (
profile.layout_id == G20_RIGHT_19_LAYOUT
and metric == "axis_pose_line_rms_mm"
and joint_name in profile.record_specs
and not profile.record_specs[
joint_name
].pose_axis_line_required
):
# Current profile policy owns whether this monocular 3-D
# diagnostic is release-critical. Revalidate the complete
# raw task under that policy instead of making an old failure
# force another acquisition forever.
return True
if (
profile.layout_id == G20_RIGHT_19_LAYOUT
and joint_name
@@ -2015,7 +2081,7 @@ def _build_sweep_plan(
"""
plan: list[SweepItem] = []
for spec in profile.sweep_specs:
if profile.supports("precheck_sweeps"):
if profile.precheck_sweeps:
plan.extend(
SweepItem(spec, -1, direction, precheck=True)
for direction in (
@@ -2708,6 +2774,7 @@ class G20ThreeCameraCalibrationNode(Node):
)
self.profile = product_contract.profile
self.zero_profile = product_contract.zero_profile
self.calibration_profile = product_contract.typed_profile
self.serial_number = str(value("serial_number"))
if self.serial_number == "UNSET":
raise ValueError("serial_number is required")
@@ -2766,6 +2833,11 @@ class G20ThreeCameraCalibrationNode(Node):
)
self.resumed_task_keys: tuple[str, ...] = ()
self.resume_source_session = ""
self.resume_checkpoint_pending = False
self.resume_position_policy = "not_requested"
self.resume_position_changed_views: tuple[str, ...] = ()
self.resume_position_unverifiable_views: tuple[str, ...] = ()
self.resume_position_drift_by_view_px: dict[str, float] = {}
self.camera_extrinsics_file = Path(
str(value("camera_extrinsics_file"))
).expanduser().resolve()
@@ -5226,7 +5298,19 @@ class G20ThreeCameraCalibrationNode(Node):
(
palm_axis_schema_compatible,
palm_axis_invalidated_tasks,
) = _palm_axis_resume_policy(self.profile, start)
) = _palm_axis_resume_policy(
self.profile,
start,
getattr(
getattr(
getattr(self, "calibration_profile", None),
"artifacts",
None,
),
"session_compatibility_tokens",
frozenset(self.profile.capabilities),
),
)
except ValueError as error:
raise RuntimeError(
"resume checkpoint algorithm capabilities differ"
@@ -5247,6 +5331,54 @@ class G20ThreeCameraCalibrationNode(Node):
"resume checkpoint geometry, algorithm capabilities, Tag "
"layout, baseline or source URDF differs"
)
current_corners_by_view = {
str(view): getattr(runtime, "locked_base_corners_xy", None)
for view, runtime in getattr(self, "views", {}).items()
}
if not current_corners_by_view:
raise RuntimeError(
"resume checkpoint requires current fixed-base references"
)
(
position_drift_by_view_px,
position_changed_views,
position_unverifiable_views,
) = _resume_fixed_base_position_compatibility(
rows,
current_corners_by_view,
float(
getattr(self, "fixed_base_maximum_corner_drift_px", 2.0)
),
)
self.resume_source_session = source.parent.name
self.resume_position_drift_by_view_px = dict(
position_drift_by_view_px
)
self.resume_position_changed_views = position_changed_views
self.resume_position_unverifiable_views = (
position_unverifiable_views
)
start_position_invalidated_tasks: tuple[str, ...] = ()
if position_changed_views or position_unverifiable_views:
if str(getattr(self, "recalibration_scope", "full")) != "full":
affected = sorted(
{*position_changed_views, *position_unverifiable_views}
)
raise RuntimeError(
"resume checkpoint start pose differs or cannot be "
"verified for partial recalibration: "
+ ",".join(affected)
)
start_position_invalidated_tasks = tuple(
spec.key for spec in self.profile.sweep_specs
)
self.resume_position_policy = (
"discard_all_tasks_for_new_start_pose"
if position_changed_views
else "discard_all_tasks_for_unverified_start_pose"
)
else:
self.resume_position_policy = "reuse_same_start_pose"
try:
changed_tag_size_ids, size_invalidated_tasks = (
resume_tasks_invalidated_by_tag_size_changes(
@@ -5261,6 +5393,7 @@ class G20ThreeCameraCalibrationNode(Node):
) from error
invalidated_task_set = set(size_invalidated_tasks)
invalidated_task_set.update(palm_axis_invalidated_tasks)
invalidated_task_set.update(start_position_invalidated_tasks)
scope_invalidated_tasks = tuple(
getattr(self, "recalibration_task_keys", ()) or ()
)
@@ -5368,7 +5501,6 @@ class G20ThreeCameraCalibrationNode(Node):
)
self.resumed_task_keys = completed
G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self)
self.resume_source_session = source.parent.name
missing_tasks = [
spec.key
for spec in self.profile.sweep_specs
@@ -5398,6 +5530,20 @@ class G20ThreeCameraCalibrationNode(Node):
"scope_invalidated_task_keys": list(
scope_invalidated_tasks
),
"start_position_policy": self.resume_position_policy,
"start_position_changed_views": list(
position_changed_views
),
"start_position_unverifiable_views": list(
position_unverifiable_views
),
"start_position_drift_by_view_px": {
view: round(float(value), 6)
for view, value in position_drift_by_view_px.items()
},
"start_position_invalidated_task_keys": list(
start_position_invalidated_tasks
),
"imported_record_count": len(reusable),
"imported_attempt_floor_by_task": attempt_floor_by_task,
"source_raw_samples_sha256": _file_sha256(source),
@@ -5551,6 +5697,19 @@ class G20ThreeCameraCalibrationNode(Node):
return response
self.started = True
self.startup_baseline_recovered = False
self.resumed_task_keys = ()
self.resume_source_session = ""
self.resume_checkpoint_pending = bool(
self.resume_raw_samples_path is not None
)
self.resume_position_policy = (
"pending_new_start_pose_check"
if self.resume_checkpoint_pending
else "not_requested"
)
self.resume_position_changed_views = ()
self.resume_position_unverifiable_views = ()
self.resume_position_drift_by_view_px = {}
self.sweep_items = []
self.pnp_task_spec = None
selected_sweep_specs = list(self.profile.sweep_specs)
@@ -5646,7 +5805,19 @@ class G20ThreeCameraCalibrationNode(Node):
self.recalibration_task_keys
),
"command_names": list(_command_names(self)),
"capabilities": sorted(self.profile.capabilities),
# Retained only as an old-session serialization token. Live
# decisions use the typed motion/measurement policies.
"capabilities": sorted(
getattr(
getattr(
getattr(self, "calibration_profile", None),
"artifacts",
None,
),
"session_compatibility_tokens",
frozenset(self.profile.capabilities),
)
),
"reference_finger": self.profile.reference_finger,
"view_tags": {
view: dict(tags)
@@ -5706,10 +5877,16 @@ class G20ThreeCameraCalibrationNode(Node):
)
try:
if self.resume_raw_samples_path is not None:
self.state = STATE_IMPORTING_BASE
self.reason = "reading_base_session_records"
if not self.resume_raw_samples_path.is_file():
raise RuntimeError(
"resume raw samples do not exist: "
f"{self.resume_raw_samples_path}"
)
self.resume_source_session = (
self.resume_raw_samples_path.parent.name
)
self.base_import_progress = {
"phase": "reading",
"phase": "waiting_for_new_start_pose_reference",
"records_read": 0,
"bytes_read": 0,
"total_bytes": int(
@@ -5717,10 +5894,9 @@ class G20ThreeCameraCalibrationNode(Node):
),
"fraction": 0.0,
}
self._publish_status(time.monotonic())
restored_tasks = self._restore_durable_task_checkpoint()
except Exception as error:
self.started = False
self.resume_checkpoint_pending = False
response.success = False
response.message = f"CFG-RESUME-009:{error}"
return response
@@ -5728,8 +5904,11 @@ class G20ThreeCameraCalibrationNode(Node):
response.success = True
response.message = (
"three-camera calibration started"
if restored_tasks == 0
else f"calibration resumed with {restored_tasks} completed tasks"
if not self.resume_checkpoint_pending
else (
"three-camera calibration started; checkpoint will be "
"verified after the new start-pose reference is locked"
)
)
return response
@@ -8157,7 +8336,7 @@ class G20ThreeCameraCalibrationNode(Node):
}
)
continue
if profile.supports("steady_command_checkpoints"):
if profile.steady_command_checkpoints:
command_store = getattr(
self, "command_records_by_joint", None
)
@@ -8438,7 +8617,7 @@ class G20ThreeCameraCalibrationNode(Node):
"maximum",
),
)
if not profile.supports("directional_zero"):
if not profile.directional_zero:
checks = checks + (
(
"hysteresis_deg",
@@ -8584,6 +8763,35 @@ class G20ThreeCameraCalibrationNode(Node):
# error is an admissibility check for constrained circles.
for metric, actual, limit in axis_checks:
if actual > limit:
if (
metric == "axis_pose_line_rms_mm"
and not joint_spec.pose_axis_line_required
):
append_jsonl(
self.raw_path,
{
"kind": (
"position_invariant_quality_diagnostic"
),
"task_name": spec.key,
"joint": joint_name,
"cycle": cycle + 1,
"metric": metric,
"actual": round(float(actual), 6),
"reference_limit": round(
float(limit), 6
),
"decision": "diagnostic_only",
"authoritative_quality": [
"tag_pnp",
"image_trajectory",
"relative_rotation",
"synchronisation",
"isolated_holdout",
],
},
)
continue
failure_joint = joint_name
quality_sources: tuple[str, ...] = ()
if metric == "axis_pose_line_rms_mm":
@@ -8789,7 +8997,7 @@ class G20ThreeCameraCalibrationNode(Node):
if index != outlier
]
failures.append(failure)
if profile.supports("cross_view_roll_curve"):
if profile.cross_view_roll_curve:
for primary_name, validation_name in (
profile.axis_validation_sources or {}
).items():
@@ -11565,6 +11773,20 @@ class G20ThreeCameraCalibrationNode(Node):
name: published_zero_offsets[name]
for name in self.zero_profile.direct_zero_joints
}
correction_plan = None
typed_profile = getattr(self, "calibration_profile", None)
if typed_profile is not None:
frozen_names = typed_profile.scope.frozen_joints[
self.recalibration_scope
]
correction_plan = build_correction_plan(
typed_profile,
source_sha256=_file_sha256(self.source_urdf_path),
scope=self.recalibration_scope,
frozen_offsets_rad={
name: urdf_offsets[name] for name in frozen_names
},
)
self.corrected_urdf_path = write_zero_corrected_urdf(
source_urdf=self.source_urdf_path,
output_directory=self.corrected_urdf_output_dir,
@@ -11572,6 +11794,7 @@ class G20ThreeCameraCalibrationNode(Node):
offsets_rad=urdf_offsets,
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
timestamp=stamp,
correction_plan=correction_plan,
)
try:
if self.standalone_thumb_calibration:
@@ -11885,6 +12108,21 @@ class G20ThreeCameraCalibrationNode(Node):
elif self.startup_baseline_recovered and self._all_preflight_ready(now):
locker = getattr(self, "_lock_fixed_base_references", None)
if locker is None or locker():
if getattr(self, "resume_checkpoint_pending", False):
self.resume_checkpoint_pending = False
self.state = STATE_IMPORTING_BASE
self.reason = "reading_base_session_records"
self.base_import_progress = {
"phase": "reading",
"records_read": 0,
"bytes_read": 0,
"total_bytes": int(
self.resume_raw_samples_path.stat().st_size
),
"fraction": 0.0,
}
self._publish_status(time.monotonic())
self._restore_durable_task_checkpoint()
self._start_next_sweep()
else:
self.reason = "locking_fixed_base_references"
@@ -12932,7 +13170,32 @@ class G20ThreeCameraCalibrationNode(Node):
"camera_extrinsics_error": self.extrinsics_error,
"resume": {
"used": bool(self.resumed_task_keys),
"checkpoint_requested": bool(
self.resume_raw_samples_path is not None
),
"checkpoint_pending": bool(
getattr(self, "resume_checkpoint_pending", False)
),
"source_session": self.resume_source_session,
"start_position_policy": getattr(
self, "resume_position_policy", "not_requested"
),
"start_position_changed_views": list(
getattr(self, "resume_position_changed_views", ())
),
"start_position_unverifiable_views": list(
getattr(
self,
"resume_position_unverifiable_views",
(),
)
),
"start_position_drift_by_view_px": {
view: round(float(value), 6)
for view, value in getattr(
self, "resume_position_drift_by_view_px", {}
).items()
},
"recalibration_scope": self.recalibration_scope,
"recalibration_task_keys": list(
self.recalibration_task_keys
@@ -19,8 +19,10 @@ import numpy as np
from scipy.spatial.transform import Rotation
import yaml
from .extrinsics import load_three_camera_extrinsics
from .full_hand import (
from ...compat import default_three_camera_config_path
from ...compat.legacy import uses_coupled_full_hand_zero_solver
from ...extrinsics import load_three_camera_extrinsics
from .profile import (
G20_REFERENCE_THUMB_CMC_JOINTS,
G20_RIGHT_19_LAYOUT,
RIGHT_19_END_ON_IMAGE_CURVE_JOINTS,
@@ -36,10 +38,11 @@ from .full_hand import (
get_hand_calibration_profile,
validate_compact_payload,
)
from .product import get_product_calibration_contract
from .sample_schema import fitting_sample_record, fitting_sample_records
from .storage import atomic_write_json
from .urdf_zero import (
from ...product import get_product_calibration_contract
from ...sample_schema import fitting_sample_record, fitting_sample_records
from ...storage import atomic_write_json
from .publication import clamp_compact_payload_to_urdf_limits
from .zero_solver import (
JointAxisMeasurement,
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
UrdfKinematicModel,
@@ -76,6 +79,42 @@ def _sha256(path: Path) -> str:
return digest.hexdigest()
def _payload_sha256(payload: Mapping[str, Any]) -> str:
encoded = (
json.dumps(payload, ensure_ascii=False, indent=2) + "\n"
).encode("utf-8")
return hashlib.sha256(encoded).hexdigest()
def _legacy_published_quality(
session: Path, *, side: str, serial_number: str
) -> dict[str, Any]:
"""Recover v1 random-validation aggregates that raw JSONL omitted."""
path = session / f"g20_{side}_{serial_number}_calibration.json"
if not path.is_file():
raise ValueError(
"legacy replay requires its published JSON because v1 raw "
"samples omitted random-validation observations"
)
payload = json.loads(path.read_text(encoding="utf-8"))
quality = payload.get("quality")
if not isinstance(quality, Mapping) or set(quality) != {
"passed",
"validation_mae_rad",
"validation_p95_rad",
}:
raise ValueError("legacy published JSON has an invalid quality block")
if not bool(quality["passed"]):
raise ValueError("legacy published JSON did not pass validation")
values = (
float(quality["validation_mae_rad"]),
float(quality["validation_p95_rad"]),
)
if any(not math.isfinite(value) or value < 0.0 for value in values):
raise ValueError("legacy published quality contains an invalid error")
return dict(quality)
def _output_suffix(output_tag: str | None) -> str:
"""Return a filename-safe suffix for a non-destructive replay variant."""
if output_tag is None:
@@ -795,6 +834,7 @@ def _quality_failures(
if (
profile.record_specs[name].zero_kind
!= "axis_cross_view_validation"
and profile.record_specs[name].pose_axis_line_required
and cross_view_side_line_source(axis) is None
and axis.pose_axis_line_rms_m
> float(parameters["axis_maximum_pose_line_rms_m"])
@@ -1009,9 +1049,8 @@ def replay_session(
output_tag: str | None = None,
) -> dict[str, Any]:
session = Path(session_dir).expanduser().resolve()
package_root = Path(__file__).resolve().parents[1]
config = (
package_root / "config" / "three_camera_calibration.yaml"
default_three_camera_config_path()
if config_file is None
else Path(config_file).expanduser().resolve()
)
@@ -1024,6 +1063,7 @@ def replay_session(
palm_axis_records_by_source,
raw_path,
) = _load_raw_session(session)
legacy_coupled_zero_solver = uses_coupled_full_hand_zero_solver(start)
model = str(start.get("model", "G20")).strip().upper()
side = str(start["hand_type"]).lower()
requested_layout_id = str(
@@ -1199,9 +1239,33 @@ def replay_session(
raise ValueError(
f"{name} has {len(records)}/18 steady command checkpoints"
)
command_fits[name] = _fit_curve(
name, records, profile=profile, baseline=baseline
zero_command = int(
baseline[profile.joint_specs[name].motor_index]
)
if name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS:
command_fits[name] = fit_joint_image_curve(
records,
maximum_radial_rms_px=float(
parameters["image_trajectory_maximum_radial_rms_px"]
),
maximum_radial_p95_px=float(
parameters["image_trajectory_maximum_radial_p95_px"]
),
minimum_radius_px=float(
parameters["image_trajectory_minimum_radius_px"]
),
minimum_arc_rad=math.radians(
float(parameters["trajectory_minimum_arc_deg"])
),
)
else:
command_fits[name] = fit_rotation_joint_curve(
records,
zero_command_u8=zero_command,
canonical_zero_direction=canonical_zero_direction(
profile, name
),
)
command_gap_limit = math.radians(
float(parameters.get("command_maximum_direction_gap_deg", 2.0))
)
@@ -1300,10 +1364,23 @@ def replay_session(
source_urdf=source_urdf,
repetitions=repetitions,
)
palm_records_for_fit = palm_axis_records_by_source
if legacy_coupled_zero_solver:
palm_records_for_fit = {
name: [
{
key: value
for key, value in record.items()
if key != "child_pose_common"
}
for record in records
]
for name, records in palm_axis_records_by_source.items()
}
palm_orientation_measurements, palm_orientation_rejections = (
fit_partial_palm_orientation_measurements(
sources=profile.palm_orientation_sources,
records_by_joint=palm_axis_records_by_source,
records_by_joint=palm_records_for_fit,
motor_by_source=profile.palm_axis_motor_by_source,
baseline_command_u8=baseline,
cycles=range(repetitions),
@@ -1543,7 +1620,7 @@ def replay_session(
fits: Mapping[str, JointCurveFit],
arguments: Mapping[str, Any],
):
if layout_id != G20_RIGHT_19_LAYOUT:
if layout_id != G20_RIGHT_19_LAYOUT or legacy_coupled_zero_solver:
return solve_urdf_zero_offsets(curves=fits, **arguments)
thumb_profile = get_right_19_thumb_zero_profile()
thumb_names = set(thumb_profile.direct_zero_joints)
@@ -1800,6 +1877,19 @@ def replay_session(
else None
),
)
payload, _clipped_runtime_joints = (
clamp_compact_payload_to_urdf_limits(payload, candidate)
)
legacy_quality_source = None
if legacy_coupled_zero_solver:
payload["quality"] = _legacy_published_quality(
session,
side=side,
serial_number=hand_serial,
)
legacy_quality_source = (
"published_v1_artifact_random_validation_omitted_from_raw"
)
validate_compact_payload(payload)
if write_outputs:
if _sha256(source_urdf) != source_hash_before:
@@ -1901,6 +1991,8 @@ def replay_session(
"maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
},
"corrected_urdf_sha256": candidate_hash,
"computed_final_json_sha256": _payload_sha256(payload),
"legacy_quality_source": legacy_quality_source,
"final_json": str(final_json) if write_outputs else None,
"corrected_urdf": str(final_urdf) if write_outputs else None,
}
@@ -17,15 +17,15 @@ import xml.etree.ElementTree as ET
import numpy as np
from .core import COMMAND_NAMES
from .trajectory import (
from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
from ...trajectory import (
_angle_for_circle,
_fit_circle_with_axis,
_fit_joint_curve,
_fit_plane_axis,
_orient_circle_positive,
)
from .zero_calibration import (
from ...zero_calibration import (
_fit_circle,
_trajectory_arc_rad,
circular_median_rad,
@@ -51,6 +51,12 @@ class JointSpec:
child_role: str | None
source_joint: str | None = None
zero_kind: str | None = None
# Keep the physical axis-line gate only when that line contributes to a
# released URDF zero/axis decision. Curve-only passive measurements may
# retain the monocular line residual as a diagnostic while their image
# trajectory, relative rotation, synchronisation and holdout gates remain
# release-critical.
pose_axis_line_required: bool = True
@property
def measured(self) -> bool:
@@ -129,6 +135,12 @@ class HandCalibrationProfile:
command_names: tuple[str, ...] = COMMAND_NAMES
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
capabilities: frozenset[str] = frozenset()
precheck_sweeps: bool = False
steady_command_checkpoints: bool = False
directional_zero: bool = False
isolated_holdout: bool = False
cross_view_roll_curve: bool = False
stable_cross_view_cone_bias: bool = False
@property
def command_count(self) -> int:
@@ -537,7 +549,13 @@ def _build_right_19_profile() -> HandCalibrationProfile:
zero_kind="urdf_axis_chain",
),
"thumb_ip": JointSpec(
"thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
"thumb_ip",
15,
False,
"front",
"thumb_mcp",
"thumb_ip",
pose_axis_line_required=False,
),
}
for finger in ("index", "middle", "ring", "pinky"):
@@ -703,6 +721,12 @@ def _build_right_19_profile() -> HandCalibrationProfile:
"palm_axis_relative_motion_v3",
}
),
precheck_sweeps=True,
steady_command_checkpoints=True,
directional_zero=True,
isolated_holdout=True,
cross_view_roll_curve=True,
stable_cross_view_cone_bias=True,
)
@@ -15,15 +15,16 @@ import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation
from .full_hand import (
from .profile import (
G20_COMBINATION_REQUIRED_TARGET_KEYS,
G20_RIGHT_19_LAYOUT,
get_hand_calibration_profile,
validate_compact_payload,
)
from .product import ProductConfig, sha256_file
from .storage import atomic_write_json
from .urdf_zero import (
from ...product import ProductConfig, sha256_file
from ...storage import atomic_write_json
from ...core.urdf import UrdfCorrectionPlan, build_correction_plan
from .zero_solver import (
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
get_zero_calibration_profile,
)
@@ -202,6 +203,7 @@ def verify_corrected_urdf(
*,
expected_offsets_rad: Mapping[str, float] | None = None,
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
correction_plan: UrdfCorrectionPlan | None = None,
) -> tuple[str, ...]:
"""Prove that only active-joint origin.rpy attributes changed.
@@ -210,6 +212,13 @@ def verify_corrected_urdf(
"""
source_text = Path(source).read_text(encoding="utf-8")
corrected_text = Path(corrected).read_text(encoding="utf-8")
if correction_plan is not None:
correction_plan.verify_source(source)
if expected_offsets_rad is None:
raise ValueError(
"correction-plan validation requires expected offsets"
)
correction_plan.authorize_offsets(expected_offsets_rad)
before = _joint_blocks(source_text)
after = _joint_blocks(corrected_text)
if set(before) != set(after):
@@ -243,6 +252,12 @@ def verify_corrected_urdf(
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
}
if endpoint_offsets:
if correction_plan is not None and not set(endpoint_offsets).issubset(
correction_plan.endpoint_limit_joints
):
raise ValueError(
"endpoint offsets are not authorized by the correction plan"
)
source_joints = _joint_elements(source)
corrected_joints = _joint_elements(corrected)
for name, offset in endpoint_offsets.items():
@@ -745,11 +760,20 @@ def finalize_session_artifacts(
name: offsets[name]
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
}
typed_profile = config.calibration_contract.typed_profile
frozen_names = typed_profile.scope.frozen_joints[calibration_scope]
correction_plan = build_correction_plan(
typed_profile,
source_sha256=config.source_urdf_sha256,
scope=calibration_scope,
frozen_offsets_rad={name: offsets[name] for name in frozen_names},
)
changed_joints = verify_corrected_urdf(
config.source_urdf,
paths["urdf"],
expected_offsets_rad=offsets,
endpoint_anchored_offsets_rad=endpoint_offsets,
correction_plan=correction_plan,
)
if standalone_thumb:
clipped_runtime_joints = {}
@@ -0,0 +1,27 @@
"""Independent reviewed profile for the right 19-Tag product layout."""
from ...core import ProfileKey
from .profile import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile
from .zero_policy import (
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
get_zero_calibration_profile,
)
from ..registry import RegisteredProfile
from ._adapter import adapt_profile
KEY = ProfileKey("G20", "right", G20_RIGHT_19_LAYOUT, 1)
def build_profile() -> RegisteredProfile:
hand = get_hand_calibration_profile(KEY.side, KEY.layout)
zero = get_zero_calibration_profile(KEY.side, KEY.layout)
return adapt_profile(
key=KEY,
namespace="/g20_calibration",
hand_profile=hand,
zero_profile=zero,
mechanical_endpoint_joints=RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
post_solve_endpoint_joints=RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
)
@@ -20,11 +20,15 @@ from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger
from .hikrobot_camera import configure_fastdds_large_image_transport
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
from .product import ProductConfig, load_product_config, sha256_file
from ...hikrobot_camera import configure_fastdds_large_image_transport
from ...operator_report import (
ProgressEstimator,
build_failure_report,
render_progress_zh,
)
from ...product import ProductConfig, load_product_config, sha256_file
from .publication import atomic_session_pointer, finalize_session_artifacts
from .storage import atomic_write_json
from ...storage import atomic_write_json
EXIT_PASS = 0
@@ -137,7 +141,7 @@ class ProgressConsole:
def _default_product_config() -> Path:
try:
installed = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
get_package_share_directory("linkerhand_calibration")
) / "config" / "g20_right_product.yaml"
if installed.is_file():
return installed
@@ -145,7 +149,7 @@ def _default_product_config() -> Path:
pass
return (
Path.cwd()
/ "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
/ "src/linkerhand_calibration/config/g20_right_product.yaml"
).resolve()
@@ -188,7 +192,7 @@ def _launch_command(
return [
"ros2",
"launch",
"g20_thumb_apriltag_calibration",
"linkerhand_calibration",
"three_camera_calibration.launch.py",
*(f"{name}:={value}" for name, value in values.items()),
]
@@ -0,0 +1,17 @@
"""Reviewed static-zero, endpoint, and mimic topology exports."""
from .profile import MIMIC_DERIVED_FINGER_DIPS
from .zero_solver import (
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
get_zero_calibration_profile,
)
__all__ = [
"MIMIC_DERIVED_FINGER_DIPS",
"RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS",
"RIGHT_19_MECHANICAL_ENDPOINT_JOINTS",
"RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS",
"get_zero_calibration_profile",
]
@@ -17,8 +17,9 @@ from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation
from scipy.stats import t as student_t
from .core import fit_rotation_axis, robust_rotation_summary
from .full_hand import (
from ...core import fit_rotation_axis, robust_rotation_summary
from ...core.urdf import UrdfCorrectionPlan
from .profile import (
G20_RIGHT_19_LAYOUT,
IMAGE_TRAJECTORY_JOINTS,
LEFT_HAND_PROFILE,
@@ -28,8 +29,8 @@ from .full_hand import (
JointCurveFit,
get_hand_calibration_profile,
)
from .sample_schema import explicit_domain_value
from .trajectory import (
from ...sample_schema import explicit_domain_value
from ...trajectory import (
_fit_circle_with_axis,
_fit_joint_curve,
_fit_plane_axis,
@@ -66,6 +67,11 @@ class ZeroCalibrationProfile:
# thumb kernel instead uses only the serial thumb chain, so its nuisance
# palm pose cannot be influenced by finger observations.
base_pose_strategy: str = "full_hand"
# A serial-chain model may use a separate palm-root joint axis to fix the
# otherwise free rotation about its primary root axis. G20 retains its
# historical defaults; other model profiles can name the physical anchor
# explicitly without introducing model-specific branches in the solver.
orientation_anchor_joint: str | None = None
@property
def reference_finger(self) -> str:
@@ -3237,7 +3243,7 @@ def solve_urdf_zero_offsets(
np.ptp(np.asarray(curves[name].angle_rad))
),
)
orientation_anchor = (
orientation_anchor = profile.orientation_anchor_joint or (
"thumb_cmc_pitch"
if profile.base_pose_strategy == "thumb_serial"
else f"{profile.reference_finger}_mcp_pitch"
@@ -3850,7 +3856,7 @@ def solve_urdf_zero_offsets(
if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad:
cone_range = float(np.ptp(cone_mismatches))
stable_product_bias = bool(
profile.hand.supports("stable_cross_view_cone_bias")
profile.hand.stable_cross_view_cone_bias
and maximum_systematic_axis_cone_bias_rad is not None
and len(cone_mismatches) >= 4
and maximum_cone_mismatch
@@ -4454,6 +4460,7 @@ def write_zero_corrected_urdf(
offsets_rad: Mapping[str, float],
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
timestamp: str | None = None,
correction_plan: UrdfCorrectionPlan | None = None,
) -> Path:
source = Path(source_urdf).expanduser().resolve()
output = Path(output_directory).expanduser().resolve()
@@ -4475,6 +4482,15 @@ def write_zero_corrected_urdf(
}
if not set(endpoint_offsets) <= set(offsets):
raise ValueError("endpoint-anchored offsets must be URDF zero targets")
if correction_plan is not None:
correction_plan.verify_source(source)
correction_plan.authorize_offsets(offsets)
if not set(endpoint_offsets).issubset(
correction_plan.endpoint_limit_joints
):
raise ValueError(
"endpoint offsets are not authorized by the correction plan"
)
if any(
not math.isfinite(value) or abs(value) > math.radians(90.0)
for value in offsets.values()
@@ -0,0 +1,12 @@
"""Registered L6 calibration profiles."""
from ..registry import ProfileRegistry
def register_profiles(registry: ProfileRegistry) -> None:
from .profile import build_profile
registry.register(build_profile())
__all__ = ["register_profiles"]
@@ -0,0 +1,524 @@
"""Schema-v6 runtime artifact and atomic partial-result publication for L6."""
from __future__ import annotations
import json
import hashlib
import math
import os
from pathlib import Path
from typing import Any, Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from .fitting import L6FitResult
from .profile import (
ACTIVE_JOINTS,
CALIBRATED_ACTIVE_JOINTS,
COMMAND_INDEX_BY_JOINT,
COMMAND_NAMES,
COUPLING_MODEL_BY_JOINT,
ENDPOINT_ANCHOR_BY_JOINT,
KEY,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
PASSIVE_JOINTS,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
build_typed_profile,
)
ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS)
def _sha256_file(path: str | Path) -> str:
digest = hashlib.sha256()
with Path(path).open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _source_joint_metadata(
source_urdf: str | Path,
) -> dict[str, dict[str, float | str]]:
root = ET.parse(Path(source_urdf)).getroot()
result: dict[str, dict[str, float | str]] = {}
for joint in root.findall("joint"):
name = str(joint.get("name", ""))
if name not in ALL_REVOLUTE_JOINTS:
continue
if joint.get("type") != "revolute":
raise ValueError(f"L6 profile joint is not revolute: {name}")
limit = joint.find("limit")
if limit is None:
raise ValueError(f"L6 source joint has no limit: {name}")
item: dict[str, float | str] = {
"lower": float(limit.get("lower", "nan")),
"upper": float(limit.get("upper", "nan")),
}
mimic = joint.find("mimic")
if mimic is not None:
item.update(
{
"source_joint": str(mimic.get("joint", "")),
"multiplier": float(mimic.get("multiplier", "nan")),
"offset": float(mimic.get("offset", "0")),
}
)
result[name] = item
if set(result) != ALL_REVOLUTE_JOINTS:
raise ValueError("source URDF does not contain exactly 11 L6 revolute joints")
return result
def _rounded_curve(values: Sequence[float]) -> list[float]:
curve = np.asarray(values, dtype=float)
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
raise ValueError("runtime curve must contain 256 finite values")
return [round(float(value), 8) for value in curve]
def _linear_curve(lower: float, upper: float) -> np.ndarray:
# feedback 255 is the open/lower endpoint and feedback 0 is upper/closed.
return np.linspace(float(upper), float(lower), 256, dtype=float)
def _validate_transfer_topology(
source: Mapping[str, Mapping[str, float | str]],
) -> None:
for transfers in (
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
):
for target, donor in transfers.items():
for field in ("lower", "upper"):
if abs(
float(source[target][field]) - float(source[donor][field])
) > 1.0e-8:
raise ValueError(
f"L6 transfer {target} differs from {donor} {field}"
)
def build_l6_runtime_payload(
*,
serial_number: str,
source_urdf: str | Path,
result: L6FitResult,
protected_inputs: Mapping[str, str],
passed: bool = True,
) -> dict[str, Any]:
"""Build all eleven L6 curves with explicit measured/frozen provenance."""
profile = build_typed_profile()
source = _source_joint_metadata(source_urdf)
_validate_transfer_topology(source)
joints: dict[str, dict[str, Any]] = {}
curves: dict[str, np.ndarray] = {}
for name in ACTIVE_JOINTS:
metadata = source[name]
motor_index = COMMAND_INDEX_BY_JOINT[name]
measurement_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(
name, name
)
if (
name in CALIBRATED_ACTIVE_JOINTS
or name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
):
fit = result.curves[measurement_source]
decreasing = np.asarray(fit.decreasing_rad, dtype=float)
increasing = np.asarray(fit.increasing_rad, dtype=float)
curve = 0.5 * (decreasing + increasing)
curve -= float(curve[255])
decreasing = decreasing - float(decreasing[255])
increasing = increasing - float(increasing[255])
zero_method = result.zero_method_by_joint[measurement_source]
zero_angles: dict[str, Any] = {
"policy": zero_method,
"measured_travel_rad": round(
float(result.travels_rad[measurement_source]), 8
),
"urdf_origin_offset_rad": round(
float(result.zero_offsets_rad[measurement_source]), 8
),
}
if measurement_source in ENDPOINT_ANCHOR_BY_JOINT:
anchor = ENDPOINT_ANCHOR_BY_JOINT[measurement_source]
if anchor == "cad_range_center":
zero_angles.update(
{
"source_lower_rad": round(
float(metadata["lower"]), 8
),
"source_upper_rad": round(
float(metadata["upper"]), 8
),
}
)
else:
endpoint_field = {
"lower_at_start": "source_lower_rad",
"upper_at_end": "source_upper_rad",
"zero_at_start": "source_joint_zero_rad",
}[anchor]
endpoint_value = {
"lower_at_start": metadata["lower"],
"upper_at_end": metadata["upper"],
"zero_at_start": 0.0,
}[anchor]
zero_angles[endpoint_field] = round(
float(endpoint_value), 8
)
if measurement_source in result.zero_fallback_reason_by_joint:
zero_angles["geometry_fallback_reason"] = (
result.zero_fallback_reason_by_joint[measurement_source]
)
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT:
zero_angles.update(
{
"policy": "transferred_from_pinky",
"transferred_from_joint": measurement_source,
}
)
else:
curve = _linear_curve(
float(metadata["lower"]), float(metadata["upper"])
)
decreasing = curve.copy()
increasing = curve.copy()
zero_angles = {
"policy": "cad_nominal",
"source_lower_rad": round(float(metadata["lower"]), 8),
"source_upper_rad": round(float(metadata["upper"]), 8),
}
curves[name] = curve
joint_payload = {
"urdf_joint": name,
"sdk_channel": COMMAND_NAMES[motor_index],
"motor_index": motor_index,
"passive": False,
"calibration_status": profile.joint_coverage[name],
"zero_command_u8": 255,
"zero_angles": zero_angles,
"angle_rad": _rounded_curve(curve),
"decreasing_rad": _rounded_curve(decreasing),
"increasing_rad": _rounded_curve(increasing),
}
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT:
joint_payload["transferred_from_joint"] = measurement_source
joints[name] = joint_payload
for name in PASSIVE_JOINTS:
metadata = source[name]
source_name = MIMIC_SOURCE_BY_JOINT[name]
motor_index = COMMAND_INDEX_BY_JOINT[source_name]
measurement_source = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
name, name
)
if (
name in MEASURED_PASSIVE_JOINTS
or name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT
):
fit = result.curves[measurement_source]
decreasing = np.asarray(fit.decreasing_rad, dtype=float)
increasing = np.asarray(fit.increasing_rad, dtype=float)
curve = 0.5 * (decreasing + increasing)
curve -= float(curve[255])
decreasing = decreasing - float(decreasing[255])
increasing = increasing - float(increasing[255])
offset = float(metadata["offset"])
curve += offset
decreasing += offset
increasing += offset
coupling = result.mimic_fits[measurement_source]
coupling_model = coupling.model
coefficients = list(coupling.mujoco_polycoef)
coefficients[0] = offset
urdf_mimic_multiplier = coupling.urdf_mimic_multiplier
urdf_mimic_policy = coupling.urdf_mimic_policy
else:
multiplier = float(metadata["multiplier"])
offset = float(metadata["offset"])
curve = offset + multiplier * curves[source_name]
decreasing = curve.copy()
increasing = curve.copy()
coupling_model = "linear_mimic"
coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0]
urdf_mimic_multiplier = multiplier
urdf_mimic_policy = "cad_nominal"
joint_payload = {
"urdf_joint": name,
"sdk_channel": COMMAND_NAMES[motor_index],
"motor_index": motor_index,
"passive": True,
"source_joint": source_name,
"mimic_offset_rad": round(float(metadata["offset"]), 8),
"coupling_model": coupling_model,
"coupling_coefficients": [
round(float(value), 10) for value in coefficients
],
"urdf_mimic_enabled": True,
"urdf_mimic_policy": urdf_mimic_policy,
"mimic_multiplier": round(float(urdf_mimic_multiplier), 8),
"calibration_status": profile.joint_coverage[name],
"zero_command_u8": 255,
"zero_angles": {"policy": "cad_static"},
"angle_rad": _rounded_curve(curve),
"decreasing_rad": _rounded_curve(decreasing),
"increasing_rad": _rounded_curve(increasing),
}
joints[name] = joint_payload
if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT:
joint_payload["transferred_from_joint"] = measurement_source
errors = np.abs(
np.concatenate(
[np.asarray(values, dtype=float) for values in result.holdout_errors_rad.values()]
)
)
payload: dict[str, Any] = {
"schema_version": 6,
"profile_id": KEY.profile_id,
"layout_id": KEY.layout,
"model": "L6",
"side": "right",
"serial_number": str(serial_number),
"calibration_scope": "partial",
"publication_pointer": "latest_partial_passed",
"angle_unit": "rad",
"command_range": [0, 255],
"curve_input_domain": "feedback_u8",
"runtime_curve_policy": "direction_aware",
"command_names": list(COMMAND_NAMES),
"baseline_command_u8": [255] * 6,
"protected_inputs": dict(protected_inputs),
"joints": joints,
"quality": {
"passed": bool(passed),
"scope": "partial",
"validation_mae_rad": round(float(np.mean(errors)), 8),
"validation_p95_rad": round(float(np.percentile(errors, 95.0)), 8),
"validation_max_rad": round(float(np.max(errors)), 8),
"thumb_axis_zero": (
None
if result.thumb_zero_result is None
else {
"method": (
"hybrid_axis_geometry_cad_range_center"
if result.zero_fallback_reason_by_joint
else "g20_serial_axis_geometry"
),
"passed": bool(result.thumb_zero_result.passed),
"geometry_fallback_reasons": dict(
sorted(result.zero_fallback_reason_by_joint.items())
),
"axis_line_rms_m": round(
float(result.thumb_zero_result.axis_line_rms_m), 10
),
"offsets_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.direct_offsets_rad.items()
)
},
"cycle_offsets_rad": {
name: [round(float(value), 10) for value in values]
for name, values in sorted(
result.thumb_zero_result.cycle_offsets_rad.items()
)
},
"validation_error_by_joint_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.validation_error_by_joint_rad.items()
)
},
}
),
},
}
validate_l6_runtime_payload(payload)
return payload
def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
required_top = {
"schema_version", "profile_id", "layout_id", "model", "side",
"serial_number", "calibration_scope", "publication_pointer",
"angle_unit", "command_range", "curve_input_domain",
"runtime_curve_policy", "command_names", "baseline_command_u8",
"protected_inputs", "joints", "quality",
}
if set(payload) != required_top:
raise ValueError("schema v6 calibration has unexpected top-level fields")
if (
payload["schema_version"] != 6
or payload["profile_id"] != KEY.profile_id
or payload["model"] != "L6"
or payload["side"] != "right"
or payload["calibration_scope"] != "partial"
):
raise ValueError("schema v6 identity is invalid")
if payload["publication_pointer"] != "latest_partial_passed":
raise ValueError("L6 partial result has the wrong publication pointer")
if payload["curve_input_domain"] != "feedback_u8":
raise ValueError("schema v6 must be indexed by feedback_u8")
if payload["runtime_curve_policy"] != "direction_aware":
raise ValueError("schema v6 must retain both motion directions")
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
raise ValueError("schema v6 units are invalid")
if tuple(payload["command_names"]) != COMMAND_NAMES:
raise ValueError("schema v6 command channel order is invalid")
if payload["baseline_command_u8"] != [255] * 6:
raise ValueError("schema v6 baseline must be six open commands")
protected = payload["protected_inputs"]
expected_hashes = {
"source_urdf_sha256", "camera_extrinsics_sha256",
"calibration_config_sha256", "tag_config_sha256",
}
if not isinstance(protected, Mapping) or set(protected) != expected_hashes:
raise ValueError("schema v6 protected inputs are incomplete")
if any(
len(str(value)) != 64
or any(char not in "0123456789abcdef" for char in str(value))
for value in protected.values()
):
raise ValueError("schema v6 protected input hash is invalid")
joints = payload["joints"]
if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS:
raise ValueError("schema v6 must contain all 11 L6 revolute joints")
profile = build_typed_profile()
for name in ACTIVE_JOINTS + PASSIVE_JOINTS:
joint = joints[name]
motor = COMMAND_INDEX_BY_JOINT[
MIMIC_SOURCE_BY_JOINT.get(name, name)
]
if joint.get("urdf_joint") != name or int(joint.get("motor_index", -1)) != motor:
raise ValueError(f"{name} has an invalid URDF/SDK mapping")
if joint.get("sdk_channel") != COMMAND_NAMES[motor]:
raise ValueError(f"{name} has an invalid SDK channel")
if joint.get("calibration_status") != profile.joint_coverage[name]:
raise ValueError(f"{name} has an invalid coverage status")
if joint.get("passive") is not (name in PASSIVE_JOINTS):
raise ValueError(f"{name} passive flag is invalid")
if joint.get("zero_command_u8") != 255:
raise ValueError(f"{name} zero command must be 255")
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
curve = np.asarray(joint.get(field), dtype=float)
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
raise ValueError(f"{name}.{field} must contain 256 finite values")
if np.any(np.diff(curve) > 1.0e-7):
raise ValueError(f"{name}.{field} must be non-increasing")
if name in PASSIVE_JOINTS:
if joint.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name]:
raise ValueError(f"{name} mimic source is invalid")
model = str(joint.get("coupling_model", ""))
expected_model = COUPLING_MODEL_BY_JOINT.get(
name, "linear_mimic"
)
if model != expected_model:
raise ValueError(f"{name} coupling model is invalid")
coefficients = np.asarray(
joint.get("coupling_coefficients"), dtype=float
)
if coefficients.shape != (6,) or not np.all(
np.isfinite(coefficients)
):
raise ValueError(f"{name} coupling coefficients are invalid")
enabled = joint.get("urdf_mimic_enabled")
if enabled is not True:
raise ValueError(f"{name} URDF mimic policy is invalid")
expected_policy = (
"endpoint_linear_fallback"
if model == "quadratic_runtime"
else "exact_linear"
if name in MEASURED_PASSIVE_JOINTS
else "cad_nominal"
)
# Early schema-v6 linear artifacts predate the explicit policy
# label; their unambiguous model/coverage combination remains
# readable. New writers always materialize the field.
policy = str(
joint.get("urdf_mimic_policy", expected_policy)
)
if policy != expected_policy:
raise ValueError(f"{name} URDF mimic fallback is invalid")
multiplier = float(joint.get("mimic_multiplier", "nan"))
if not math.isfinite(multiplier) or multiplier <= 0.0:
raise ValueError(f"{name} mimic multiplier is invalid")
if model == "linear_mimic":
if abs(multiplier - coefficients[1]) > 1.0e-7:
raise ValueError(f"{name} mimic multiplier is inconsistent")
if np.any(np.abs(coefficients[2:]) > 1.0e-10):
raise ValueError(f"{name} linear mimic is not linear")
if abs(float(coefficients[0]) - float(
joint.get("mimic_offset_rad", "nan")
)) > 1.0e-7:
raise ValueError(f"{name} coupling offset is inconsistent")
transferred_from = (
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name)
or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name)
)
if transferred_from is not None:
if joint.get("transferred_from_joint") != transferred_from:
raise ValueError(f"{name} transfer provenance is invalid")
donor = joints[transferred_from]
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
if joint[field] != donor[field]:
raise ValueError(f"{name} transfer curve differs from donor")
quality = payload["quality"]
if not isinstance(quality, Mapping) or quality.get("scope") != "partial":
raise ValueError("schema v6 quality scope must be partial")
if quality.get("passed") is not True:
raise ValueError("schema v6 quality.passed must be true")
def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> Path:
destination = Path(path).resolve()
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_name(f".{destination.name}.{os.getpid()}.tmp")
try:
with temporary.open("w", encoding="utf-8") as stream:
json.dump(payload, stream, ensure_ascii=False, indent=2, sort_keys=True)
stream.write("\n")
stream.flush()
os.fsync(stream.fileno())
os.replace(temporary, destination)
finally:
if temporary.exists():
temporary.unlink()
return destination
def publish_partial_session(serial_root: str | Path, session: str | Path) -> Path:
parent = Path(serial_root).resolve()
target = Path(session).resolve()
if target.parent != parent or not target.is_dir():
raise ValueError("partial session must be a direct existing child")
destination = parent / "latest_partial_passed"
temporary = parent / f".latest_partial_passed.{os.getpid()}.tmp"
if temporary.exists() or temporary.is_symlink():
temporary.unlink()
os.symlink(target.name, temporary, target_is_directory=True)
os.replace(temporary, destination)
return destination
def artifact_hashes(json_path: str | Path, urdf_path: str | Path) -> dict[str, str]:
return {
"calibration_json_sha256": _sha256_file(json_path),
"corrected_urdf_sha256": _sha256_file(urdf_path),
}
__all__ = [
"ALL_REVOLUTE_JOINTS",
"artifact_hashes",
"atomic_write_json",
"build_l6_runtime_payload",
"publish_partial_session",
"validate_l6_runtime_payload",
]
@@ -0,0 +1,708 @@
"""L6 curve, endpoint-zero, holdout, and mimic fitting."""
from __future__ import annotations
from dataclasses import dataclass
import math
from pathlib import Path
from typing import Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from scipy.optimize import least_squares
from ..g20.profile import (
HandCalibrationProfile,
JointCurveFit,
JointSpec,
)
from ..g20.zero_solver import (
ZeroCalibrationProfile,
ZeroSolveResult,
fit_joint_axis_measurement,
fit_rotation_joint_curve,
rotation_curve_holdout_errors,
solve_urdf_zero_offsets,
with_depth_free_axis_projection,
)
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
COMMAND_INDEX_BY_JOINT,
COMMAND_NAMES,
COUPLING_MODEL_BY_JOINT,
ENDPOINT_ANCHOR_BY_JOINT,
KEY,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
)
@dataclass(frozen=True)
class MimicFit:
source_joint: str
target_joint: str
model: str
coefficients: tuple[float, ...]
urdf_mimic_multiplier: float
urdf_mimic_policy: str
cycle_coefficients: tuple[tuple[float, ...], ...]
maximum_cycle_range: float
maximum_cycle_prediction_range_rad: float
residual_rms_rad: float
residual_p95_rad: float
residual_max_rad: float
@property
def multiplier(self) -> float:
"""Linear term retained for compatible diagnostics and artifacts."""
return float(self.coefficients[0])
@property
def cycle_multipliers(self) -> tuple[float, ...]:
return tuple(float(values[0]) for values in self.cycle_coefficients)
@property
def mujoco_polycoef(self) -> tuple[float, ...]:
"""MuJoCo q_target = p0 + p1*q_source + ... coefficients."""
return (0.0, *self.coefficients, *(0.0,) * (5 - len(self.coefficients)))
@dataclass(frozen=True)
class L6FitResult:
curves: Mapping[str, JointCurveFit]
zero_offsets_rad: Mapping[str, float]
travels_rad: Mapping[str, float]
mimic_fits: Mapping[str, MimicFit]
holdout_errors_rad: Mapping[str, tuple[float, ...]]
zero_method_by_joint: Mapping[str, str]
zero_fallback_reason_by_joint: Mapping[str, str]
thumb_zero_result: ZeroSolveResult | None = None
L6_THUMB_AXIS_JOINTS: tuple[str, ...] = (
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
"rh_thumb_dip",
"rh_pinky_mcp_pitch",
)
def _l6_thumb_zero_profile() -> ZeroCalibrationProfile:
"""Describe the observable L6 thumb/palm axis graph to the G20 solver."""
joint_specs = {
"rh_thumb_cmc_roll": JointSpec(
"rh_thumb_cmc_roll", 1, True, "top", "top_base", "thumb_roll"
),
"rh_thumb_cmc_pitch": JointSpec(
"rh_thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_pitch"
),
"rh_thumb_dip": JointSpec(
"rh_thumb_dip", 0, False, "front", "thumb_pitch", "thumb_dip"
),
# This root axis fixes the palm-frame phase around the thumb roll axis.
# Its electrical zero is irrelevant because rotating a revolute joint
# does not change its own physical screw axis.
"rh_pinky_mcp_pitch": JointSpec(
"rh_pinky_mcp_pitch", 5, True, "side", "side_base", "pinky_pitch"
),
}
hand = HandCalibrationProfile(
side="right",
reference_finger="pinky",
view_tags={},
preflight_view_roles={},
joint_specs=joint_specs,
sweep_specs=(),
image_trajectory_joints=frozenset(),
roll_clearance_commands={},
thumb_pitch_clearance_commands={},
layout_id=KEY.layout,
model="L6",
command_names=COMMAND_NAMES,
baseline_command=(255,) * 6,
directional_zero=True,
isolated_holdout=True,
)
return ZeroCalibrationProfile(
hand=hand,
direct_zero_joints=(
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
),
axis_joints=L6_THUMB_AXIS_JOINTS,
inherited_zero_joints={},
inherited_static_zero_joints={},
constrained_circle_joints=frozenset(L6_THUMB_AXIS_JOINTS),
root_anchor_joints=frozenset({"rh_thumb_cmc_roll"}),
axis_parent_joint={
"rh_thumb_cmc_pitch": "rh_thumb_cmc_roll",
},
phase_parent_joint={
"rh_thumb_dip": "rh_thumb_cmc_pitch",
},
offset_observer_joint={
"rh_thumb_cmc_roll": "rh_thumb_cmc_pitch",
"rh_thumb_cmc_pitch": "rh_thumb_dip",
},
same_view_axis_pair_by_offset={},
fixed_direct_zero_offsets_rad={},
static_output_zero_offsets_rad={},
base_pose_strategy="thumb_serial",
orientation_anchor_joint="rh_pinky_mcp_pitch",
)
def curve_travel_rad(fit: JointCurveFit) -> float:
decreasing = float(fit.decreasing_rad[0] - fit.decreasing_rad[255])
increasing = float(fit.increasing_rad[0] - fit.increasing_rad[255])
travel = 0.5 * (decreasing + increasing)
if not math.isfinite(travel) or travel <= 0.0:
raise ValueError("L6 fitted travel must be finite and positive")
return travel
def derive_endpoint_zero_offsets(
source_urdf: str | Path,
curves: Mapping[str, JointCurveFit],
*,
endpoint_anchor_by_joint: Mapping[str, str] | None = None,
maximum_offset_rad: float = math.radians(15.0),
) -> tuple[dict[str, float], dict[str, float]]:
"""Anchor each measured joint to its profile-selected CAD endpoint.
Every published dynamic curve is zero at feedback 255. ``upper_at_end``
rotates the static frame by ``source_upper - measured_travel`` so feedback
0 lands on the CAD upper endpoint. ``lower_at_start`` rotates it by the
source lower value so feedback 255 lands on the CAD lower endpoint.
``cad_range_center`` splits a source-vs-measured travel discrepancy equally
between the two endpoints when neither source endpoint is a trusted datum.
``zero_at_start`` keeps the CAD joint frame itself at feedback 255. All
policies publish the normalized corrected coordinate [0, measured travel].
"""
joints = {
str(joint.get("name")): joint
for joint in ET.parse(Path(source_urdf)).getroot().findall("joint")
}
offsets: dict[str, float] = {}
travels: dict[str, float] = {}
anchors = endpoint_anchor_by_joint or dict(ENDPOINT_ANCHOR_BY_JOINT)
if not anchors or not set(anchors).issubset(CALIBRATED_ACTIVE_JOINTS):
raise ValueError("L6 endpoint anchors must target measured active joints")
for name in sorted(CALIBRATED_ACTIVE_JOINTS):
if name not in curves:
raise ValueError(f"missing measured L6 curve: {name}")
joint = joints.get(name)
limit = None if joint is None else joint.find("limit")
if (
limit is None
or limit.get("upper") is None
or limit.get("lower") is None
):
raise ValueError(f"source URDF joint has incomplete limits: {name}")
travel = curve_travel_rad(curves[name])
travels[name] = travel
if name not in anchors:
continue
policy = str(anchors[name])
if policy == "upper_at_end":
offset = float(limit.get("upper")) - travel
elif policy == "lower_at_start":
offset = float(limit.get("lower"))
elif policy == "cad_range_center":
offset = 0.5 * (
float(limit.get("lower"))
+ float(limit.get("upper"))
- travel
)
elif policy == "zero_at_start":
offset = 0.0
else:
raise ValueError(f"unsupported L6 endpoint anchor: {policy}")
if not math.isfinite(offset) or abs(offset) > maximum_offset_rad:
raise ValueError(
f"{name} endpoint zero offset exceeds 15 degrees: "
f"{math.degrees(offset):.3f}"
)
offsets[name] = offset
return offsets, travels
def _has_complete_axis_geometry(
records_by_joint: Mapping[str, Sequence[Mapping[str, object]]],
) -> bool:
required = {
"relative_translation_xyz_m",
"parent_pose_common",
"child_pose_common",
"view_normal_common_xyz",
"camera_center_common_xyz_m",
"state_u8",
}
return all(
rows and all(required.issubset(row) for row in rows)
for name in L6_THUMB_AXIS_JOINTS
for rows in (records_by_joint.get(name, ()),)
)
def _fit_l6_thumb_axis_zero(
source_urdf: str | Path,
records_by_joint: Mapping[str, Sequence[Mapping[str, object]]],
curves: Mapping[str, JointCurveFit],
*,
fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None,
require_passed: bool = True,
) -> ZeroSolveResult:
"""Recover thumb roll/pitch zeros from four physical screw axes."""
profile = _l6_thumb_zero_profile()
measurements = []
by_key: dict[tuple[str, int], object] = {}
for cycle in range(4):
# Fit the two root/reference axes before the serial passive observer.
for name in (
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
"rh_pinky_mcp_pitch",
):
rows = records_by_joint[name]
view_normal = rows[0]["view_normal_common_xyz"]
measurement = fit_joint_axis_measurement(
name,
rows,
cycle=cycle,
zero_command_u8=255,
constrained_circle_joints=profile.constrained_circle_joints,
view_normal_common_xyz=view_normal,
canonical_zero_direction="decreasing",
)
measurement = with_depth_free_axis_projection(
measurement,
rows[0]["camera_center_common_xyz_m"],
)
measurements.append(measurement)
by_key[(name, cycle)] = measurement
dip_rows = records_by_joint["rh_thumb_dip"]
pitch_axis = by_key[("rh_thumb_cmc_pitch", cycle)]
dip = fit_joint_axis_measurement(
"rh_thumb_dip",
dip_rows,
cycle=cycle,
zero_command_u8=255,
axis_common_constraint=pitch_axis.axis_common_xyz,
constrained_circle_joints=profile.constrained_circle_joints,
view_normal_common_xyz=dip_rows[0]["view_normal_common_xyz"],
canonical_zero_direction="decreasing",
)
dip = with_depth_free_axis_projection(
dip,
dip_rows[0]["camera_center_common_xyz_m"],
)
measurements.append(dip)
result = solve_urdf_zero_offsets(
source_urdf=source_urdf,
measurements=measurements,
curves=curves,
motor_by_joint={
name: COMMAND_INDEX_BY_JOINT[
MIMIC_SOURCE_BY_JOINT.get(name, name)
]
for name in L6_THUMB_AXIS_JOINTS
},
training_cycles=(0, 1, 2),
validation_cycle=3,
maximum_offset_rad=math.radians(15.0),
finger_maximum_offset_rad=math.radians(15.0),
joint_maximum_offset_rad={
"rh_thumb_cmc_roll": math.radians(15.0),
"rh_thumb_cmc_pitch": math.radians(15.0),
},
maximum_cycle_difference_rad=math.radians(0.75),
minimum_applied_offset_rad=math.radians(0.1),
maximum_validation_mae_rad=math.radians(1.0),
maximum_validation_p95_rad=math.radians(2.0),
maximum_validation_error_rad=math.radians(3.0),
maximum_confidence_half_width_rad=math.radians(1.0),
maximum_pose_axis_line_rms_m=0.0015,
hand_type="right",
tag_layout=KEY.layout,
fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets_rad,
zero_profile=profile,
)
if require_passed and not result.passed:
details = ",".join(
f"{name}={reason}"
for name, reason in sorted(result.failure_reasons.items())
)
raise ValueError("L6 thumb axis zero solve failed:" + details)
return result
def _coupling_regression(
active: Sequence[float], passive: Sequence[float], *, degree: int
) -> tuple[tuple[float, ...], np.ndarray]:
x = np.asarray(active, dtype=float)
y = np.asarray(passive, dtype=float)
if x.shape != y.shape or x.ndim != 1 or x.size < 16:
raise ValueError("mimic curves must be aligned finite vectors")
if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)):
raise ValueError("mimic curves must be finite")
if degree not in {1, 2}:
raise ValueError("L6 coupling degree must be one or two")
if float(x @ x) <= 1.0e-9:
raise ValueError("active mimic source has insufficient travel")
design = np.column_stack([x ** power for power in range(1, degree + 1)])
initial = np.linalg.lstsq(design, y, rcond=None)[0]
scale = max(
math.radians(0.25),
float(np.median(np.abs(y - design @ initial))),
)
fitted = least_squares(
lambda value: y - design @ value,
np.asarray(initial, dtype=float),
loss="soft_l1",
f_scale=scale,
)
if not fitted.success or not np.all(np.isfinite(fitted.x)):
raise ValueError("robust through-origin mimic regression failed")
coefficients = tuple(float(value) for value in fitted.x)
grid = np.linspace(0.0, float(np.max(x)), 256)
derivative = np.full_like(grid, coefficients[0])
if degree == 2:
derivative += 2.0 * coefficients[1] * grid
if float(np.min(derivative)) < -1.0e-7:
raise ValueError("L6 coupling model is not monotonic")
return coefficients, y - design @ fitted.x
def fit_coupling_model(
source_joint: str,
target_joint: str,
active_fit: JointCurveFit,
passive_fit: JointCurveFit,
*,
model: str,
cycle_curve_pairs: Sequence[
tuple[Sequence[float], Sequence[float]]
] = (),
minimum_multiplier: float = 0.5,
maximum_multiplier: float = 1.5,
maximum_cycle_range: float = 0.03,
maximum_cycle_prediction_range_rad: float = math.radians(1.0),
maximum_residual_p95_rad: float = math.radians(2.0),
maximum_residual_rad: float = math.radians(3.0),
) -> MimicFit:
if model not in {"linear_mimic", "quadratic_runtime"}:
raise ValueError(f"unsupported L6 coupling model: {model}")
degree = 1 if model == "linear_mimic" else 2
active = np.concatenate(
(
np.asarray(active_fit.decreasing_rad, dtype=float),
np.asarray(active_fit.increasing_rad, dtype=float),
)
)
passive = np.concatenate(
(
np.asarray(passive_fit.decreasing_rad, dtype=float),
np.asarray(passive_fit.increasing_rad, dtype=float),
)
)
coefficients, residual = _coupling_regression(
active, passive, degree=degree
)
multiplier = coefficients[0]
if not minimum_multiplier <= multiplier <= maximum_multiplier:
raise ValueError(
f"{target_joint} coupling linear term is outside [0.5, 1.5]"
)
cycle_coefficients = tuple(
_coupling_regression(
active_cycle, passive_cycle, degree=degree
)[0]
for active_cycle, passive_cycle in cycle_curve_pairs
)
cycle_multipliers = tuple(values[0] for values in cycle_coefficients)
cycle_range = (
0.0
if len(cycle_multipliers) < 2
else float(max(cycle_multipliers) - min(cycle_multipliers))
)
if cycle_range > maximum_cycle_range:
raise ValueError(
f"{target_joint} coupling linear-term cycle range exceeds 0.03"
)
cycle_prediction_range = 0.0
if len(cycle_coefficients) >= 2:
grid = np.linspace(0.0, float(np.max(active)), 256)
predictions = np.asarray(
[
sum(value * grid ** (index + 1) for index, value in enumerate(values))
for values in cycle_coefficients
]
)
cycle_prediction_range = float(
np.max(np.ptp(predictions, axis=0))
)
if cycle_prediction_range > maximum_cycle_prediction_range_rad:
raise ValueError(
f"{target_joint} coupling cycle prediction range exceeds 1 degree"
)
absolute = np.abs(residual)
rms = float(np.sqrt(np.mean(np.square(residual))))
p95 = float(np.percentile(absolute, 95.0))
maximum = float(np.max(absolute))
if p95 > maximum_residual_p95_rad or maximum > maximum_residual_rad:
raise ValueError(
"coupling_residual_exceeds:"
f"joint={target_joint}:model={model}:"
f"linear_term={multiplier:.6f}:"
f"p95_deg={math.degrees(p95):.3f}:"
f"maximum_deg={math.degrees(maximum):.3f}:"
f"p95_limit_deg={math.degrees(maximum_residual_p95_rad):.3f}:"
f"maximum_limit_deg={math.degrees(maximum_residual_rad):.3f}"
)
return MimicFit(
source_joint=source_joint,
target_joint=target_joint,
model=model,
coefficients=coefficients,
# Standard URDF has only a linear mimic. For a nonlinear coupling,
# preserve the familiar editor/RViz linkage with a fallback line that
# is exact at both the open zero and measured closed endpoint. The
# direction-aware runtime curves and MuJoCo polynomial remain exact in
# between those endpoints.
urdf_mimic_multiplier=(
multiplier
if model == "linear_mimic"
else curve_travel_rad(passive_fit) / curve_travel_rad(active_fit)
),
urdf_mimic_policy=(
"exact_linear"
if model == "linear_mimic"
else "endpoint_linear_fallback"
),
cycle_coefficients=cycle_coefficients,
maximum_cycle_range=cycle_range,
maximum_cycle_prediction_range_rad=cycle_prediction_range,
residual_rms_rad=rms,
residual_p95_rad=p95,
residual_max_rad=maximum,
)
def fit_mimic_multiplier(
source_joint: str,
target_joint: str,
active_fit: JointCurveFit,
passive_fit: JointCurveFit,
*,
cycle_curve_pairs: Sequence[
tuple[Sequence[float], Sequence[float]]
] = (),
minimum_multiplier: float = 0.5,
maximum_multiplier: float = 1.5,
maximum_cycle_range: float = 0.03,
maximum_residual_p95_rad: float = math.radians(2.0),
maximum_residual_rad: float = math.radians(3.0),
) -> MimicFit:
return fit_coupling_model(
source_joint,
target_joint,
active_fit,
passive_fit,
model="linear_mimic",
cycle_curve_pairs=cycle_curve_pairs,
minimum_multiplier=minimum_multiplier,
maximum_multiplier=maximum_multiplier,
maximum_cycle_range=maximum_cycle_range,
maximum_residual_p95_rad=maximum_residual_p95_rad,
maximum_residual_rad=maximum_residual_rad,
)
def fit_l6_session(
source_urdf: str | Path,
records_by_joint: Mapping[str, Sequence[Mapping[str, object]]],
*,
require_thumb_axis_zero: bool = False,
) -> L6FitResult:
"""Fit three training cycles and validate the isolated fourth cycle."""
expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
if set(records_by_joint) != expected:
raise ValueError("L6 records must contain exactly five measured joints")
curves: dict[str, JointCurveFit] = {}
holdout: dict[str, tuple[float, ...]] = {}
cycle_curves: dict[str, dict[int, JointCurveFit]] = {}
for name in sorted(expected):
rows = [dict(row) for row in records_by_joint[name]]
training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}]
validation = [row for row in rows if int(row["cycle"]) == 3]
if not training or not validation:
raise ValueError(f"{name} is missing training or holdout records")
fit = fit_rotation_joint_curve(training, zero_command_u8=255)
errors = rotation_curve_holdout_errors(
fit, validation, zero_command_u8=255
)
absolute = np.abs(np.asarray(errors, dtype=float))
if (
float(np.mean(absolute)) > math.radians(1.0)
or float(np.percentile(absolute, 95.0)) > math.radians(2.0)
or float(np.max(absolute)) > math.radians(3.0)
):
raise ValueError(f"{name} isolated holdout failed")
correction_limit = math.radians(
3.0 if name in MEASURED_PASSIVE_JOINTS else 2.0
)
if fit.maximum_monotonic_correction_rad > correction_limit:
raise ValueError(f"{name} monotonic correction exceeds limit")
if fit.maximum_hysteresis_rad > math.radians(2.0):
raise ValueError(f"{name} hysteresis exceeds 2 degrees")
curves[name] = fit
holdout[name] = errors
cycle_curves[name] = {
cycle: fit_rotation_joint_curve(
[row for row in training if int(row["cycle"]) == cycle],
zero_command_u8=255,
)
for cycle in (0, 1, 2)
}
endpoint_offsets, travels = derive_endpoint_zero_offsets(
source_urdf,
curves,
endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT,
)
thumb_zero_result: ZeroSolveResult | None = None
offsets = {
"rh_thumb_cmc_roll": 0.0,
"rh_thumb_cmc_pitch": endpoint_offsets["rh_thumb_cmc_pitch"],
**endpoint_offsets,
}
pinky_endpoint_method = {
"upper_at_end": "mechanical_upper_endpoint",
"lower_at_start": "mechanical_lower_endpoint",
"cad_range_center": "cad_range_center",
"zero_at_start": "source_joint_zero",
}[ENDPOINT_ANCHOR_BY_JOINT["rh_pinky_mcp_pitch"]]
zero_methods = {
"rh_thumb_cmc_roll": "source_joint_zero_unpublished",
"rh_thumb_cmc_pitch": "cad_range_center_unpublished",
"rh_pinky_mcp_pitch": pinky_endpoint_method,
}
zero_fallback_reasons: dict[str, str] = {}
has_axis_geometry = _has_complete_axis_geometry(records_by_joint)
if require_thumb_axis_zero and not has_axis_geometry:
raise ValueError(
"L6 thumb absolute zero requires G20-compatible common-frame "
"Tag pose trajectories; this session must be reacquired"
)
if has_axis_geometry:
geometric_result = _fit_l6_thumb_axis_zero(
source_urdf,
records_by_joint,
curves,
require_passed=False,
)
thumb_zero_result = geometric_result
pitch_failure = geometric_result.failure_reasons.get(
"rh_thumb_cmc_pitch"
)
endpoint_fallback_reasons = {
"zero_offset_exceeds_configured_limit",
"zero_offset_reached_diagnostic_bound",
}
if (
not geometric_result.passed
and set(geometric_result.failure_reasons)
== {"rh_thumb_cmc_pitch"}
and pitch_failure in endpoint_fallback_reasons
):
# L6_RIGHT_001 demonstrated a stable pitch-to-DIP axis-line phase
# beyond the diagnostic search bound. That phase includes
# physical link geometry and is not a safe encoder-zero observation
# when it contradicts both the reviewed endpoint and the +/-15 deg
# write limit. Keep roll geometric, but freeze pitch to its
# independent measured/CAD range-centre datum. Any roll,
# holdout, cone, confidence, or multi-joint failure remains a hard
# rejection.
zero_fallback_reasons["rh_thumb_cmc_pitch"] = str(pitch_failure)
thumb_zero_result = _fit_l6_thumb_axis_zero(
source_urdf,
records_by_joint,
curves,
fixed_direct_zero_offsets_rad={
"rh_thumb_cmc_pitch": endpoint_offsets[
"rh_thumb_cmc_pitch"
]
},
)
elif not geometric_result.passed:
details = ",".join(
f"{name}={reason}"
for name, reason in sorted(
geometric_result.failure_reasons.items()
)
)
raise ValueError("L6 thumb axis zero solve failed:" + details)
offsets.update(
{
name: float(thumb_zero_result.direct_offsets_rad[name])
for name in (
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
)
}
)
zero_methods.update(
{
"rh_thumb_cmc_roll": "urdf_serial_axis_geometry",
"rh_thumb_cmc_pitch": (
"cad_range_center_after_geometry_rejection"
if "rh_thumb_cmc_pitch" in zero_fallback_reasons
else "urdf_serial_axis_geometry"
),
}
)
mimic_fits: dict[str, MimicFit] = {}
for target in sorted(MEASURED_PASSIVE_JOINTS):
source = MIMIC_SOURCE_BY_JOINT[target]
cycle_pairs = []
for cycle in (0, 1, 2):
active = cycle_curves[source][cycle]
passive = cycle_curves[target][cycle]
cycle_pairs.append(
(
tuple(active.decreasing_rad) + tuple(active.increasing_rad),
tuple(passive.decreasing_rad) + tuple(passive.increasing_rad),
)
)
mimic_fits[target] = fit_coupling_model(
source,
target,
curves[source],
curves[target],
model=COUPLING_MODEL_BY_JOINT[target],
cycle_curve_pairs=cycle_pairs,
)
return L6FitResult(
curves=curves,
zero_offsets_rad=offsets,
travels_rad=travels,
mimic_fits=mimic_fits,
holdout_errors_rad=holdout,
zero_method_by_joint=zero_methods,
zero_fallback_reason_by_joint=zero_fallback_reasons,
thumb_zero_result=thumb_zero_result,
)
__all__ = [
"L6FitResult",
"MimicFit",
"curve_travel_rad",
"derive_endpoint_zero_offsets",
"L6_THUMB_AXIS_JOINTS",
"fit_coupling_model",
"fit_l6_session",
"fit_mimic_multiplier",
]
@@ -0,0 +1,82 @@
"""Safe six-channel motion helpers for the partial L6 profile."""
from __future__ import annotations
import math
from typing import Sequence
from ...core import CalibrationProfile, TaskSpec
def cosine_position_trajectory_u8(
start_u8: float,
target_u8: float,
elapsed_seconds: float,
full_range_duration_seconds: float,
) -> tuple[float, float, float]:
"""Return a bounded, zero-end-velocity L6 command trajectory sample."""
duration = (
float(full_range_duration_seconds)
* abs(float(target_u8) - float(start_u8))
/ 255.0
)
if full_range_duration_seconds <= 0.0:
raise ValueError("full_range_duration_seconds must be positive")
if duration <= 0.0:
return float(target_u8), 1.0, 0.0
phase = min(1.0, max(0.0, float(elapsed_seconds) / duration))
blend = 0.5 - 0.5 * math.cos(math.pi * phase)
value = float(start_u8) + (float(target_u8) - float(start_u8)) * blend
return value, phase, duration
def build_calibration_motion_command(
task: TaskSpec,
command_u8: int,
*,
profile: CalibrationProfile,
) -> list[int]:
values = list(profile.command.baseline_u8)
for index, value in task.auxiliary_commands:
values[int(index)] = int(value)
values[int(task.command_index)] = int(command_u8)
return values
def build_calibration_preparation_waypoints(
task: TaskSpec,
*,
profile: CalibrationProfile,
current_command: Sequence[int] | None = None,
) -> tuple[tuple[int, ...], ...]:
del current_command
start = build_calibration_motion_command(
task, task.start_u8, profile=profile
)
return (tuple(start),)
def build_calibration_return_waypoints(
target_command: Sequence[int] | None = None,
*,
profile: CalibrationProfile,
current_command: Sequence[int] | None = None,
**_: object,
) -> tuple[tuple[int, ...], ...]:
del current_command
target = (
tuple(int(value) for value in target_command)
if target_command is not None
else tuple(profile.command.baseline_u8)
)
if len(target) != profile.command.command_count:
raise ValueError("L6 return command has the wrong channel count")
return (target,)
__all__ = [
"build_calibration_motion_command",
"build_calibration_preparation_waypoints",
"build_calibration_return_waypoints",
"cosine_position_trajectory_u8",
]
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,267 @@
"""Shared online/offline finalization path for one L6 partial session."""
from __future__ import annotations
from datetime import datetime
import json
import math
from pathlib import Path
from typing import Any, Mapping, Sequence
from .artifacts import (
artifact_hashes,
atomic_write_json,
build_l6_runtime_payload,
publish_partial_session,
)
from .fitting import L6FitResult, fit_l6_session
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
CORRECTED_PASSIVE_JOINTS,
MEASURED_PASSIVE_JOINTS,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
)
from .urdf import L6UrdfCorrection, write_l6_corrected_urdf
MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
ENDPOINT_SNAP_TOLERANCE_U8 = 2
def canonical_feedback_command_u8(feedback_u8: float) -> int:
"""Map a reached L6 feedback endpoint onto the curve's 0/255 domain."""
value = int(round(float(feedback_u8)))
if value <= ENDPOINT_SNAP_TOLERANCE_U8:
return 0
if value >= 255 - ENDPOINT_SNAP_TOLERANCE_U8:
return 255
return value
def accepted_records_by_joint(
records: Sequence[Mapping[str, Any]],
) -> dict[str, list[dict[str, Any]]]:
"""Select the newest attempt for every task/cycle/direction."""
samples = [
dict(row)
for row in records
if row.get("kind") == "l6_joint_sample"
and str(row.get("joint", "")) in MEASURED_JOINTS
]
latest_attempt: dict[tuple[str, int, str], int] = {}
for row in samples:
key = (
str(row["task_name"]),
int(row["cycle"]),
str(row["direction"]),
)
latest_attempt[key] = max(
latest_attempt.get(key, 0), int(row.get("attempt", 1))
)
result = {name: [] for name in MEASURED_JOINTS}
for row in samples:
key = (
str(row["task_name"]),
int(row["cycle"]),
str(row["direction"]),
)
if int(row.get("attempt", 1)) != latest_attempt[key]:
continue
accepted = {
"cycle": int(row["cycle"]),
"direction": str(row["direction"]),
# The reusable fitter calls the independent variable
# command_u8; schema v6 deliberately supplies measured SDK
# feedback here, never the requested controller set-point.
"command_u8": canonical_feedback_command_u8(
float(row["feedback_u8"])
),
"feedback_u8": float(row["feedback_u8"]),
"relative_quaternion_xyzw": list(
row["relative_quaternion_xyzw"]
),
}
# Schema-v6.1 adds the G20-compatible pose trajectory required for
# absolute thumb CMC zero recovery. Keep the projection here so the
# online and offline finalizers consume byte-equivalent fitting rows.
geometric_fields = (
"relative_translation_xyz_m",
"parent_pose_common",
"child_pose_common",
"view_normal_common_xyz",
"camera_center_common_xyz_m",
"state_u8",
)
if any(field in row for field in geometric_fields):
missing = [field for field in geometric_fields if field not in row]
if missing:
raise ValueError(
"L6 geometric sample is incomplete: " + ",".join(missing)
)
for field in geometric_fields:
value = row[field]
accepted[field] = (
dict(value) if isinstance(value, Mapping) else list(value)
)
result[str(row["joint"])].append(accepted)
return result
def load_l6_raw_samples(path: str | Path) -> list[dict[str, Any]]:
source = Path(path).expanduser().resolve()
if not source.is_file():
raise ValueError(f"raw L6 sample file does not exist: {source}")
rows: list[dict[str, Any]] = []
with source.open("r", encoding="utf-8") as stream:
for line_number, line in enumerate(stream, 1):
if not line.strip():
continue
try:
value = json.loads(line)
except json.JSONDecodeError as error:
raise ValueError(
f"invalid L6 JSONL record at line {line_number}"
) from error
if not isinstance(value, Mapping):
raise ValueError(f"L6 JSONL line {line_number} is not an object")
rows.append(dict(value))
return rows
def finalize_l6_session(
*,
session_dir: str | Path,
serial_number: str,
source_urdf: str | Path,
protected_inputs: Mapping[str, str],
records: Sequence[Mapping[str, Any]],
publish: bool = True,
timestamp: str | None = None,
) -> tuple[dict[str, Any], L6FitResult, L6UrdfCorrection]:
directory = Path(session_dir).expanduser().resolve()
directory.mkdir(parents=True, exist_ok=True)
result = fit_l6_session(
source_urdf,
accepted_records_by_joint(records),
require_thumb_axis_zero=True,
)
# Validate the complete runtime schema before materializing any corrected
# URDF. A fit/schema rejection therefore leaves only the node's failure
# diagnostic and the immutable raw samples.
payload = build_l6_runtime_payload(
serial_number=serial_number,
source_urdf=source_urdf,
result=result,
protected_inputs=protected_inputs,
passed=True,
)
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
correction = write_l6_corrected_urdf(
source_urdf=source_urdf,
output_directory=directory,
serial_number=serial_number,
result=result,
timestamp=stamp,
)
json_path = directory / f"l6_right_{serial_number}_partial_calibration.json"
atomic_write_json(json_path, payload)
summary = {
"schema_version": 1,
"profile_id": "L6/right/l6_right_8/v1",
"serial_number": str(serial_number),
"result": "PARTIAL_PASS",
"publication_pointer": "latest_partial_passed",
"calibrated_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS),
"active_zero_methods": dict(sorted(result.zero_method_by_joint.items())),
"active_zero_fallback_reasons": dict(
sorted(result.zero_fallback_reason_by_joint.items())
),
"thumb_axis_zero": (
None
if result.thumb_zero_result is None
else {
"offsets_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.direct_offsets_rad.items()
)
},
"axis_line_rms_m": round(
float(result.thumb_zero_result.axis_line_rms_m), 10
),
"validation_error_by_joint_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.validation_error_by_joint_rad.items()
)
},
}
),
"measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS),
"transferred_active_joints": dict(
sorted(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items())
),
"transferred_passive_joints": dict(
sorted(TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items())
),
"passive_coupling": {
name: {
"model": fit.model,
"transferred_from_joint": (
donor if donor != name else None
),
"coefficients": [
round(float(value), 10) for value in fit.coefficients
],
"urdf_mimic_enabled": True,
"urdf_mimic_multiplier": round(
float(fit.urdf_mimic_multiplier), 10
),
"urdf_mimic_policy": fit.urdf_mimic_policy,
"residual_p95_deg": round(
math.degrees(float(fit.residual_p95_rad)), 6
),
"residual_max_deg": round(
math.degrees(float(fit.residual_max_rad)), 6
),
"cycle_prediction_range_deg": round(
math.degrees(
float(fit.maximum_cycle_prediction_range_rad)
),
6,
),
}
for name, donor in sorted(
{
target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
target, target
)
for target in CORRECTED_PASSIVE_JOINTS
}.items()
)
for fit in (result.mimic_fits[donor],)
},
"explicit_runtime_joints": sorted(
correction.explicit_runtime_joints
),
"artifacts": {
"json": json_path.name,
"urdf": correction.path.name,
**artifact_hashes(json_path, correction.path),
},
}
atomic_write_json(directory / "calibration_summary_zh.json", summary)
if publish:
publish_partial_session(directory.parent, directory)
return payload, result, correction
__all__ = [
"ENDPOINT_SNAP_TOLERANCE_U8",
"MEASURED_JOINTS",
"accepted_records_by_joint",
"canonical_feedback_command_u8",
"finalize_l6_session",
"load_l6_raw_samples",
]
@@ -0,0 +1,392 @@
"""Reviewed partial-calibration profile for the right L6 eight-Tag rig."""
from __future__ import annotations
from ...core import (
ArtifactPolicy,
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
)
from ..registry import EngineBindings, RegisteredProfile
from .motion import (
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
)
KEY = ProfileKey("L6", "right", "l6_right_8", 1)
COMMAND_NAMES: tuple[str, ...] = (
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
)
ACTIVE_JOINTS: tuple[str, ...] = (
"rh_thumb_cmc_pitch",
"rh_thumb_cmc_roll",
"rh_index_mcp_pitch",
"rh_middle_mcp_pitch",
"rh_ring_mcp_pitch",
"rh_pinky_mcp_pitch",
)
PASSIVE_JOINTS: tuple[str, ...] = (
"rh_thumb_dip",
"rh_index_dip",
"rh_middle_dip",
"rh_ring_dip",
"rh_pinky_dip",
)
CALIBRATED_ACTIVE_JOINTS = frozenset(
{
"rh_thumb_cmc_pitch",
"rh_thumb_cmc_roll",
"rh_pinky_mcp_pitch",
}
)
MEASURED_PASSIVE_JOINTS = frozenset(
{"rh_thumb_dip", "rh_pinky_dip"}
)
# The four L6 fingers use the same six-channel mechanism. This profile has
# visual Tags only on the pinky, so the remaining three fingers deliberately
# inherit the pinky's measured travel, feedback curves, and passive coupling
# while retaining their own CAD frames and geometry. The shared MCP zero is
# anchored at the observed open endpoint; it is not inferred by forcing the
# measured closed travel back onto the shorter CAD upper limit.
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = {
"rh_index_mcp_pitch": "rh_pinky_mcp_pitch",
"rh_middle_mcp_pitch": "rh_pinky_mcp_pitch",
"rh_ring_mcp_pitch": "rh_pinky_mcp_pitch",
}
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT = {
"rh_index_dip": "rh_pinky_dip",
"rh_middle_dip": "rh_pinky_dip",
"rh_ring_dip": "rh_pinky_dip",
}
CORRECTED_ACTIVE_JOINTS = frozenset(
CALIBRATED_ACTIVE_JOINTS | TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.keys()
)
CORRECTED_PASSIVE_JOINTS = frozenset(
MEASURED_PASSIVE_JOINTS | TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.keys()
)
ENDPOINT_ANCHOR_BY_JOINT = {
# Thumb pitch normally uses serial-axis geometry. If that observation is
# incompatible with the source link geometry, align the centre of the
# measured physical range with the centre of the source CAD range. Real
# endpoint comparison showed lower_at_start slightly under-corrected and
# upper_at_end over-corrected this serial, so neither endpoint is an
# independently trustworthy absolute datum.
"rh_thumb_cmc_pitch": "cad_range_center",
# Feedback 255 is the repeatable open/lower endpoint. The measured travel
# is about 70.55 deg while the source CAD upper is 65 deg. Anchoring the
# closed endpoint therefore introduced a -5.55 deg offset into all four
# fingers and made intermediate pinch poses systematically under-flexed.
"rh_pinky_mcp_pitch": "lower_at_start",
}
COMMAND_INDEX_BY_JOINT = {
"rh_thumb_cmc_pitch": 0,
"rh_thumb_cmc_roll": 1,
"rh_index_mcp_pitch": 2,
"rh_middle_mcp_pitch": 3,
"rh_ring_mcp_pitch": 4,
"rh_pinky_mcp_pitch": 5,
}
MIMIC_SOURCE_BY_JOINT = {
"rh_thumb_dip": "rh_thumb_cmc_pitch",
"rh_index_dip": "rh_index_mcp_pitch",
"rh_middle_dip": "rh_middle_mcp_pitch",
"rh_ring_dip": "rh_ring_mcp_pitch",
"rh_pinky_dip": "rh_pinky_mcp_pitch",
}
COUPLING_MODEL_BY_JOINT = {
"rh_thumb_dip": "linear_mimic",
# The L6 pinky transmission has a repeatable changing ratio over its
# travel. Keep the exact direction-aware lookup at runtime and use the
# quadratic centre relation only for MuJoCo's equality constraint.
"rh_pinky_dip": "quadratic_runtime",
"rh_index_dip": "quadratic_runtime",
"rh_middle_dip": "quadratic_runtime",
"rh_ring_dip": "quadratic_runtime",
}
def build_typed_profile() -> CalibrationProfile:
active = frozenset(ACTIVE_JOINTS)
passive = frozenset(PASSIVE_JOINTS)
frozen_active = active - CALIBRATED_ACTIVE_JOINTS
measurements = {
"rh_thumb_cmc_roll": MeasurementSpec(
"rh_thumb_cmc_roll",
"relative_rotation",
"top",
"top_base",
"thumb_roll",
),
"rh_thumb_cmc_pitch": MeasurementSpec(
"rh_thumb_cmc_pitch",
"relative_rotation",
"front",
"front_base",
"thumb_pitch",
),
"rh_thumb_dip": MeasurementSpec(
"rh_thumb_dip",
"relative_rotation",
"front",
"thumb_pitch",
"thumb_dip",
),
"rh_pinky_mcp_pitch": MeasurementSpec(
"rh_pinky_mcp_pitch",
"relative_rotation",
"side",
"side_base",
"pinky_pitch",
),
"rh_pinky_dip": MeasurementSpec(
"rh_pinky_dip",
"relative_rotation",
"side",
"pinky_pitch",
"pinky_dip",
),
}
tasks = (
TaskSpec(
"thumb_roll_top",
"top",
1,
("rh_thumb_cmc_roll",),
auxiliary_commands=((0, 255),),
preflight_speed_u8=1,
formal_speed_u8=1,
),
TaskSpec(
"thumb_pitch_dip_front",
"front",
0,
("rh_thumb_cmc_pitch", "rh_thumb_dip"),
auxiliary_commands=((1, 255),),
preflight_speed_u8=1,
formal_speed_u8=1,
),
TaskSpec(
"pinky_pitch_dip_side",
"side",
5,
("rh_pinky_mcp_pitch", "rh_pinky_dip"),
preflight_speed_u8=1,
formal_speed_u8=1,
),
)
coverage = {
**{
name: (
"measured_static_dynamic"
if name in CALIBRATED_ACTIVE_JOINTS
else "transferred_static_dynamic"
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
else "cad_nominal"
)
for name in active
},
**{
name: (
"measured_dynamic_cad_static"
if name in MEASURED_PASSIVE_JOINTS
else "transferred_dynamic_cad_static"
if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT
else "mimic_nominal"
)
for name in passive
},
}
return CalibrationProfile(
key=KEY,
namespace="/l6_calibration",
command=CommandLayout(
names=COMMAND_NAMES,
baseline_u8=(255,) * 6,
command_index_by_joint=COMMAND_INDEX_BY_JOINT,
urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS},
feedback_name_aliases={"thumb_cmc_yaw": "thumb_cmc_roll"},
speed_slot_by_command_index={index: index for index in range(6)},
),
vision=VisionRigSpec(
views=(
ViewSpec(
"front",
(
TagSpec("front_base", 0, fixed_reference=True),
TagSpec("thumb_pitch", 1),
TagSpec("thumb_dip", 2),
),
),
ViewSpec(
"side",
(
TagSpec("side_base", 3, fixed_reference=True),
TagSpec("pinky_pitch", 4),
TagSpec("pinky_dip", 5),
),
),
ViewSpec(
"top",
(
TagSpec("top_base", 6, fixed_reference=True),
TagSpec("thumb_roll", 7),
),
),
),
common_frame="calibration_common",
extrinsic_reference_view="front",
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=True,
steady_command_checkpoints=True,
speed_parameters={
"preflight_u8": 1,
"formal_u8": 1,
"speed_settle_seconds": 0.2,
"command_trajectory_full_range_seconds": 6.0,
"torque_u8": 80,
"endpoint_hold_seconds": 1.0,
"stall_timeout_seconds": 2.0,
},
),
measurement=MeasurementPolicy(
measurements=measurements,
directional_zero=True,
),
zero=ZeroSolvePolicy(
active_joints=active,
passive_joints=passive,
direct_zero_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS)),
axis_joints=tuple(
sorted(CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS)
),
mechanical_endpoint_joints=frozenset(ENDPOINT_ANCHOR_BY_JOINT),
post_solve_endpoint_joints=frozenset(),
mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT,
cad_frozen_joints=passive,
endpoint_anchor_by_joint={
name: ENDPOINT_ANCHOR_BY_JOINT[name]
for name in ENDPOINT_ANCHOR_BY_JOINT
},
fitted_mimic_joints=MEASURED_PASSIVE_JOINTS,
coupling_model_by_joint=COUPLING_MODEL_BY_JOINT,
),
quality=QualityPolicy(
training_cycles=(0, 1, 2),
holdout_cycle=3,
hard_threshold_keys=frozenset(
{
"minimum_detection_rate",
"maximum_state_image_skew_ms",
"maximum_hysteresis_rad",
"maximum_validation_error_rad",
"maximum_mimic_residual_rad",
}
),
isolated_holdout=True,
),
scope=ScopePolicy(
calibrate_joints={"partial": CALIBRATED_ACTIVE_JOINTS},
frozen_joints={"partial": frozen_active},
default_scope="partial",
),
artifacts=ArtifactPolicy(
output_schema_version=6,
calibration_filename=(
"l6_right_{serial_number}_partial_calibration.json"
),
corrected_urdf_filename=(
"linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf"
),
protected_input_fields=frozenset(
{
"source_urdf_sha256",
"camera_extrinsics_sha256",
"calibration_config_sha256",
"tag_config_sha256",
}
),
publication_pointer="latest_partial_passed",
session_compatibility_tokens=frozenset(
{"l6_partial_v1", "feedback_curves_v6"}
),
publish_corrected_urdf=True,
),
joint_coverage=coverage,
)
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 build_profile() -> RegisteredProfile:
typed = build_typed_profile()
return RegisteredProfile(
profile=typed,
engine=EngineBindings(
hand_profile=typed,
zero_profile=typed.zero,
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,
),
)
__all__ = [
"ACTIVE_JOINTS",
"CALIBRATED_ACTIVE_JOINTS",
"COMMAND_NAMES",
"KEY",
"MEASURED_PASSIVE_JOINTS",
"MIMIC_SOURCE_BY_JOINT",
"PASSIVE_JOINTS",
"build_profile",
"build_typed_profile",
]
@@ -0,0 +1,622 @@
"""One-command online runner and deterministic offline replay for L6 right."""
from __future__ import annotations
import argparse
from datetime import datetime
import hashlib
import json
import os
from pathlib import Path
import re
import signal
import subprocess
import sys
import time
from typing import Any, Mapping
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger
from ...operator_report import (
ProgressEstimator,
render_compact_progress_header_zh,
)
from ...product import ProductConfig, load_product_config
from ...storage import atomic_write_json
from .pipeline import finalize_l6_session, load_l6_raw_samples
_STATE_LABELS = {
"WAIT_DEVICES": "等待六通道反馈和三相机内参",
"READY": "设备就绪",
"RUNNING": "标定中",
"PASSED": "通过",
"PAUSED": "已暂停",
"ABORTED": "已中止",
}
_TASK_LABELS = {
"thumb_roll_top": "拇指 CMC roll(上方机位,ID6→ID7)",
"thumb_pitch_dip_front": "拇指 CMC pitch / DIP(正面机位,ID0→ID1→ID2)",
"pinky_pitch_dip_side": "小指 MCP pitch / DIP(侧面机位,ID3→ID4→ID5)",
}
_PHASE_LABELS = {
"baseline": "安全恢复基准形态",
"preflight": "任务运动预检",
"prepare": "扫描起点准备",
"retry_prepare": "自动重扫起点准备",
"sweep": "正式扫描",
}
_DIRECTION_LABELS = {
"decreasing": "递减",
"increasing": "递增",
}
def _progress_fraction(status: Mapping[str, Any]) -> tuple[float, int, int]:
state = str(status.get("state", ""))
step_count = max(0, int(status.get("step_count", 0) or 0))
step_index = int(status.get("step_index", -1) or 0)
step_fraction = float(status.get("step_fraction", 0.0) or 0.0)
if state == "PASSED":
overall = 1.0
elif step_count and step_index >= 0:
overall = min(1.0, max(0.0, (step_index + step_fraction) / step_count))
else:
overall = 0.0
current_step = min(step_count, max(0, step_index + 1)) if step_count else 0
return overall, current_step, step_count
def _duration_zh(seconds: float | None) -> str:
if seconds is None or seconds < 0.0:
return "计算中"
value = int(round(seconds))
return f"{value // 60}分{value % 60:02d}秒"
def _l6_reason_zh(status: Mapping[str, Any]) -> tuple[str, str, str]:
reason = str(status.get("reason", "unknown"))
if reason.startswith("fixed_base_tag_moved:"):
fields = reason.split(":")
view = fields[1] if len(fields) > 1 else "unknown"
labels = {"front": "正面", "side": "侧面", "top": "上方"}
match = re.search(r"drift_px=([0-9.]+)", reason)
drift = match.group(1) if match else "未知"
limit = float(status.get("base_corner_drift_limit_px", 2.0) or 2.0)
return (
"OBS-BASE-DRIFT-105",
f"{labels.get(view, view)}机位的掌心固定基准 Tag 相对本方向扫描前"
f"锁定位置连续漂移,最大角点位移 {drift} px,超过 {limit:g} px。",
"检查手掌支架、相机和掌心基准 Tag 是否松动或被碰触;固定后重新开始。"
"程序已禁止发布本次结果。",
)
if reason.startswith("sweep_quality_failed:"):
fields = reason.split(":", 2)
details = fields[2] if len(fields) > 2 else "未提供明细"
return (
"OBS-SWEEP-QUALITY-104",
"当前方向经过自动重扫后仍未满足采集门限;具体未通过项:"
f"{details}。",
"查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate;先处理遮挡或"
"反馈采样问题,再重新开始。程序已禁止发布本次结果。",
)
if reason.startswith("non_target_motor_moved:"):
return (
"MOTION-NONTARGET-304",
"扫描期间检测到非目标电机离开保持位置。",
"停止其他控制节点并检查机械耦合或反馈通道顺序,确认后重新开始。",
)
if reason.startswith("multiple_state_publishers:"):
count = status.get("state_publisher_count", "?")
return (
"DEVICE-DUPLICATE-SDK-203",
f"检测到 {count} 个 L6 状态发布者;这通常表示已有 SDK/GUI 未退出。",
"先停止单独启动的 linker_hand_sdk 和 GUI,只保留本标定命令自动拉起的 SDK,"
"再重新开始。程序已禁止同时控制同一只手。",
)
if reason.startswith("multiple_command_publishers:"):
count = status.get("command_publisher_count", "?")
return (
"DEVICE-COMMAND-CONFLICT-204",
f"检测到 {count} 个 L6 控制命令发布者,标定节点之外还有程序在控制手。",
"停止 GUI、手动控制节点或其他标定进程,只保留当前标定命令后重新开始。",
)
if reason.startswith("mechanical_stall:"):
return (
"MOTION-STALL-303",
"目标电机连续两秒没有向目标推进,程序已保持当前位置。",
"检查碰撞、摩擦和机械端点;不要连续重启强推。",
)
if reason.startswith("motion_timeout:"):
return (
"MOTION-TIMEOUT-302",
"当前运动在规定时间内没有到达目标位置。",
"检查 CAN 反馈、电机状态和机械阻挡后重新开始。",
)
if reason.startswith("fit_or_publication_failed:"):
if (
"mimic_residual_exceeds:" in reason
or "coupling_residual_exceeds:" in reason
):
joint_match = re.search(r"joint=([^:]+)", reason)
model_match = re.search(r"model=([^:]+)", reason)
multiplier_match = re.search(
r"(?:multiplier|linear_term)=([0-9.]+)", reason
)
p95_match = re.search(r"p95_deg=([0-9.]+)", reason)
maximum_match = re.search(r"maximum_deg=([0-9.]+)", reason)
joint = joint_match.group(1) if joint_match else "未知关节"
model = model_match.group(1) if model_match else "linear_mimic"
multiplier = multiplier_match.group(1) if multiplier_match else "未知"
p95 = p95_match.group(1) if p95_match else "未知"
maximum = maximum_match.group(1) if maximum_match else "未知"
return (
"FIT-MIMIC-503",
f"{joint} 的 {model} 耦合模型未达到精度门限:"
f"线性项 {multiplier},"
f"残差 P95={p95}°、最大={maximum}°。",
"原始视觉曲线已保留;不要放宽门限或发布错误 URDF。请检查 Tag 刚性、"
"遮挡与机械重复性后重新采集。",
)
detail = reason.split(":", 1)[1] if ":" in reason else "未知"
return (
"FIT-PUBLISH-501",
f"采集完成后的拟合、质量验证或 URDF 安全写回失败:{detail}。",
"保留本会话,不要修改源 URDF;复制下方诊断块给开发者。",
)
if reason.startswith("operator_abort"):
return "OPERATOR-ABORT-001", "操作员主动中止了本次标定。", "排除现场问题后重新开始。"
return (
"L6-CAL-500",
"L6 标定因未分类保护条件停止。",
"保留会话目录和运行日志,并复制下方诊断块给开发者。",
)
def render_l6_progress_zh(
status: Mapping[str, Any],
estimator: ProgressEstimator | None = None,
) -> str:
"""Render L6 progress in the same operator-oriented layout as G20."""
state = str(status.get("state", ""))
overall, _current_step, _step_count = _progress_fraction(status)
task = status.get("task_name")
phase = status.get("phase")
if task:
task_text = _TASK_LABELS.get(str(task), str(task))
elif phase == "baseline":
task_text = "全手基准姿态"
elif state == "WAIT_DEVICES":
task_text = "等待设备连接"
elif state == "READY":
task_text = "等待开始"
else:
task_text = "无"
eta = _duration_zh(estimator.remaining(overall) if estimator else None)
cycle = status.get("cycle")
cycle_text = "-" if cycle is None else str(int(cycle) + 1)
direction = status.get("direction")
direction_text = _DIRECTION_LABELS.get(str(direction), "-")
target = status.get("target_u8")
requested = status.get("current_command_u8", target)
actual = status.get("actual_u8")
actual_text = "未知" if actual is None else f"{float(actual):.1f}"
valid = int(status.get("valid_frames", 0) or 0)
total = int(status.get("total_frames", 0) or 0)
rate = float(status.get("tag_detection_rate", 0.0) or 0.0)
joint_rate = float(
status.get(
"joint_frame_rate",
(float(valid) / total) if total else 0.0,
)
or 0.0
)
recognized = [int(value) for value in status.get("recognized_tag_ids", [])]
unrecognized = [
int(value) for value in status.get("unrecognized_tag_ids", [])
]
recognized_text = "/".join(f"ID{value}" for value in recognized) or "无"
unrecognized_text = "/".join(f"ID{value}" for value in unrecognized) or "无"
attempt = int(status.get("attempt", 1) or 1)
lines = render_compact_progress_header_zh(
serial_number=str(status.get("serial_number", "?")),
progress=overall,
eta=eta,
stage=_PHASE_LABELS.get(str(phase), _STATE_LABELS.get(state, state)),
cycle=cycle_text,
repetitions=4,
task=task_text,
requested=requested,
actual=actual_text,
direction=direction_text,
tag_status=(
f"已识别 {recognized_text};未识别/不合格 {unrecognized_text};"
f"本方向各Tag最低 {rate:.1%};联合 {valid}/{total} 帧"
f"({joint_rate:.1%})"
),
ready_cameras=3,
feedback_hz=float(status.get("feedback_hz", 0.0) or 0.0),
valid_frames=valid,
automatic_retry_count=max(0, attempt - 1),
)
if state in {"PAUSED", "ABORTED"}:
_code, problem, suggestion = _l6_reason_zh(status)
lines.extend((f"原因:{problem}", f"建议:{suggestion}"))
return "\n".join(lines)
class _ProgressConsole:
def __init__(self) -> None:
self.last_text = ""
self.estimator = ProgressEstimator.start()
def update(self, status: Mapping[str, Any]) -> None:
text = render_l6_progress_zh(status, self.estimator)
if text == self.last_text:
return
self.last_text = text
if sys.stdout.isatty():
sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
sys.stdout.flush()
else:
print(text, flush=True)
class _Monitor(Node):
def __init__(self, progress: _ProgressConsole) -> None:
super().__init__("l6_calibration_runner")
self.status: dict[str, Any] = {}
self.progress = progress
self.create_subscription(
String, "/l6_calibration/status", self._status_callback, 10
)
self.start_client = self.create_client(Trigger, "/l6_calibration/start")
self.abort_client = self.create_client(Trigger, "/l6_calibration/abort")
def _status_callback(self, message: String) -> None:
try:
value = json.loads(message.data)
except json.JSONDecodeError:
return
if isinstance(value, dict):
self.status = value
self.progress.update(value)
def _launch_command(
config: ProductConfig,
session: Path,
*,
record_bag: bool,
commands_enabled: bool,
) -> list[str]:
arguments = {
"model": config.model,
"hand_type": config.side,
"tag_layout": config.tag_layout,
"serial_number": config.serial_number,
"can_interface": config.can_interface,
"source_urdf_path": str(config.source_urdf),
"source_urdf_expected_sha256": config.source_urdf_sha256,
"camera_extrinsics_file": str(config.camera_extrinsics),
"camera_extrinsics_expected_sha256": config.camera_extrinsics_sha256,
"calibration_config": str(config.calibration_config),
"calibration_config_expected_sha256": config.calibration_config_sha256,
"tag_config": str(config.tag_config),
"tag_config_expected_sha256": config.tag_config_sha256,
"output_root": str(config.output_root),
"session_dir": str(session),
"corrected_urdf_output_dir": str(session),
"recalibration_scope": "partial",
"calibration_speed": "1",
"index_roll_calibration_speed": "1",
"index_flex_calibration_speed": "1",
"commands_enabled": str(commands_enabled).lower(),
"record_bag": str(record_bag).lower(),
}
for view, camera in config.cameras.items():
arguments[f"{view}_camera_serial"] = camera["serial_number"]
arguments[f"{view}_camera_name"] = camera["camera_name"]
arguments[f"{view}_camera_info_url"] = camera["camera_info"]
return [
"ros2", "launch", "linkerhand_calibration",
"three_camera_calibration.launch.py",
*(f"{name}:={value}" for name, value in arguments.items()),
]
def _wait_until(
monitor: _Monitor,
process: subprocess.Popen[Any],
predicate,
*,
timeout: float | None,
) -> bool:
started = time.monotonic()
while rclpy.ok():
if process.poll() is not None:
return False
rclpy.spin_once(monitor, timeout_sec=0.2)
if predicate(monitor.status):
return True
if timeout is not None and time.monotonic() - started > timeout:
return False
return False
def _stop_stack(process: subprocess.Popen[Any]) -> None:
"""Stop the whole launch process group without leaking child-node noise."""
if process.poll() is not None:
return
try:
os.killpg(process.pid, signal.SIGINT)
except ProcessLookupError:
return
try:
process.wait(timeout=15.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGTERM)
except ProcessLookupError:
return
try:
process.wait(timeout=5.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGKILL)
except ProcessLookupError:
return
process.wait(timeout=5.0)
def _l6_failure_report(
config: ProductConfig,
session: Path,
status: Mapping[str, Any],
log_path: Path,
) -> str:
code, problem, suggestion = _l6_reason_zh(status)
task_key = status.get("task_name")
task = "无" if task_key is None else _TASK_LABELS.get(str(task_key), str(task_key))
metrics = {
"task": task,
"cycle": status.get("cycle"),
"direction": status.get("direction"),
"valid_frames": status.get("valid_frames", 0),
"detection_frames": status.get("total_frames", 0),
"detection_rate": status.get("tag_detection_rate", 0.0),
"detection_rate_by_role": status.get("tag_detection_rate_by_role", {}),
"tag_seen_rate_by_role": status.get("tag_seen_rate_by_role", {}),
"recognized_tag_ids": status.get("recognized_tag_ids", []),
"unrecognized_tag_ids": status.get("unrecognized_tag_ids", []),
"joint_frame_rate": status.get("joint_frame_rate", 0.0),
"all_tags_quality_rate": status.get("all_tags_quality_rate", 0.0),
"pnp_valid_rate": status.get("pnp_valid_rate", 0.0),
"state_sync_rate": status.get("state_sync_rate", 0.0),
"observation_rejection_counts": status.get(
"observation_rejection_counts", {}
),
"actual_u8": status.get("actual_u8"),
"current_command_u8": status.get("current_command_u8"),
"base_corner_drift_px": status.get("base_corner_drift_px", {}),
"state_publisher_count": status.get("state_publisher_count"),
"command_publisher_count": status.get("command_publisher_count"),
"failure_reason": status.get("reason"),
}
payload = {
"schema_version": 1,
"profile_id": status.get("profile_id"),
"serial_number": config.serial_number,
"result": "FAIL",
"error_code": code,
"stage": status.get("phase")
or ("fit" if str(status.get("reason", "")).startswith(
"fit_or_publication_failed:"
) else status.get("state", "unknown")),
"reason": status.get("reason", "unknown"),
"problem_zh": problem,
"automatic_action_zh": "已停止运动、保持当前位置并禁止发布标定 JSON/URDF",
"suggestion_zh": suggestion,
"metrics": metrics,
"feedback_hz": status.get("feedback_hz", 0.0),
"hashes": {
"product_config_sha256": hashlib.sha256(
config.path.read_bytes()
).hexdigest(),
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"source_urdf_sha256": config.source_urdf_sha256,
},
"session_dir": str(session),
"log_path": str(log_path),
"quality": {"passed": False},
}
atomic_write_json(session / "calibration_summary_zh.json", payload)
return "\n".join(
[
"========== 请复制以下内容给开发者 ==========",
f"会话编号:{config.serial_number}_{session.name}",
"结果:FAIL",
f"错误代码:{code}",
f"失败阶段:{payload['stage']}",
f"问题:{problem}",
f"自动处理:{payload['automatic_action_zh']}",
"关键指标:"
+ json.dumps(metrics, ensure_ascii=False, separators=(",", ":")),
f"反馈状态:{float(payload['feedback_hz'] or 0.0):.1f} Hz",
f"配置哈希:{payload['hashes']['product_config_sha256']}",
f"外参哈希:{config.camera_extrinsics_sha256}",
f"源 URDF 哈希:{config.source_urdf_sha256}",
f"会话目录:{session}",
f"运行日志:{log_path}",
f"建议:{suggestion}",
"========== 复制结束 ==========",
]
)
def _run_online(
config: ProductConfig,
*,
record_bag: bool,
commands_enabled: bool,
) -> int:
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
while session.exists():
time.sleep(1.0)
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
session.mkdir(parents=True)
command = _launch_command(
config, session, record_bag=record_bag, commands_enabled=commands_enabled
)
log_path = session / "calibration.log"
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
process = subprocess.Popen(
command,
cwd=config.workspace,
stdout=log_stream,
stderr=subprocess.STDOUT,
text=True,
start_new_session=True,
)
rclpy.init()
monitor = _Monitor(_ProgressConsole())
try:
ready = _wait_until(
monitor,
process,
lambda status: status.get("state")
in {"READY", "PAUSED", "ABORTED"},
timeout=120.0,
)
if ready and monitor.status.get("state") in {"PAUSED", "ABORTED"}:
print(
_l6_failure_report(config, session, monitor.status, log_path),
flush=True,
)
return 3
if not ready:
print("L6 启动失败:120秒内未收到六通道反馈和三相机内参。", flush=True)
return 2
if not monitor.start_client.wait_for_service(timeout_sec=10.0):
print("L6 标定 /start 服务不可用。", flush=True)
return 2
future = monitor.start_client.call_async(Trigger.Request())
while rclpy.ok() and not future.done():
rclpy.spin_once(monitor, timeout_sec=0.2)
response = future.result()
if response is None or not response.success:
print(f"L6 标定未启动:{getattr(response, 'message', '')}", flush=True)
return 2
print(
"L6右手标定已启动:通道0/1/5,预检/正式速度1,"
"全行程6秒余弦缓入缓出,torque 80。",
flush=True,
)
finished = _wait_until(
monitor,
process,
lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"},
timeout=None,
)
if not finished:
print("L6 标定进程意外退出。", flush=True)
return 2
status = monitor.status
if status.get("state") != "PASSED":
print(_l6_failure_report(config, session, status, log_path), flush=True)
return 3
print(
"\n".join(
[
"PASS:L6右手三主动关节与两条DIP实测通过;"
"小指结果已迁移到食指、中指和无名指。",
f"部分结果:{config.session_root / 'latest_partial_passed'}",
f"JSON:{status.get('final_json')}",
f"URDF:{status.get('final_urdf')}",
]
),
flush=True,
)
return 0
except KeyboardInterrupt:
if monitor.abort_client.wait_for_service(timeout_sec=2.0):
monitor.abort_client.call_async(Trigger.Request())
rclpy.spin_once(monitor, timeout_sec=1.0)
return 130
finally:
monitor.destroy_node()
if rclpy.ok():
rclpy.shutdown()
_stop_stack(process)
log_stream.flush()
os.fsync(log_stream.fileno())
log_stream.close()
def main(args: list[str] | None = None) -> None:
parser = argparse.ArgumentParser(
description="L6 right partial three-camera calibration"
)
parser.add_argument("--config", required=True)
parser.add_argument("--workspace", default=None)
parser.add_argument("--record-bag", action="store_true")
parser.add_argument("--commands-disabled", action="store_true")
parser.add_argument("--validate-only", action="store_true")
parser.add_argument("--offline-raw", default="")
parser.add_argument("--offline-output", default="")
parser.add_argument("--publish-offline", action="store_true")
selected = parser.parse_args(args)
config = load_product_config(
selected.config,
workspace=selected.workspace,
check_can=not bool(selected.validate_only or selected.offline_raw),
)
if selected.validate_only:
print(
f"配置有效:{config.profile_key.profile_id},源URDF "
f"{config.source_urdf_sha256}",
flush=True,
)
return
if selected.offline_raw:
output = (
Path(selected.offline_output).expanduser().resolve()
if selected.offline_output
else config.session_root
/ (datetime.now().strftime("%Y%m%d_%H%M%S") + "_offline")
)
output.mkdir(parents=True, exist_ok=False)
payload, _fit, correction = finalize_l6_session(
session_dir=output,
serial_number=config.serial_number,
source_urdf=config.source_urdf,
protected_inputs={
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"tag_config_sha256": config.tag_config_sha256,
},
records=load_l6_raw_samples(selected.offline_raw),
publish=selected.publish_offline,
)
print(
f"离线回放PASS:schema {payload['schema_version']},URDF {correction.path}",
flush=True,
)
return
raise SystemExit(
_run_online(
config,
record_bag=selected.record_bag,
commands_enabled=not selected.commands_disabled,
)
)
__all__ = ["main", "render_l6_progress_zh"]
@@ -0,0 +1,342 @@
"""Minimal, auditable URDF correction for the partial L6 profile."""
from __future__ import annotations
from dataclasses import dataclass
from datetime import datetime
import math
import os
from pathlib import Path
import re
import shutil
from typing import Mapping
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation
from ..g20.zero_solver import _materialize_relative_mesh_assets
from .fitting import L6FitResult
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
CORRECTED_ACTIVE_JOINTS,
CORRECTED_PASSIVE_JOINTS,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
)
@dataclass(frozen=True)
class L6UrdfCorrection:
path: Path
origin_offsets_rad: Mapping[str, float]
corrected_limits_rad: Mapping[str, tuple[float, float]]
mimic_multipliers: Mapping[str, float]
coupling_polycoef: Mapping[str, tuple[float, ...]]
explicit_runtime_joints: frozenset[str]
def _triplet(value: str) -> np.ndarray:
result = np.asarray([float(item) for item in str(value).split()], dtype=float)
if result.shape != (3,) or not np.all(np.isfinite(result)):
raise ValueError(f"invalid URDF vector: {value}")
return result
def _replace_attribute(
block: str, element: str, attribute: str, value: str
) -> str:
pattern = re.compile(
rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])"
rf"(?P<value>[^\"']*)\2",
re.DOTALL,
)
match = pattern.search(block)
if match is None:
raise ValueError(f"{element} has no {attribute} attribute")
start, end = match.span("value")
return block[:start] + value + block[end:]
def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str:
origin = joint.find("origin")
if origin is None or origin.get("rpy") is None:
raise ValueError(f"joint {joint.get('name')} has no origin.rpy")
# Preserve the reviewed CAD spelling exactly when the calibrated open
# endpoint is the source joint zero. Apart from avoiding Euler round-off,
# this makes it explicit that a zero correction must not rotate the frame.
if abs(float(offset)) <= 1.0e-12:
return str(origin.get("rpy"))
axis_node = joint.find("axis")
axis = _triplet(
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
)
norm = float(np.linalg.norm(axis))
if norm <= 1.0e-12:
raise ValueError(f"joint {joint.get('name')} has a degenerate axis")
source = Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0")))
corrected = source * Rotation.from_rotvec(axis / norm * float(offset))
return " ".join(
f"{float(value):.15g}" for value in corrected.as_euler("xyz")
)
def _validate_passive_ranges(
joints: Mapping[str, ET.Element], result: L6FitResult
) -> None:
for target in sorted(CORRECTED_PASSIVE_JOINTS):
source = MIMIC_SOURCE_BY_JOINT[target]
measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(
source, source
)
measured_target = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
target, target
)
joint = joints[target]
mimic = joint.find("mimic")
limit = joint.find("limit")
if mimic is None or limit is None:
raise ValueError(f"passive joint is missing mimic or limit: {target}")
if mimic.get("joint") != source:
raise ValueError(f"passive source differs from profile: {target}")
offset = float(mimic.get("offset", "0"))
coupling = result.mimic_fits[measured_target]
travel = float(result.travels_rad[measured_source])
grid = np.linspace(0.0, travel, 256)
predicted = np.full_like(grid, offset)
for power, coefficient in enumerate(coupling.coefficients, 1):
predicted += float(coefficient) * grid ** power
lower = float(limit.get("lower", "-inf"))
upper = float(limit.get("upper", "inf"))
if (
float(np.min(predicted)) < lower - 1.0e-8
or float(np.max(predicted)) > upper + 1.0e-8
):
raise ValueError(
f"{target} fitted coupling range exceeds preserved passive limit"
)
measured = result.curves[measured_target]
observed = np.concatenate(
(
np.asarray(measured.decreasing_rad, dtype=float),
np.asarray(measured.increasing_rad, dtype=float),
)
) + offset
if float(np.min(observed)) < lower - 1.0e-8 or float(
np.max(observed)
) > upper + 1.0e-8:
raise ValueError(
f"{target} measured curve exceeds preserved passive limit"
)
def write_l6_corrected_urdf(
*,
source_urdf: str | Path,
output_directory: str | Path,
serial_number: str,
result: L6FitResult,
timestamp: str | None = None,
) -> L6UrdfCorrection:
source = Path(source_urdf).expanduser().resolve()
output = Path(output_directory).expanduser().resolve()
if not source.is_file():
raise ValueError(f"source URDF does not exist: {source}")
if "calibrated" in source.stem.lower():
raise ValueError("source URDF must be the immutable original CAD file")
if set(result.zero_offsets_rad) != CALIBRATED_ACTIVE_JOINTS:
raise ValueError("L6 zero result has the wrong active joint set")
if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS:
raise ValueError("L6 travel result has the wrong active joint set")
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
raise ValueError("L6 mimic result has the wrong passive joint set")
tree = ET.parse(source)
root = tree.getroot()
joints = {
str(joint.get("name")): joint for joint in root.findall("joint")
}
required = CORRECTED_ACTIVE_JOINTS | CORRECTED_PASSIVE_JOINTS
missing = required - set(joints)
if missing:
raise ValueError("source URDF is missing L6 targets: " + ",".join(sorted(missing)))
_validate_passive_ranges(joints, result)
active_replacements: dict[str, tuple[str, str, str]] = {}
for name in sorted(CORRECTED_ACTIVE_JOINTS):
measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
offset = float(result.zero_offsets_rad[measured_source])
travel = float(result.travels_rad[measured_source])
if not math.isfinite(offset) or abs(offset) > math.radians(15.0):
raise ValueError(f"invalid L6 zero offset: {name}")
if not math.isfinite(travel) or not 0.0 < travel < math.pi:
raise ValueError(f"invalid L6 travel: {name}")
active_replacements[name] = (
_corrected_origin_rpy(joints[name], offset),
"0",
f"{travel:.15g}",
)
mimic_replacements = {
name: f"{float(result.mimic_fits[donor].urdf_mimic_multiplier):.15g}"
for name, donor in sorted(
{
target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
target, target
)
for target in CORRECTED_PASSIVE_JOINTS
}.items()
)
}
explicit_runtime_joints = frozenset(
name for name in CORRECTED_PASSIVE_JOINTS
if result.mimic_fits[
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
].model == "quadratic_runtime"
)
coupling_polycoef = {
name: tuple(
float(value)
for value in result.mimic_fits[
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
].mujoco_polycoef
)
for name in sorted(CORRECTED_PASSIVE_JOINTS)
}
equality_replacements: dict[str, str] = {}
for equality in root.findall("./mujoco/equality/joint"):
target = str(equality.get("joint1", ""))
source_name = str(equality.get("joint2", ""))
if target not in coupling_polycoef:
continue
if MIMIC_SOURCE_BY_JOINT[target] != source_name:
raise ValueError(f"MuJoCo equality source differs for {target}")
coefficients = [
float(value) for value in str(equality.get("polycoef", "")).split()
]
if len(coefficients) != 6:
raise ValueError(f"MuJoCo equality polycoef is invalid for {target}")
offset = float(joints[target].find("mimic").get("offset", "0"))
coefficients = list(coupling_polycoef[target])
coefficients[0] = offset
equality_name = str(equality.get("name", ""))
if not equality_name:
raise ValueError(f"MuJoCo equality has no name for {target}")
equality_replacements[equality_name] = " ".join(
f"{value:.15g}" for value in coefficients
)
if len(equality_replacements) != len(coupling_polycoef):
raise ValueError("source URDF lacks a MuJoCo equality for fitted coupling")
original = source.read_text(encoding="utf-8")
# The file also contains MuJoCo/transmission ``<joint>`` elements. Requiring
# a URDF ``type`` attribute keeps the surgical block replacement confined
# to the eleven kinematic joints.
joint_pattern = re.compile(
r"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
r".*?</joint>",
re.DOTALL,
)
def replace_joint(match: re.Match[str]) -> str:
name = match.group("name")
block = match.group(0)
if name in active_replacements:
rpy, lower, upper = active_replacements[name]
block = _replace_attribute(block, "origin", "rpy", rpy)
block = _replace_attribute(block, "limit", "lower", lower)
block = _replace_attribute(block, "limit", "upper", upper)
if name in mimic_replacements:
block = _replace_attribute(
block, "mimic", "multiplier", mimic_replacements[name]
)
return block
corrected = joint_pattern.sub(replace_joint, original)
for equality_name, polycoef in equality_replacements.items():
equality_pattern = re.compile(
rf"(<joint\b[^>]*\bname\s*=\s*([\"'])"
rf"{re.escape(equality_name)}\2[^>]*>)",
re.DOTALL,
)
match = equality_pattern.search(corrected)
if match is None:
raise ValueError(f"could not locate MuJoCo equality {equality_name}")
replacement = _replace_attribute(
match.group(0), "joint", "polycoef", polycoef
)
corrected = corrected[: match.start()] + replacement + corrected[match.end() :]
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
if re.fullmatch(r"\d{8}_\d{6}", stamp) is None:
raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS")
safe_serial = "".join(
char if char.isalnum() or char in "_.-" else "_"
for char in str(serial_number)
)
if not safe_serial:
raise ValueError("serial number must not be empty")
output.mkdir(parents=True, exist_ok=True)
destination = output / (
f"{source.stem}_partial_zero_calibrated_{safe_serial}_{stamp}.urdf"
)
if destination.exists() or destination == source:
raise ValueError(f"refusing to overwrite URDF: {destination}")
_materialize_relative_mesh_assets(source=source, output=output, urdf_root=root)
# Keep the complete vendor mesh bundle beside the generated URDF, including
# auxiliary meshes not referenced by this particular XML revision.
source_meshes = source.parent / "meshes"
if source_meshes.is_dir():
destination_meshes = output / "meshes"
destination_meshes.mkdir(parents=True, exist_ok=True)
for mesh in source_meshes.iterdir():
if mesh.is_file():
shutil.copy2(mesh, destination_meshes / mesh.name)
temporary = destination.with_suffix(".urdf.tmp")
try:
with temporary.open("w", encoding="utf-8") as stream:
stream.write(corrected)
stream.flush()
os.fsync(stream.fileno())
os.replace(temporary, destination)
finally:
if temporary.exists():
temporary.unlink()
return L6UrdfCorrection(
path=destination,
origin_offsets_rad={
name: float(
result.zero_offsets_rad[
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
]
)
for name in sorted(CORRECTED_ACTIVE_JOINTS)
},
corrected_limits_rad={
name: (
0.0,
float(
result.travels_rad[
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
]
),
)
for name in sorted(CORRECTED_ACTIVE_JOINTS)
},
mimic_multipliers={
name: float(
result.mimic_fits[
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
].urdf_mimic_multiplier
)
for name in sorted(mimic_replacements)
},
coupling_polycoef=coupling_polycoef,
explicit_runtime_joints=explicit_runtime_joints,
)
__all__ = ["L6UrdfCorrection", "write_l6_corrected_urdf"]
@@ -0,0 +1,86 @@
"""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
from .l6 import register_profiles as register_l6_profiles
registry = ProfileRegistry()
register_profiles(registry)
register_l6_profiles(registry)
_DEFAULT_REGISTRY = registry
return _DEFAULT_REGISTRY
@@ -36,7 +36,7 @@ from .acquisition import (
interpolate_state_u8,
tag_quality_is_valid,
)
from .core import (
from .compat.legacy.thumb_core import (
BASELINE_COMMAND,
COMMAND_NAMES,
DIRECTION_DECREASING,
@@ -0,0 +1,7 @@
"""One-release module alias for the relocated model replay implementation."""
import sys
from .models.g20 import offline_replay as _implementation
sys.modules[__name__] = _implementation
@@ -0,0 +1,9 @@
"""Compatibility import for the generic registered-profile runner."""
from .runtime.runner import main
__all__ = ["main"]
if __name__ == "__main__":
main()
@@ -21,6 +21,37 @@ def _duration(seconds: float | None) -> str:
return f"{value // 60}分{value % 60:02d}秒"
def render_compact_progress_header_zh(
*,
serial_number: str,
progress: float,
eta: str,
stage: str,
cycle: object,
repetitions: object,
task: str,
requested: object,
actual: object,
direction: str,
tag_status: str,
ready_cameras: int,
feedback_hz: float,
valid_frames: int,
automatic_retry_count: int,
) -> list[str]:
"""Render the model-independent five-line operator progress header."""
return [
f"[{serial_number}] 标定中 {float(progress) * 100:5.1f}% "
f"预计剩余 {eta}",
f"阶段:{stage}(第 {cycle}/{repetitions} 轮)",
f"任务:{task} 命令/反馈:{requested}/{actual} 方向:{direction}",
f"Tag:{tag_status} 相机:{int(ready_cameras)}/3 正常 "
f"反馈:{float(feedback_hz):.1f} Hz",
f"质量:有效帧 {int(valid_frames)} 已自动重扫 "
f"{int(automatic_retry_count)} 次",
]
def _task_tag_id_status(views: Mapping[str, Any]) -> str:
"""Render every currently required Tag ID with its live visibility."""
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
@@ -273,13 +304,23 @@ def render_progress_zh(
== "waiting_for_task_tags_at_sweep_start"
else _duration(estimator.remaining(progress))
)
lines = [
f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}",
f"阶段:{stage}(第 {cycle}/{repetitions} 轮)",
f"任务:{joint} 命令/反馈:{requested}/{actual} 方向:{direction}",
f"Tag:{tag_status}{task_tag_ids} 相机:{ready_cameras}/3 正常 反馈:{feedback_hz:.1f} Hz",
f"质量:有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次",
]
lines = render_compact_progress_header_zh(
serial_number=serial_number,
progress=progress,
eta=eta,
stage=stage,
cycle=cycle,
repetitions=repetitions,
task=joint,
requested=requested,
actual=actual,
direction=direction,
tag_status=tag_status + task_tag_ids,
ready_cameras=ready_cameras,
feedback_hz=feedback_hz,
valid_frames=int(active.get("valid_frames", 0)),
automatic_retry_count=retry,
)
if str(status.get("reason", "")) == "waiting_for_task_tags_at_sweep_start":
pnp_wait = _pnp_wait_status(views)
if pnp_wait:
@@ -305,13 +346,24 @@ def render_progress_zh(
f"{fit_attempt_limit - 1} 次异常轮补采)"
)
resume = status.get("resume", {})
if isinstance(resume, Mapping) and resume.get("used"):
lines.append(
"断点:已恢复 "
f"{int(resume.get('completed_task_count', 0))}/"
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
"失败任务已丢弃并重新采集"
)
if isinstance(resume, Mapping):
position_policy = str(resume.get("start_position_policy", ""))
if resume.get("checkpoint_pending"):
lines.append(
"断点:正在建立本次起始位置基准,完成后再决定是否复用旧任务"
)
elif position_policy.startswith("discard_all_tasks_"):
lines.append(
"位置:检测到标定前机械手位置已变化或旧位置无法可靠验证;"
"未混用旧断点,本次整手从头采集"
)
elif resume.get("used"):
lines.append(
"断点:已恢复 "
f"{int(resume.get('completed_task_count', 0))}/"
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
"起始位置一致,失败任务已丢弃并重新采集"
)
return "\n".join(lines)
@@ -0,0 +1,3 @@
"""Compatibility import for the model-independent PnP implementation."""
from .core.geometry.pnp import * # noqa: F401,F403

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