O6gui预设动作修改、O12标定初始化提交

This commit is contained in:
lxp
2026-09-04 18:35:04 +08:00
parent 2356bd6247
commit a8ddcc6296
28 changed files with 2862 additions and 146 deletions
@@ -9,11 +9,21 @@ class HandConfig:
joint_names_en: Optional[List[str]] = None joint_names_en: Optional[List[str]] = None
init_pos: List[int] = field(default_factory=list) init_pos: List[int] = field(default_factory=list)
preset_actions: Optional[Dict[str, List[int]]] = None preset_actions: Optional[Dict[str, List[int]]] = None
preset_action_overrides: Optional[Dict[str, Dict[str, List[int]]]] = None
# Integer GUI values are divided by this scale before publishing. # Integer GUI values are divided by this scale before publishing.
# Legacy hands use raw u8 values (scale=1); O12 uses milliradians. # Legacy hands use raw u8 values (scale=1); O12 uses milliradians.
position_scale: int = 1 position_scale: int = 1
position_unit: str = "u8" position_unit: str = "u8"
def get_preset_actions(self, hand_type: str) -> Dict[str, List[int]]:
"""Return preset actions with hand-specific values applied."""
actions = dict(self.preset_actions or {})
if self.preset_action_overrides:
actions.update(
self.preset_action_overrides.get(hand_type.lower(), {})
)
return actions
# ------------------------------------------------------------------ # ------------------------------------------------------------------
# 常量字典(仅构建一次) # 常量字典(仅构建一次)
# ------------------------------------------------------------------ # ------------------------------------------------------------------
@@ -228,10 +238,17 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"": [92, 87, 255, 255, 255, 0], "": [92, 87, 255, 255, 255, 0],
"": [92, 87, 255, 255, 255, 255], "": [92, 87, 255, 255, 255, 255],
"": [255, 255, 255, 255, 255, 255], "": [255, 255, 255, 255, 255, 255],
"OK": [139, 91, 103, 250, 250, 250], "OK": [95, 75, 116, 255, 255, 255],
"拇指对中指": [88, 2, 255, 114, 255, 255],
"点赞": [250, 79, 0, 0, 0, 0], "点赞": [250, 79, 0, 0, 0, 0],
"握拳": [102, 18, 0, 0, 0, 0], "握拳": [102, 18, 0, 0, 0, 0],
} },
preset_action_overrides={
"right": {
"OK": [95, 83, 122, 255, 255, 255],
"拇指对中指": [95, 8, 255, 114, 255, 255],
},
},
), ),
"L6": HandConfig( "L6": HandConfig(
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"], joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
+9 -7
View File
@@ -363,6 +363,7 @@ class HandControlGUI(QWidget):
self.hand_joint = self.ros_manager.hand_joint self.hand_joint = self.ros_manager.hand_joint
self.hand_type = self.ros_manager.hand_type self.hand_type = self.ros_manager.hand_type
self.hand_config = _HAND_CONFIGS[self.hand_joint] self.hand_config = _HAND_CONFIGS[self.hand_joint]
self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
self.feedback_positions = None self.feedback_positions = None
# O12 only publishes after an explicit slider/preset action. This # O12 only publishes after an explicit slider/preset action. This
# prevents opening or closing the real hand merely by launching GUI. # prevents opening or closing the real hand merely by launching GUI.
@@ -636,9 +637,9 @@ class HandControlGUI(QWidget):
def create_system_preset_buttons(self, parent_layout): def create_system_preset_buttons(self, parent_layout):
"""创建系统预设动作按钮""" """创建系统预设动作按钮"""
self.preset_buttons = [] # 清空按钮列表 self.preset_buttons = [] # 清空按钮列表
if self.hand_config.preset_actions: if self.preset_actions:
buttons = [] buttons = []
for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()): for idx, (name, positions) in enumerate(self.preset_actions.items()):
button = QPushButton(name) button = QPushButton(name)
button.setProperty("category", "preset") button.setProperty("category", "preset")
button.clicked.connect( button.clicked.connect(
@@ -889,7 +890,7 @@ class HandControlGUI(QWidget):
def on_cycle_clicked(self): def on_cycle_clicked(self):
"""循环运行预设动作按钮点击事件处理""" """循环运行预设动作按钮点击事件处理"""
if not self.hand_config.preset_actions: if not self.preset_actions:
QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行") QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
return return
@@ -912,19 +913,19 @@ class HandControlGUI(QWidget):
def run_next_action(self): def run_next_action(self):
"""运行下一个预设动作""" """运行下一个预设动作"""
if not self.hand_config.preset_actions: if not self.preset_actions:
return return
# 重置所有按钮颜色 # 重置所有按钮颜色
self.reset_preset_buttons_color() self.reset_preset_buttons_color()
# 计算下一个动作索引 # 计算下一个动作索引
self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions) self.current_action_index = (self.current_action_index + 1) % len(self.preset_actions)
# 获取下一个动作 # 获取下一个动作
action_names = list(self.hand_config.preset_actions.keys()) action_names = list(self.preset_actions.keys())
action_name = action_names[self.current_action_index] action_name = action_names[self.current_action_index]
action_positions = self.hand_config.preset_actions[action_name] action_positions = self.preset_actions[action_name]
# 执行动作 # 执行动作
self.on_preset_action_clicked(action_positions) self.on_preset_action_clicked(action_positions)
@@ -949,6 +950,7 @@ class HandControlGUI(QWidget):
"""关节类型改变事件处理""" """关节类型改变事件处理"""
self.hand_joint = joint_type self.hand_joint = joint_type
self.hand_config = _HAND_CONFIGS[self.hand_joint] self.hand_config = _HAND_CONFIGS[self.hand_joint]
self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
# 更新手部信息 # 更新手部信息
info_text = f"""手部类型: {self.hand_type} info_text = f"""手部类型: {self.hand_type}
+18
View File
@@ -0,0 +1,18 @@
from gui_control.config.constants import HAND_CONFIGS
def test_o6_ok_preset_is_hand_specific() -> None:
config = HAND_CONFIGS["O6"]
assert config.get_preset_actions("left")["OK"] == [
95, 75, 116, 255, 255, 255,
]
assert config.get_preset_actions("right")["OK"] == [
95, 83, 122, 255, 255, 255,
]
assert config.get_preset_actions("left")["拇指对中指"] == [
88, 2, 255, 114, 255, 255,
]
assert config.get_preset_actions("right")["拇指对中指"] == [
95, 8, 255, 114, 255, 255,
]
+30
View File
@@ -1,5 +1,35 @@
# LinkerHand 专业标定包 # LinkerHand 专业标定包
## O12 右手 16-Tag 完整标定
O12 使用 vendor `omnihand_pro_2025_node` 的标准弧度接口,固定订阅
`/o12/right/joint_states`、发布 `/o12/right/joint_cmd`。标定程序不会使用
0–2000 混合控制接口,也不会按可能错位的 `JointState.name[]` 重排数据;12 路
`position[]` 顺序及 `thumb_roll → thumb_cmc_roll`
`thumb_abad → thumb_cmc_yaw` 映射由产品 Profile 固定。
把 [o12_right_product.yaml](config/o12_right_product.yaml) 中的
`serial_number` 改成实物串号后,只需运行:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/o12_right_product.yaml
```
runner 会自动加载仓库内 vendor Jazzy overlay,启动 HCAN device 0/channel 0
节点、三相机、AprilTag 与标定节点;READY 后自动开始。启动前会确认 12 路
POSITION 模式、错误码、温度和反馈,并以不超过 3° 的低速点动执行固定通道预检。
正式扫描以 20 Hz 发布端点速度为零的弧度余弦轨迹;中止、堵转或质量失败时保持
当前位置。结果发布到 `calibration_output/<O12串号>/latest_passed`,其中 JSON
为 schema v7 弧度 knots,不生成 256 点 u8 表。
只验证配置、16 张 16 mm Tag、相机/外参、源 URDF 和 SDK 配置哈希而不运动:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/o12_right_product.yaml --validate-only
```
## O6 右手局部标定(o6_right_8/v1 ## O6 右手局部标定(o6_right_8/v1
O6 使用与 L6 相同的三机位八 Tag 观测拓扑,但保留 O6 自己的六通道协议与 O6 使用与 L6 相同的三机位八 Tag 观测拓扑,但保留 O6 自己的六通道协议与
@@ -0,0 +1,41 @@
/o12_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, 3, 12, 13]
frames: [front_base, thumb_cmc, thumb_mcp, thumb_dip, middle_roll, index_roll]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
/o12_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: [4, 5, 6, 7, 8, 9, 10, 11]
frames: [side_base, pinky_mcp, pinky_pip, pinky_dip, middle_pip, middle_dip, index_pip, index_dip]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
/o12_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: [14, 15]
frames: [top_base, thumb_yaw]
sizes: [0.016, 0.016]
@@ -0,0 +1,43 @@
schema_version: 3
profile_id: O12/right/o12_right_16/v1
model: O12
side: right
tag_layout: o12_right_16
namespace: /o12_calibration
serial_number: O12_RIGHT_001
output_root: calibration_output
sdk:
driver: omnihand_pro_2025_node
transport: hcan
setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml
config_sha256: ca1791c822bf1e99f25db8806397c3f877100c9252fcce6c53a7a6b8cef7694a
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/o12_right/linkerhand_o12_t3_right-0703.urdf
source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml
calibration_config_sha256: 9defe39a3f4b31c3764cbdcd9ee5b570da70e810215844515e8cc01e56414728
tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml
tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
release:
required_independent_passes: 1
static_repeatability_deg: 1.0
@@ -0,0 +1,50 @@
o12_calibration:
ros__parameters:
command_topic: /o12/right/joint_cmd
state_topic: /o12/right/joint_states
front_camera_info_topic: /o12_calibration/front/camera/camera_info
front_detections_topic: /o12_calibration/front/apriltag/detections
side_camera_info_topic: /o12_calibration/side/camera/camera_info
side_detections_topic: /o12_calibration/side/apriltag/detections
top_camera_info_topic: /o12_calibration/top/camera/camera_info
top_detections_topic: /o12_calibration/top/apriltag/detections
# O12 standard interface is radians. Raw 0..2000 mixed control is forbidden.
command_rate_hz: 20.0
repetitions: 4
tag_size_m: 0.016
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
minimum_detection_rate: 0.95
minimum_joint_frame_rate: 0.85
minimum_feedback_hz: 15.0
maximum_state_image_skew_ms: 50.0
minimum_sweep_frames: 40
minimum_state_span_fraction: 0.90
minimum_sweep_bins: 32
maximum_bin_gap: 2
endpoint_tolerance_rad: 0.01
endpoint_hold_seconds: 1.0
motor_stall_timeout_seconds: 2.0
position_timeout_seconds: 60.0
sweep_timeout_seconds: 180.0
automatic_sweep_retry_limit: 2
non_target_motion_tolerance_rad: 0.015
maximum_temperature_c: 70
# 当前 O12 固件返回空温度报告。仍发起查询;5秒无结果后依赖已验证的
# joint_error_states bit1 过热保护,并在会话记录中明确标注降级。
temperature_report_required: false
temperature_fallback_after_seconds: 5.0
# 现场确认初始保守速度过慢;正式/点动速度提高到2倍,避让另有硬上限。
motion_speed_scale: 2.0
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
fixed_base_maximum_corner_drift_px: 2.0
fixed_base_movement_confirmation_frames: 5
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
@@ -73,6 +73,8 @@ def _launch_stack(context):
/ ( / (
"three_camera_tags_g20_right_19.yaml" "three_camera_tags_g20_right_19.yaml"
if tag_layout == "g20_right_19" if tag_layout == "g20_right_19"
else "o12_right_16_tags.yaml"
if tag_layout == "o12_right_16"
else "o6_right_8_tags.yaml" else "o6_right_8_tags.yaml"
if tag_layout == "o6_right_8" if tag_layout == "o6_right_8"
else "l6_right_8_tags.yaml" else "l6_right_8_tags.yaml"
@@ -85,8 +87,15 @@ def _launch_stack(context):
if not tag_config.is_file(): if not tag_config.is_file():
raise RuntimeError(f"tag config does not exist: {tag_config}") raise RuntimeError(f"tag config does not exist: {tag_config}")
topic_prefix = f"/{model.lower()}" topic_prefix = f"/{model.lower()}"
command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd" is_o12 = model == "O12"
state_topic = f"{topic_prefix}/cb_{hand_type}_hand_state" command_topic = (
f"{topic_prefix}/{hand_type}/joint_cmd"
if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
)
state_topic = (
f"{topic_prefix}/{hand_type}/joint_states"
if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_state"
)
info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info" info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info"
requested_source = LaunchConfiguration("source_urdf_path").perform(context) requested_source = LaunchConfiguration("source_urdf_path").perform(context)
source_urdf = ( source_urdf = (
@@ -250,14 +259,24 @@ def _launch_stack(context):
output="screen", output="screen",
emulate_tty=True, emulate_tty=True,
) )
sdk = Node( sdk = (
package="linker_hand_ros2_sdk", Node(
executable="linker_hand_sdk", package="omnihand_node",
name="linker_hand_sdk", executable="omnihand_pro_2025_node",
output="screen", name="omnihand_pro_2025_node",
condition=IfCondition(LaunchConfiguration("start_sdk")), namespace="o12",
parameters=[ output="screen",
{ condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[LaunchConfiguration("vendor_sdk_config")],
)
if is_o12
else Node(
package="linker_hand_ros2_sdk",
executable="linker_hand_sdk",
name="linker_hand_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[{
"hand_type": hand_type, "hand_type": hand_type,
"hand_joint": model, "hand_joint": model,
"can": LaunchConfiguration("can_interface"), "can": LaunchConfiguration("can_interface"),
@@ -281,8 +300,8 @@ def _launch_stack(context):
"defer_state_reads_while_commanding": False, "defer_state_reads_while_commanding": False,
"repeat_position_commands": False, "repeat_position_commands": False,
"is_touch": False, "is_touch": False,
} }],
], )
) )
calibration = Node( calibration = Node(
package="linkerhand_calibration", package="linkerhand_calibration",
@@ -331,6 +350,9 @@ def _launch_stack(context):
"tag_config_expected_sha256": LaunchConfiguration( "tag_config_expected_sha256": LaunchConfiguration(
"tag_config_expected_sha256" "tag_config_expected_sha256"
), ),
"sdk_config_expected_sha256": LaunchConfiguration(
"sdk_config_expected_sha256"
),
"corrected_urdf_output_dir": LaunchConfiguration( "corrected_urdf_output_dir": LaunchConfiguration(
"corrected_urdf_output_dir" "corrected_urdf_output_dir"
), ),
@@ -385,6 +407,14 @@ def _launch_stack(context):
command_topic, command_topic,
state_topic, state_topic,
info_topic, info_topic,
*(
[
"/o12/right/joint_control_mode_states",
"/o12/right/joint_error_states",
"/o12/right/joint_temperature_states",
]
if is_o12 else []
),
f"{calibration_namespace}/status", f"{calibration_namespace}/status",
], ],
output="screen", output="screen",
@@ -509,6 +539,10 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument( DeclareLaunchArgument(
"tag_config_expected_sha256", default_value="" "tag_config_expected_sha256", default_value=""
), ),
DeclareLaunchArgument("vendor_sdk_config", default_value=""),
DeclareLaunchArgument(
"sdk_config_expected_sha256", default_value=""
),
DeclareLaunchArgument( DeclareLaunchArgument(
"corrected_urdf_output_dir", default_value="" "corrected_urdf_output_dir", default_value=""
), ),
@@ -167,11 +167,16 @@ def interpolate_state_u8(
if nearest is None: if nearest is None:
return None return None
gap = abs(int(stamp_ns) - int(nearest.stamp_ns)) gap = abs(int(stamp_ns) - int(nearest.stamp_ns))
if gap > maximum_skew_ns or len(nearest.position_u8) != 20: if gap > maximum_skew_ns or not nearest.position_u8:
return None return None
return (tuple(float(value) for value in nearest.position_u8), gap) return (tuple(float(value) for value in nearest.position_u8), gap)
# Physical-angle profiles use the same timestamp interpolation. Keep the old
# public name for compatibility and offer a unit-neutral spelling to new code.
interpolate_state = interpolate_state_u8
class ContinuousSweepCollector: class ContinuousSweepCollector:
"""Collect timestamp-synchronised observations during one end-to-end move.""" """Collect timestamp-synchronised observations during one end-to-end move."""
@@ -17,6 +17,7 @@ import math
from pathlib import Path from pathlib import Path
from typing import Any, Mapping, Sequence from typing import Any, Mapping, Sequence
import numpy as np
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
from sensor_msgs.msg import JointState from sensor_msgs.msg import JointState
@@ -27,6 +28,7 @@ from .full_hand import (
validate_compact_payload, validate_compact_payload,
) )
from .models import get_default_registry, validate_schema_v6_runtime_payload from .models import get_default_registry, validate_schema_v6_runtime_payload
from .models.o12.artifacts import validate_o12_runtime_payload
from .core import ProfileKey from .core import ProfileKey
@@ -88,7 +90,9 @@ class CalibratedCommandMapper:
self, payload: Mapping[str, Any], *, expected_side: str | None = None self, payload: Mapping[str, Any], *, expected_side: str | None = None
) -> None: ) -> None:
schema_version = int(payload["schema_version"]) schema_version = int(payload["schema_version"])
if schema_version == 6: if schema_version == 7:
validate_o12_runtime_payload(payload)
elif schema_version == 6:
validate_schema_v6_runtime_payload(payload) validate_schema_v6_runtime_payload(payload)
else: else:
validate_compact_payload(payload) validate_compact_payload(payload)
@@ -103,7 +107,7 @@ class CalibratedCommandMapper:
raise ValueError("calibration quality.passed must be true") raise ValueError("calibration quality.passed must be true")
layout_id = ( layout_id = (
str(payload["layout_id"]) str(payload["layout_id"])
if schema_version == 6 if schema_version in {6, 7}
else infer_compact_payload_layout(payload) else infer_compact_payload_layout(payload)
) )
self.side = side self.side = side
@@ -119,9 +123,9 @@ class CalibratedCommandMapper:
"command_u8" if schema_version == 4 else "", "command_u8" if schema_version == 4 else "",
) )
) )
if self.input_domain not in {"command_u8", "feedback_u8"}: if self.input_domain not in {"command_u8", "feedback_u8", "feedback_rad"}:
raise ValueError("calibration curve_input_domain is invalid") raise ValueError("calibration curve_input_domain is invalid")
if schema_version == 6: if schema_version in {6, 7}:
self.command_names = tuple(str(value) for value in payload["command_names"]) 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.urdf_joint_names = tuple(str(name) for name in payload["joints"])
self._motor_by_joint = { self._motor_by_joint = {
@@ -134,6 +138,9 @@ class CalibratedCommandMapper:
self.feedback_name_aliases = dict( self.feedback_name_aliases = dict(
registered.profile.command.feedback_name_aliases registered.profile.command.feedback_name_aliases
) )
self.feedback_by_index = bool(
registered.profile.command.feedback_by_index
)
else: else:
profile = get_hand_calibration_profile(side, layout_id) profile = get_hand_calibration_profile(side, layout_id)
self.command_names = G20_COMMAND_NAMES self.command_names = G20_COMMAND_NAMES
@@ -143,6 +150,7 @@ class CalibratedCommandMapper:
for name in self.urdf_joint_names for name in self.urdf_joint_names
} }
self.feedback_name_aliases = {} self.feedback_name_aliases = {}
self.feedback_by_index = False
self._curves = { self._curves = {
name: tuple( name: tuple(
float(value) float(value)
@@ -170,7 +178,19 @@ class CalibratedCommandMapper:
} }
self._previous_by_motor: dict[int, float] = {} self._previous_by_motor: dict[int, float] = {}
self._direction_by_motor: dict[int, str] = {} self._direction_by_motor: dict[int, str] = {}
self.direction_deadband_u8 = 0.5 self.direction_deadband_u8 = 0.002 if schema_version == 7 else 0.5
self._knots = {
name: tuple(float(value) for value in payload["joints"][name].get(
"curve_input_knots_rad", ()
))
for name in self.urdf_joint_names
}
self._raw_increasing_branch = {
name: str(payload["joints"][name].get(
"raw_increasing_curve_branch", "increasing"
))
for name in self.urdf_joint_names
}
@staticmethod @staticmethod
def _command_index(value: float) -> int: def _command_index(value: float) -> int:
@@ -183,7 +203,7 @@ class CalibratedCommandMapper:
self, positions: Sequence[float], names: Sequence[str] = () self, positions: Sequence[float], names: Sequence[str] = ()
) -> tuple[float, ...]: ) -> tuple[float, ...]:
values = tuple(float(value) for value in positions) values = tuple(float(value) for value in positions)
if names: if names and not self.feedback_by_index:
if len(names) != len(values): if len(names) != len(values):
raise ValueError( raise ValueError(
"JointState names and positions must have equal length" "JointState names and positions must have equal length"
@@ -208,7 +228,10 @@ class CalibratedCommandMapper:
f"{len(self.command_names)} positions" f"{len(self.command_names)} positions"
) )
command = values command = values
indices = tuple(self._command_index(value) for value in command) indices = (
() if self.input_domain == "feedback_rad"
else tuple(self._command_index(value) for value in command)
)
direction_by_motor: dict[int, str | None] = {} direction_by_motor: dict[int, str | None] = {}
for motor, value in enumerate(command): for motor, value in enumerate(command):
previous = self._previous_by_motor.get(motor) previous = self._previous_by_motor.get(motor)
@@ -223,6 +246,13 @@ class CalibratedCommandMapper:
for name in self.urdf_joint_names: for name in self.urdf_joint_names:
motor = self._motor_by_joint[name] motor = self._motor_by_joint[name]
direction = direction_by_motor[motor] direction = direction_by_motor[motor]
if self.input_domain == "feedback_rad" and direction is not None:
raw_increasing = self._raw_increasing_branch[name]
direction = (
raw_increasing
if direction == "increasing"
else "increasing" if raw_increasing == "decreasing" else "decreasing"
)
curves = ( curves = (
self._increasing_curves self._increasing_curves
if direction == "increasing" if direction == "increasing"
@@ -230,7 +260,10 @@ class CalibratedCommandMapper:
if direction == "decreasing" if direction == "decreasing"
else self._curves else self._curves
) )
result.append(curves[name][indices[motor]]) if self.input_domain == "feedback_rad":
result.append(float(np.interp(command[motor], self._knots[name], curves[name])))
else:
result.append(curves[name][indices[motor]])
for motor, value in enumerate(command): for motor, value in enumerate(command):
self._previous_by_motor[motor] = value self._previous_by_motor[motor] = value
direction = direction_by_motor[motor] direction = direction_by_motor[motor]
@@ -259,6 +292,8 @@ def default_input_topic(
return f"/{str(model).lower()}/cb_{side}_hand_state" return f"/{str(model).lower()}/cb_{side}_hand_state"
if input_domain == "command_u8": if input_domain == "command_u8":
return f"/{str(model).lower()}/cb_{side}_hand_control_cmd" return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
if input_domain == "feedback_rad":
return f"/{str(model).lower()}/{side}/joint_states"
raise ValueError("calibration curve_input_domain is invalid") raise ValueError("calibration curve_input_domain is invalid")
@@ -9,7 +9,7 @@ from ..core import ProfileKey
def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey: def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
version = int(raw.get("schema_version", -1)) version = int(raw.get("schema_version", -1))
if version == 2: if version in {2, 3}:
key = ProfileKey.parse(str(raw.get("profile_id", ""))) key = ProfileKey.parse(str(raw.get("profile_id", "")))
for field, actual in ( for field, actual in (
("model", key.model), ("model", key.model),
@@ -21,7 +21,7 @@ def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
raise ValueError(f"{field} differs from profile_id") raise ValueError(f"{field} differs from profile_id")
return key return key
if version != 1: if version != 1:
raise ValueError("product config schema_version must be 1 or 2") raise ValueError("product config schema_version must be 1, 2 or 3")
model = str(raw.get("model", "")).strip().upper() model = str(raw.get("model", "")).strip().upper()
side = str(raw.get("side", "")).strip().lower() side = str(raw.get("side", "")).strip().lower()
layout = str(raw.get("tag_layout", "")).strip().lower() layout = str(raw.get("tag_layout", "")).strip().lower()
@@ -3,6 +3,7 @@
from __future__ import annotations from __future__ import annotations
from dataclasses import dataclass, field from dataclasses import dataclass, field
import math
from pathlib import PurePath from pathlib import PurePath
from typing import Mapping from typing import Mapping
@@ -59,11 +60,54 @@ class CommandLayout:
# SDK speed commands are not necessarily one value per position channel. # SDK speed commands are not necessarily one value per position channel.
# This mapping makes that protocol detail explicit in a profile. # This mapping makes that protocol detail explicit in a profile.
speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict) speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict)
# Physical-angle SDKs use these fields. ``baseline_u8`` remains the
# deployed compatibility contract for legacy byte-command profiles.
unit: str = "u8"
baseline: tuple[float, ...] = ()
lower_bounds: tuple[float, ...] = ()
upper_bounds: tuple[float, ...] = ()
feedback_by_index: bool = False
@property @property
def command_count(self) -> int: def command_count(self) -> int:
return len(self.names) return len(self.names)
@property
def baseline_values(self) -> tuple[float, ...]:
source = self.baseline if self.baseline else self.baseline_u8
return tuple(float(value) for value in source)
@property
def minimum_values(self) -> tuple[float, ...]:
return (
tuple(float(value) for value in self.lower_bounds)
if self.lower_bounds
else (0.0,) * self.command_count
)
@property
def maximum_values(self) -> tuple[float, ...]:
return (
tuple(float(value) for value in self.upper_bounds)
if self.upper_bounds
else (255.0,) * self.command_count
)
def normalize(self, index: int, value: float) -> float:
lower = self.minimum_values[int(index)]
upper = self.maximum_values[int(index)]
if not math.isfinite(float(value)) or upper <= lower:
raise ValueError("command value or bounds are invalid")
return (float(value) - lower) / (upper - lower)
def denormalize(self, index: int, progress: float) -> float:
lower = self.minimum_values[int(index)]
upper = self.maximum_values[int(index)]
phase = float(progress)
if not math.isfinite(phase) or not 0.0 <= phase <= 1.0:
raise ValueError("normalized command progress must be in [0, 1]")
return lower + phase * (upper - lower)
@dataclass(frozen=True) @dataclass(frozen=True)
class TagSpec: class TagSpec:
@@ -105,12 +149,24 @@ class TaskSpec:
view: str view: str
command_index: int command_index: int
joints: tuple[str, ...] joints: tuple[str, ...]
auxiliary_commands: tuple[tuple[int, int], ...] = () auxiliary_commands: tuple[tuple[int, float], ...] = ()
validation_only: bool = False validation_only: bool = False
start_u8: int = 255 start_u8: int = 255
end_u8: int = 0 end_u8: int = 0
preflight_speed_u8: int | None = None preflight_speed_u8: int | None = None
formal_speed_u8: int | None = None formal_speed_u8: int | None = None
start: float | None = None
end: float | None = None
preflight_speed: float | None = None
formal_speed: float | None = None
@property
def start_value(self) -> float:
return float(self.start_u8 if self.start is None else self.start)
@property
def end_value(self) -> float:
return float(self.end_u8 if self.end is None else self.end)
@dataclass(frozen=True) @dataclass(frozen=True)
@@ -232,12 +288,30 @@ def validate_profile(profile: CalibrationProfile) -> None:
"""Hard-check all cross-policy references before hardware is enabled.""" """Hard-check all cross-policy references before hardware is enabled."""
errors: list[str] = [] errors: list[str] = []
command = profile.command command = profile.command
if not command.names or len(command.names) != len(command.baseline_u8): baseline = command.baseline_values
if not command.names or len(command.names) != len(baseline):
errors.append("command names and baseline must be non-empty and aligned") errors.append("command names and baseline must be non-empty and aligned")
if len(set(command.names)) != len(command.names): if len(set(command.names)) != len(command.names):
errors.append("command names must be unique") errors.append("command names must be unique")
if any(value < 0 or value > 255 for value in command.baseline_u8): if command.unit not in {"u8", "rad"}:
errors.append("command unit must be u8 or rad")
if command.unit == "u8" and any(value < 0 or value > 255 for value in baseline):
errors.append("baseline command values must be in [0, 255]") errors.append("baseline command values must be in [0, 255]")
if (
len(command.minimum_values) != command.command_count
or len(command.maximum_values) != command.command_count
):
errors.append("command bounds must align with command names")
elif any(
not math.isfinite(lower)
or not math.isfinite(upper)
or lower >= upper
or not lower <= value <= upper
for lower, upper, value in zip(
command.minimum_values, command.maximum_values, baseline
)
):
errors.append("command bounds or baseline values are invalid")
indices = set(range(command.command_count)) indices = set(range(command.command_count))
if not set(command.disabled_indices).issubset(indices): if not set(command.disabled_indices).issubset(indices):
errors.append("disabled command index is out of range") errors.append("disabled command index is out of range")
@@ -291,13 +365,24 @@ def validate_profile(profile: CalibrationProfile) -> None:
errors.append(f"task {task.key} references unknown measurements") errors.append(f"task {task.key} references unknown measurements")
if any(index not in indices for index, _ in task.auxiliary_commands): if any(index not in indices for index, _ in task.auxiliary_commands):
errors.append(f"task {task.key} auxiliary index is out of range") 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: elif any(
not command.minimum_values[index] <= float(value) <= command.maximum_values[index]
for index, value in task.auxiliary_commands
):
errors.append(f"task {task.key} auxiliary command is out of range")
lower = command.minimum_values[task.command_index]
upper = command.maximum_values[task.command_index]
if not lower <= task.start_value <= upper or not lower <= task.end_value <= upper:
errors.append(f"task {task.key} sweep endpoint is out of range") errors.append(f"task {task.key} sweep endpoint is out of range")
if task.start_u8 == task.end_u8: if task.start_value == task.end_value:
errors.append(f"task {task.key} sweep endpoints must differ") errors.append(f"task {task.key} sweep endpoints must differ")
for speed in (task.preflight_speed_u8, task.formal_speed_u8): for speed in (task.preflight_speed_u8, task.formal_speed_u8):
if speed is not None and not 0 <= speed <= 255: if speed is not None and not 0 <= speed <= 255:
errors.append(f"task {task.key} speed is out of range") errors.append(f"task {task.key} speed is out of range")
if command.unit == "rad" and (
task.formal_speed is None or float(task.formal_speed) <= 0.0
):
errors.append(f"task {task.key} physical speed must be positive")
for name, spec in profile.measurement.measurements.items(): for name, spec in profile.measurement.measurements.items():
if name != spec.joint: if name != spec.joint:
errors.append(f"measurement mapping key differs for {name}") errors.append(f"measurement mapping key differs for {name}")
@@ -3,6 +3,7 @@
from __future__ import annotations from __future__ import annotations
from dataclasses import dataclass, field from dataclasses import dataclass, field
import math
from typing import Any, Mapping from typing import Any, Mapping
@@ -17,11 +18,22 @@ class SampleRecord:
timestamp_ns: int timestamp_ns: int
values: Mapping[str, Any] values: Mapping[str, Any]
quality: Mapping[str, float] = field(default_factory=dict) quality: Mapping[str, float] = field(default_factory=dict)
command: float | None = None
command_unit: str = "u8"
progress_01: float | None = None
def __post_init__(self) -> None: def __post_init__(self) -> None:
if not self.task_key or not self.measurement or not self.view: if not self.task_key or not self.measurement or not self.view:
raise ValueError("sample task, measurement, and view are required") raise ValueError("sample task, measurement, and view are required")
if self.cycle < 0 or not 0 <= self.command_u8 <= 255: if self.cycle < 0:
raise ValueError("sample cycle or command is out of range") raise ValueError("sample cycle or command is out of range")
if self.command_unit == "u8" and not 0 <= self.command_u8 <= 255:
raise ValueError("sample cycle or command is out of range")
if self.command_unit not in {"u8", "rad"}:
raise ValueError("sample command unit must be u8 or rad")
if self.command is not None and not math.isfinite(float(self.command)):
raise ValueError("sample command must be finite")
if self.progress_01 is not None and not 0.0 <= float(self.progress_01) <= 1.0:
raise ValueError("sample progress must be in [0, 1]")
if self.timestamp_ns < 0: if self.timestamp_ns < 0:
raise ValueError("sample timestamp must be non-negative") raise ValueError("sample timestamp must be non-negative")
@@ -39,11 +39,12 @@ class MotionStep:
phase: str phase: str
task_key: str | None task_key: str | None
command_index: int | None command_index: int | None
target_u8: int target_u8: float
speed_u8: int speed_u8: float
cycle: int | None = None cycle: int | None = None
direction: str | None = None direction: str | None = None
attempt: int = 1 attempt: int = 1
target_command: tuple[float, ...] | None = None
@property @property
def recording(self) -> bool: def recording(self) -> bool:
@@ -67,7 +68,11 @@ class L6ThreeCameraCalibrationNode(Node):
self.profile = build_typed_profile() if profile is None else profile self.profile = build_typed_profile() if profile is None else profile
self.model_name = self.profile.key.model self.model_name = self.profile.key.model
self.command_names = tuple(self.profile.command.names) self.command_names = tuple(self.profile.command.names)
self.baseline_command = tuple(self.profile.command.baseline_u8) self.command_count = self.profile.command.command_count
self.command_unit = self.profile.command.unit
self.baseline_command = tuple(self.profile.command.baseline_values)
self.command_lower = tuple(self.profile.command.minimum_values)
self.command_upper = tuple(self.profile.command.maximum_values)
self.sample_kind = str(sample_kind) self.sample_kind = str(sample_kind)
self.sweep_quality_kind = f"{self.model_name.lower()}_sweep_observation_quality" self.sweep_quality_kind = f"{self.model_name.lower()}_sweep_observation_quality"
self.finalize_session = finalizer or finalize_l6_session self.finalize_session = finalizer or finalize_l6_session
@@ -80,10 +85,10 @@ class L6ThreeCameraCalibrationNode(Node):
self.raw_path, self.raw_path,
{ {
"kind": "session_start", "kind": "session_start",
"sample_schema_version": 6, "sample_schema_version": self.profile.artifacts.output_schema_version,
"profile_id": self.profile.key.profile_id, "profile_id": self.profile.key.profile_id,
"serial_number": self.serial_number, "serial_number": self.serial_number,
"curve_input_domain": "feedback_u8", "curve_input_domain": f"feedback_{self.command_unit}",
}, },
) )
@@ -159,6 +164,7 @@ class L6ThreeCameraCalibrationNode(Node):
self.step_last_command_u8: tuple[int, ...] | None = None self.step_last_command_u8: tuple[int, ...] | None = None
self.step_trajectory_phase = 0.0 self.step_trajectory_phase = 0.0
self.step_trajectory_duration_seconds = 0.0 self.step_trajectory_duration_seconds = 0.0
self.step_moving_indices: frozenset[int] = frozenset()
self.step_valid_frames = 0 self.step_valid_frames = 0
self.step_total_frames = 0 self.step_total_frames = 0
self.step_required_roles: tuple[str, ...] = () self.step_required_roles: tuple[str, ...] = ()
@@ -175,7 +181,11 @@ class L6ThreeCameraCalibrationNode(Node):
self.started = False self.started = False
self.commanded_speed: int | None = None self.commanded_speed: int | None = None
self.retry_counts: dict[tuple[str, int, str], int] = {} self.retry_counts: dict[tuple[str, int, str], int] = {}
self.create_timer(0.01, self._tick) self.create_timer(
1.0 / float(self.command_rate_hz)
if self.command_unit == "rad" else 0.01,
self._tick,
)
self.create_timer(0.5, self._publish_status) self.create_timer(0.5, self._publish_status)
def _declare_parameters(self) -> None: def _declare_parameters(self) -> None:
@@ -191,6 +201,7 @@ class L6ThreeCameraCalibrationNode(Node):
"camera_extrinsics_expected_sha256": "", "camera_extrinsics_expected_sha256": "",
"calibration_config_expected_sha256": "", "calibration_config_expected_sha256": "",
"tag_config_expected_sha256": "", "tag_config_expected_sha256": "",
"sdk_config_expected_sha256": "",
"command_topic": f"/{model}/cb_right_hand_control_cmd", "command_topic": f"/{model}/cb_right_hand_control_cmd",
"state_topic": f"/{model}/cb_right_hand_state", "state_topic": f"/{model}/cb_right_hand_state",
"setting_topic": f"/{model}/cb_hand_setting_cmd", "setting_topic": f"/{model}/cb_hand_setting_cmd",
@@ -245,6 +256,10 @@ class L6ThreeCameraCalibrationNode(Node):
"pnp_maximum_translation_jump_m": 0.04, "pnp_maximum_translation_jump_m": 0.04,
"pnp_maximum_tag_tilt_deg": 75.0, "pnp_maximum_tag_tilt_deg": 75.0,
"pnp_tracker_reset_seconds": 5.0, "pnp_tracker_reset_seconds": 5.0,
"command_rate_hz": float(speed_parameters.get("command_rate_hz", 100.0)),
"endpoint_tolerance_rad": 0.01,
"non_target_motion_tolerance_rad": 0.015,
"minimum_state_span_fraction": 0.90,
} }
for name, default in defaults.items(): for name, default in defaults.items():
self.declare_parameter(name, default) self.declare_parameter(name, default)
@@ -280,8 +295,16 @@ class L6ThreeCameraCalibrationNode(Node):
), ),
"tag_config_sha256": str(value("tag_config_expected_sha256")), "tag_config_sha256": str(value("tag_config_expected_sha256")),
} }
if any(len(item) != 64 for item in self.protected_inputs.values()): if "sdk_config_sha256" in self.profile.artifacts.protected_input_fields:
raise ValueError("all four immutable input SHA-256 values are required") self.protected_inputs["sdk_config_sha256"] = str(
value("sdk_config_expected_sha256")
)
if (
set(self.protected_inputs)
!= self.profile.artifacts.protected_input_fields
or any(len(item) != 64 for item in self.protected_inputs.values())
):
raise ValueError("all profile-protected SHA-256 values are required")
self.command_topic = str(value("command_topic")) self.command_topic = str(value("command_topic"))
self.state_topic = str(value("state_topic")) self.state_topic = str(value("state_topic"))
self.setting_topic = str(value("setting_topic")) self.setting_topic = str(value("setting_topic"))
@@ -311,16 +334,31 @@ class L6ThreeCameraCalibrationNode(Node):
"pnp_maximum_reprojection_error_px", "pnp_reprojection_tie_px", "pnp_maximum_reprojection_error_px", "pnp_reprojection_tie_px",
"pnp_maximum_pose_jump_deg", "pnp_maximum_translation_jump_m", "pnp_maximum_pose_jump_deg", "pnp_maximum_translation_jump_m",
"pnp_maximum_tag_tilt_deg", "pnp_tracker_reset_seconds", "pnp_maximum_tag_tilt_deg", "pnp_tracker_reset_seconds",
"command_rate_hz", "endpoint_tolerance_rad",
"non_target_motion_tolerance_rad", "minimum_state_span_fraction",
): ):
setattr(self, name, value(name)) setattr(self, name, value(name))
self.maximum_state_image_skew_ns = int( self.maximum_state_image_skew_ns = int(
float(self.maximum_state_image_skew_ms) * 1_000_000 float(self.maximum_state_image_skew_ms) * 1_000_000
) )
for name in ( if self.command_unit == "u8":
"baseline_speed_u8", "preflight_speed_u8", "formal_speed_u8" for name in (
): "baseline_speed_u8", "preflight_speed_u8", "formal_speed_u8"
if not 1 <= int(getattr(self, name)) <= 255: ):
raise ValueError(f"{name} must be in [1, 255]") if not 1 <= int(getattr(self, name)) <= 255:
raise ValueError(f"{name} must be in [1, 255]")
else:
self.endpoint_tolerance_u8 = float(self.endpoint_tolerance_rad)
self.non_target_motion_tolerance_u8 = float(
self.non_target_motion_tolerance_rad
)
self.minimum_state_span_u8 = float(self.minimum_state_span_fraction)
if not 1.0 <= float(self.command_rate_hz) <= 100.0:
raise ValueError("command_rate_hz must be in [1, 100]")
if not 0.0 < float(self.endpoint_tolerance_u8) <= 0.1:
raise ValueError("endpoint_tolerance_rad must be in (0, 0.1]")
if not 0.0 < float(self.minimum_state_span_u8) <= 1.0:
raise ValueError("minimum_state_span_fraction must be in (0, 1]")
if not 0.0 <= float(self.speed_settle_seconds) <= 5.0: if not 0.0 <= float(self.speed_settle_seconds) <= 5.0:
raise ValueError("speed_settle_seconds must be in [0, 5]") raise ValueError("speed_settle_seconds must be in [0, 5]")
if not 2.0 <= float(self.command_trajectory_full_range_seconds) <= 30.0: if not 2.0 <= float(self.command_trajectory_full_range_seconds) <= 30.0:
@@ -333,27 +371,38 @@ class L6ThreeCameraCalibrationNode(Node):
raise ValueError("minimum_joint_frame_rate must be in (0, 1]") raise ValueError("minimum_joint_frame_rate must be in (0, 1]")
def _build_steps(self) -> list[MotionStep]: def _build_steps(self) -> list[MotionStep]:
command_unit = getattr(self, "command_unit", "u8")
steps = [ steps = [
MotionStep( MotionStep(
"baseline", None, None, 255, int(self.baseline_speed_u8) "baseline", None, None, 255, int(self.baseline_speed_u8)
) )
] ]
for task in self.profile.motion.tasks: for task in self.profile.motion.tasks:
midpoint = int(round(0.5 * (task.start_u8 + task.end_u8))) midpoint = 0.5 * (task.start_value + task.end_value)
for target in (task.start_u8, midpoint, task.end_u8, task.start_u8): preflight_speed = (
task.preflight_speed
if command_unit == "rad"
else task.preflight_speed_u8 or self.preflight_speed_u8
)
formal_speed = (
task.formal_speed
if command_unit == "rad"
else self.formal_speed_u8
)
for target in (task.start_value, midpoint, task.end_value, task.start_value):
steps.append( steps.append(
MotionStep( MotionStep(
"preflight", task.key, task.command_index, target, "preflight", task.key, task.command_index, target,
int(task.preflight_speed_u8 or self.preflight_speed_u8), float(preflight_speed),
) )
) )
for cycle in (0, 1, 2, 3): for cycle in (0, 1, 2, 3):
steps.extend( steps.extend(
[ [
MotionStep("prepare", task.key, task.command_index, task.start_u8, int(self.formal_speed_u8), cycle), MotionStep("prepare", task.key, task.command_index, task.start_value, float(formal_speed), cycle),
MotionStep("sweep", task.key, task.command_index, task.end_u8, int(self.formal_speed_u8), cycle, "decreasing"), MotionStep("sweep", task.key, task.command_index, task.end_value, float(formal_speed), cycle, "decreasing"),
MotionStep("prepare", task.key, task.command_index, task.end_u8, int(self.formal_speed_u8), cycle), MotionStep("prepare", task.key, task.command_index, task.end_value, float(formal_speed), cycle),
MotionStep("sweep", task.key, task.command_index, task.start_u8, int(self.formal_speed_u8), cycle, "increasing"), MotionStep("sweep", task.key, task.command_index, task.start_value, float(formal_speed), cycle, "increasing"),
] ]
) )
return steps return steps
@@ -378,9 +427,18 @@ class L6ThreeCameraCalibrationNode(Node):
return response return response
def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
self._publish_command(list(self.baseline_command)) hold = (
list(self.latest_state_u8)
if self.command_unit == "rad" and len(self.latest_state_u8) == self.command_count
else list(self.baseline_command)
)
self._publish_command(hold)
self.state = "ABORTED" self.state = "ABORTED"
self.reason = "operator_abort_returning_to_open_baseline" self.reason = (
"operator_abort_holding_current_position"
if self.command_unit == "rad"
else "operator_abort_returning_to_open_baseline"
)
response.success = True response.success = True
response.message = self.reason response.message = self.reason
return response return response
@@ -392,9 +450,9 @@ class L6ThreeCameraCalibrationNode(Node):
self.image_sizes[view] = (int(message.width), int(message.height)) self.image_sizes[view] = (int(message.width), int(message.height))
def _state_callback(self, message: JointState) -> None: def _state_callback(self, message: JointState) -> None:
if len(message.position) != 6: if len(message.position) != self.command_count:
return return
if message.name: if message.name and not self.profile.command.feedback_by_index:
by_name = dict(zip((str(name) for name in message.name), message.position)) by_name = dict(zip((str(name) for name in message.name), message.position))
for alias, canonical in self.profile.command.feedback_name_aliases.items(): for alias, canonical in self.profile.command.feedback_name_aliases.items():
if alias in by_name and canonical not in by_name: if alias in by_name and canonical not in by_name:
@@ -406,6 +464,13 @@ class L6ThreeCameraCalibrationNode(Node):
state = tuple(float(value) for value in message.position) state = tuple(float(value) for value in message.position)
if not all(math.isfinite(value) for value in state): if not all(math.isfinite(value) for value in state):
return return
if any(
value < self.command_lower[index] - 1.0e-6
or value > self.command_upper[index] + 1.0e-6
for index, value in enumerate(state)
):
self._pause("feedback_outside_registered_command_domain")
return
stamp = _stamp_ns(message.header.stamp) stamp = _stamp_ns(message.header.stamp)
if stamp <= 0: if stamp <= 0:
stamp = int(self.get_clock().now().nanoseconds) stamp = int(self.get_clock().now().nanoseconds)
@@ -416,9 +481,15 @@ class L6ThreeCameraCalibrationNode(Node):
step = self._current_step() step = self._current_step()
if step is not None and step.task_key is not None: if step is not None and step.task_key is not None:
for index, actual in enumerate(state): for index, actual in enumerate(state):
if index == step.command_index: if index == step.command_index or index in self.step_moving_indices:
continue continue
if abs(actual - self.baseline_command[index]) > float(self.non_target_motion_tolerance_u8): expected = (
self.step_last_command_u8[index]
if self.step_last_command_u8 is not None
else self.baseline_command[index]
)
tolerance = float(self.non_target_motion_tolerance_u8)
if abs(actual - expected) > tolerance:
self._pause( self._pause(
f"non_target_motor_moved:channel={index}:feedback={actual:.2f}" f"non_target_motor_moved:channel={index}:feedback={actual:.2f}"
) )
@@ -666,6 +737,9 @@ class L6ThreeCameraCalibrationNode(Node):
if recording_this_view: if recording_this_view:
self.step_pnp_valid_frames += 1 self.step_pnp_valid_frames += 1
self.last_view_valid_at[view] = time.monotonic() self.last_view_valid_at[view] = time.monotonic()
preflight_hook = getattr(self, "_observe_preflight_pose", None)
if preflight_hook is not None and step is not None and step.phase == "preflight":
preflight_hook(view, selected, step)
if step is None or not step.recording or self._task(step.task_key).view != view: if step is None or not step.recording or self._task(step.task_key).view != view:
return return
matched = interpolate_state_u8( matched = interpolate_state_u8(
@@ -680,9 +754,12 @@ class L6ThreeCameraCalibrationNode(Node):
self.step_state_sync_frames += 1 self.step_state_sync_frames += 1
task = self._task(step.task_key) task = self._task(step.task_key)
feedback = float(state_u8[task.command_index]) feedback = float(state_u8[task.command_index])
if not 0.0 <= feedback <= 255.0: lower = self.command_lower[task.command_index]
self._count_step_rejection("feedback:outside_u8_domain") upper = self.command_upper[task.command_index]
if not lower <= feedback <= upper:
self._count_step_rejection("feedback:outside_registered_domain")
return return
progress = self.profile.command.normalize(task.command_index, feedback)
for joint in task.joints: for joint in task.joints:
measurement = self.profile.measurement.measurements[joint] measurement = self.profile.measurement.measurements[joint]
parent = selected[str(measurement.parent_role)] parent = selected[str(measurement.parent_role)]
@@ -718,10 +795,6 @@ class L6ThreeCameraCalibrationNode(Node):
# Keep the established schema contract: this field labels the # Keep the established schema contract: this field labels the
# requested sweep endpoint. The live shaped set-point is a # requested sweep endpoint. The live shaped set-point is a
# separate diagnostic and is never used as the fit domain. # separate diagnostic and is never used as the fit domain.
"requested_command_u8": int(step.target_u8),
"trajectory_command_u8": round(self.step_requested_u8, 6),
"feedback_u8": round(feedback, 6),
"state_u8": [round(float(value), 6) for value in state_u8],
"state_image_sync_error_ms": round(abs(skew_ns) / 1_000_000.0, 6), "state_image_sync_error_ms": round(abs(skew_ns) / 1_000_000.0, 6),
"relative_quaternion_xyzw": [ "relative_quaternion_xyzw": [
float(value) for value in relative.as_quat() float(value) for value in relative.as_quat()
@@ -749,6 +822,22 @@ class L6ThreeCameraCalibrationNode(Node):
), ),
"image_stamp_ns": stamp, "image_stamp_ns": stamp,
} }
if self.command_unit == "u8":
record.update({
"requested_command_u8": int(step.target_u8),
"trajectory_command_u8": round(self.step_requested_u8, 6),
"feedback_u8": round(feedback, 6),
"state_u8": [round(float(value), 6) for value in state_u8],
})
else:
record.update({
"requested_command_rad": round(float(step.target_u8), 9),
"trajectory_command_rad": round(self.step_requested_u8, 9),
"command_rad": round(self.step_requested_u8, 9),
"feedback_rad": round(feedback, 9),
"state_rad": [round(float(value), 9) for value in state_u8],
"progress_01": round(progress, 9),
})
self.raw_records.append(record) self.raw_records.append(record)
append_jsonl(self.raw_path, record) append_jsonl(self.raw_path, record)
self.step_valid_frames += 1 self.step_valid_frames += 1
@@ -760,7 +849,7 @@ class L6ThreeCameraCalibrationNode(Node):
return 0.0 if elapsed <= 0 else (len(self.state_receive_times) - 1) / elapsed return 0.0 if elapsed <= 0 else (len(self.state_receive_times) - 1) / elapsed
def _publish_torque(self) -> None: def _publish_torque(self) -> None:
if not self.commands_enabled: if not self.commands_enabled or self.command_unit != "u8":
return return
message = String() message = String()
message.data = json.dumps( message.data = json.dumps(
@@ -771,7 +860,10 @@ class L6ThreeCameraCalibrationNode(Node):
) )
self.setting_publisher.publish(message) self.setting_publisher.publish(message)
def _publish_speed(self, speed: int) -> None: def _publish_speed(self, speed: float) -> None:
if self.command_unit != "u8":
self.commanded_speed = float(speed)
return
if not self.commands_enabled or self.commanded_speed == int(speed): if not self.commands_enabled or self.commanded_speed == int(speed):
return return
message = String() message = String()
@@ -784,47 +876,70 @@ class L6ThreeCameraCalibrationNode(Node):
self.setting_publisher.publish(message) self.setting_publisher.publish(message)
self.commanded_speed = int(speed) self.commanded_speed = int(speed)
def _publish_command(self, values: list[int]) -> None: def _publish_command(self, values: list[float]) -> None:
if not self.commands_enabled: if not self.commands_enabled:
return return
if len(values) != self.command_count:
raise ValueError("command has the wrong channel count")
bounded = [
float(np.clip(value, self.command_lower[index], self.command_upper[index]))
for index, value in enumerate(values)
]
message = JointState() message = JointState()
message.header.stamp = self.get_clock().now().to_msg() message.header.stamp = self.get_clock().now().to_msg()
message.name = list(self.command_names) message.name = (
message.position = [float(value) for value in values] [] if self.profile.command.feedback_by_index else list(self.command_names)
)
message.position = bounded
self.command_publisher.publish(message) self.command_publisher.publish(message)
def _target_command(self, step: MotionStep) -> tuple[float, ...]:
if step.target_command is not None:
if len(step.target_command) != self.command_count:
raise ValueError("motion-step target has the wrong channel count")
return tuple(float(value) for value in step.target_command)
target = [float(value) for value in self.baseline_command]
if step.task_key is not None:
for index, value in self._task(step.task_key).auxiliary_commands:
target[int(index)] = float(value)
if step.command_index is not None:
target[step.command_index] = float(step.target_u8)
return tuple(target)
def _begin_step(self, step: MotionStep) -> None: def _begin_step(self, step: MotionStep) -> None:
now = time.monotonic() now = time.monotonic()
if self.commanded_speed != int(step.speed_u8): if self.commanded_speed != step.speed_u8:
self._publish_speed(step.speed_u8) self._publish_speed(step.speed_u8)
self.step_speed_ready_at = now + float(self.speed_settle_seconds) self.step_speed_ready_at = now + float(self.speed_settle_seconds)
self.reason = f"setting_speed:{step.speed_u8}" self.reason = f"setting_speed:{step.speed_u8}"
return return
if now < self.step_speed_ready_at: if now < self.step_speed_ready_at:
return return
if len(self.latest_state_u8) != 6: if len(self.latest_state_u8) != self.command_count:
return return
self.step_start_state_u8 = tuple(float(value) for value in self.latest_state_u8) self.step_start_state_u8 = tuple(float(value) for value in self.latest_state_u8)
self.step_started_at = now self.step_started_at = now
self.step_last_progress_at = now self.step_last_progress_at = now
self.step_last_feedback = ( self.step_last_feedback = (
float(self.latest_state_u8[step.command_index]) float(self.latest_state_u8[step.command_index])
if step.command_index is not None and len(self.latest_state_u8) == 6 if step.command_index is not None and len(self.latest_state_u8) == self.command_count
else ( else (
float(np.mean(self.latest_state_u8)) float(np.mean(self.latest_state_u8))
if len(self.latest_state_u8) == 6 if len(self.latest_state_u8) == self.command_count
else float("nan") else float("nan")
) )
) )
self.step_initial_feedback = self.step_last_feedback self.step_initial_feedback = self.step_last_feedback
self.step_requested_u8 = self.step_initial_feedback self.step_requested_u8 = self.step_initial_feedback
target = [float(value) for value in self.baseline_command] target = list(self._target_command(step))
if step.command_index is not None:
target[step.command_index] = float(step.target_u8)
errors = [ errors = [
abs(end - start) abs(end - start)
for start, end in zip(self.step_start_state_u8, target) for start, end in zip(self.step_start_state_u8, target)
] ]
self.step_moving_indices = frozenset(
index for index, error in enumerate(errors)
if error > float(self.non_target_motion_tolerance_u8)
)
self.step_last_distance_u8 = ( self.step_last_distance_u8 = (
float(sum(errors)) float(sum(errors))
if step.command_index is None if step.command_index is None
@@ -832,14 +947,16 @@ class L6ThreeCameraCalibrationNode(Node):
) )
self.step_initial_distance_u8 = self.step_last_distance_u8 self.step_initial_distance_u8 = self.step_last_distance_u8
maximum_distance = max(errors) maximum_distance = max(errors)
_, _, self.step_trajectory_duration_seconds = ( if self.command_unit == "rad":
cosine_position_trajectory_u8( self.step_trajectory_duration_seconds = (
0.0, 0.0 if maximum_distance <= 0.0 else
maximum_distance, math.pi * maximum_distance / (2.0 * float(step.speed_u8))
0.0, )
else:
_, _, self.step_trajectory_duration_seconds = cosine_position_trajectory_u8(
0.0, maximum_distance, 0.0,
float(self.command_trajectory_full_range_seconds), float(self.command_trajectory_full_range_seconds),
) )
)
self.step_trajectory_phase = 0.0 self.step_trajectory_phase = 0.0
self.step_last_command_u8 = None self.step_last_command_u8 = None
self.step_hold_since = None self.step_hold_since = None
@@ -855,32 +972,34 @@ class L6ThreeCameraCalibrationNode(Node):
) )
def _advance_step_trajectory(self, step: MotionStep, now: float) -> None: def _advance_step_trajectory(self, step: MotionStep, now: float) -> None:
baseline = tuple(getattr(self, "baseline_command", (255,) * 6)) if hasattr(self, "_target_command"):
target_values = self._target_command(step)
else:
# Compatibility for the isolated legacy trajectory unit harness.
target = [255.0] * len(self.step_start_state_u8)
if step.command_index is not None:
target[step.command_index] = float(step.target_u8)
target_values = tuple(target)
elapsed = max(0.0, float(now) - self.step_started_at) elapsed = max(0.0, float(now) - self.step_started_at)
_, phase, _ = cosine_position_trajectory_u8( duration = float(
0.0, getattr(
max( self,
( "step_trajectory_duration_seconds",
abs(float(baseline[index]) - value) self.command_trajectory_full_range_seconds,
if step.command_index is None )
else abs(float(step.target_u8) - value)
if index == step.command_index
else abs(float(baseline[index]) - value)
)
for index, value in enumerate(self.step_start_state_u8)
),
elapsed,
float(self.command_trajectory_full_range_seconds),
) )
phase = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration)
blend = 0.5 - 0.5 * math.cos(math.pi * phase) blend = 0.5 - 0.5 * math.cos(math.pi * phase)
targets = [float(value) for value in baseline]
if step.command_index is not None:
targets[step.command_index] = float(step.target_u8)
values = [ values = [
start + (target - start) * blend start + (target - start) * blend
for start, target in zip(self.step_start_state_u8, targets) for start, target in zip(self.step_start_state_u8, target_values)
] ]
command = tuple(int(np.clip(round(value), 0, 255)) for value in values) command = tuple(
int(np.clip(round(value), 0, 255))
if getattr(self, "command_unit", "u8") == "u8"
else float(np.clip(value, self.command_lower[index], self.command_upper[index]))
for index, value in enumerate(values)
)
self.step_trajectory_phase = phase self.step_trajectory_phase = phase
self.step_requested_u8 = ( self.step_requested_u8 = (
float(np.mean(values)) float(np.mean(values))
@@ -901,20 +1020,41 @@ class L6ThreeCameraCalibrationNode(Node):
and int(row["attempt"]) == step.attempt and int(row["attempt"]) == step.attempt
and row["joint"] == task.joints[0] and row["joint"] == task.joints[0]
] ]
feedback = np.asarray([float(row["feedback_u8"]) for row in rows]) command_unit = getattr(self, "command_unit", "u8")
bins = sorted(set(int(round(value)) for value in feedback)) feedback_field = "feedback_u8" if command_unit == "u8" else "feedback_rad"
gap = max((right - left for left, right in zip(bins, bins[1:])), default=256) feedback = np.asarray([float(row[feedback_field]) for row in rows])
if command_unit == "u8":
bins = sorted(set(int(round(value)) for value in feedback))
span = float(np.ptp(feedback)) if feedback.size else 0.0
required_span = float(self.minimum_state_span_u8)
gap = max((right - left for left, right in zip(bins, bins[1:])), default=256)
maximum_gap = float(self.maximum_bin_gap)
else:
normalized = sorted(
set(
min(31, max(0, int(self.profile.command.normalize(task.command_index, value) * 32.0)))
for value in feedback
)
)
bins = normalized
span = (
float(np.ptp([self.profile.command.normalize(task.command_index, value) for value in feedback]))
if feedback.size else 0.0
)
required_span = float(self.minimum_state_span_u8)
gap = max((right - left for left, right in zip(bins, bins[1:])), default=32)
maximum_gap = float(self.maximum_bin_gap)
observation = self._step_observation_metrics() observation = self._step_observation_metrics()
detection_rate = float(observation["tag_detection_rate"]) detection_rate = float(observation["tag_detection_rate"])
joint_frame_rate = float(observation["joint_frame_rate"]) joint_frame_rate = float(observation["joint_frame_rate"])
failures = [] failures = []
if len(rows) < int(self.minimum_sweep_frames): if len(rows) < int(self.minimum_sweep_frames):
failures.append(f"frames={len(rows)}") failures.append(f"frames={len(rows)}")
if feedback.size == 0 or float(np.ptp(feedback)) < float(self.minimum_state_span_u8): if feedback.size == 0 or span < required_span:
failures.append("feedback_span") failures.append("feedback_span")
if len(bins) < int(self.minimum_sweep_bins): if len(bins) < int(self.minimum_sweep_bins):
failures.append(f"bins={len(bins)}") failures.append(f"bins={len(bins)}")
if gap > int(self.maximum_bin_gap): if gap > maximum_gap:
failures.append(f"maximum_gap={gap}") failures.append(f"maximum_gap={gap}")
if detection_rate < float(self.minimum_detection_rate): if detection_rate < float(self.minimum_detection_rate):
role_rates = observation["tag_detection_rate_by_role"] role_rates = observation["tag_detection_rate_by_role"]
@@ -964,10 +1104,11 @@ class L6ThreeCameraCalibrationNode(Node):
attempt = retries + 2 attempt = retries + 2
self.retry_counts[key] = retries + 1 self.retry_counts[key] = retries + 1
task = self._task(str(step.task_key)) task = self._task(str(step.task_key))
start = task.start_u8 if step.direction == "decreasing" else task.end_u8 start = task.start_value if step.direction == "decreasing" else task.end_value
retry_speed = float(step.speed_u8) * (0.75 if attempt == 2 else 0.5 / 0.75)
replacement = [ replacement = [
MotionStep("retry_prepare", step.task_key, step.command_index, start, step.speed_u8, step.cycle, attempt=attempt), MotionStep("retry_prepare", step.task_key, step.command_index, start, retry_speed, step.cycle, attempt=attempt),
MotionStep("sweep", step.task_key, step.command_index, step.target_u8, step.speed_u8, step.cycle, step.direction, attempt), MotionStep("sweep", step.task_key, step.command_index, step.target_u8, retry_speed, step.cycle, step.direction, attempt),
] ]
self.steps[self.step_index + 1:self.step_index + 1] = replacement self.steps[self.step_index + 1:self.step_index + 1] = replacement
append_jsonl( append_jsonl(
@@ -1013,7 +1154,7 @@ class L6ThreeCameraCalibrationNode(Node):
) )
return return
if not self.started: if not self.started:
if len(self.latest_state_u8) == 6 and set(self.camera_matrices) == set( if len(self.latest_state_u8) == self.command_count and set(self.camera_matrices) == set(
self.profile.vision.view_names self.profile.vision.view_names
): ):
self.state = "READY" self.state = "READY"
@@ -1031,11 +1172,9 @@ class L6ThreeCameraCalibrationNode(Node):
if now - self.step_started_at > float(timeout): if now - self.step_started_at > float(timeout):
self._pause(f"motion_timeout:{self.reason}") self._pause(f"motion_timeout:{self.reason}")
return return
if len(self.latest_state_u8) != 6: if len(self.latest_state_u8) != self.command_count:
return return
target_state = [float(value) for value in self.baseline_command] target_state = list(self._target_command(step))
if step.command_index is not None:
target_state[step.command_index] = float(step.target_u8)
endpoint_errors = [ endpoint_errors = [
abs(value - target_state[index]) abs(value - target_state[index])
for index, value in enumerate(self.latest_state_u8) for index, value in enumerate(self.latest_state_u8)
@@ -1062,7 +1201,9 @@ class L6ThreeCameraCalibrationNode(Node):
# final target also rejects movement in the wrong direction. # final target also rejects movement in the wrong direction.
if ( if (
not math.isfinite(self.step_last_distance_u8) not math.isfinite(self.step_last_distance_u8)
or progress_distance <= self.step_last_distance_u8 - 0.5 or progress_distance <= self.step_last_distance_u8 - (
0.5 if self.command_unit == "u8" else 0.002
)
): ):
self.step_last_distance_u8 = progress_distance self.step_last_distance_u8 = progress_distance
self.step_last_progress_at = now self.step_last_progress_at = now
@@ -1113,7 +1254,11 @@ class L6ThreeCameraCalibrationNode(Node):
return return
self._publish_command(list(self.baseline_command)) self._publish_command(list(self.baseline_command))
self.state = "PASSED" self.state = "PASSED"
self.reason = "partial_calibration_passed" self.reason = (
"partial_calibration_passed"
if self.profile.scope.default_scope != "full"
else "full_calibration_passed"
)
self.final_json = str( self.final_json = str(
self.session_dir self.session_dir
/ self.profile.artifacts.calibration_filename.format( / self.profile.artifacts.calibration_filename.format(
@@ -1130,16 +1275,20 @@ class L6ThreeCameraCalibrationNode(Node):
self.reason = str(reason) self.reason = str(reason)
# Holding the latest position is safer than issuing an automatic move # Holding the latest position is safer than issuing an automatic move
# after a stall, unexpected motor motion, or moved palm reference. # after a stall, unexpected motor motion, or moved palm reference.
if len(self.latest_state_u8) == 6: if len(self.latest_state_u8) == self.command_count:
self._publish_command( self._publish_command(
[int(np.clip(round(value), 0, 255)) for value in self.latest_state_u8] [
int(np.clip(round(value), 0, 255))
if self.command_unit == "u8" else float(value)
for value in self.latest_state_u8
]
) )
append_jsonl(self.raw_path, {"kind": "paused", "reason": self.reason}) append_jsonl(self.raw_path, {"kind": "paused", "reason": self.reason})
def _status(self) -> dict[str, Any]: def _status(self) -> dict[str, Any]:
step = self._current_step() step = self._current_step()
feedback = float("nan") feedback = float("nan")
if len(self.latest_state_u8) == 6: if len(self.latest_state_u8) == self.command_count:
feedback = ( feedback = (
float(np.mean(self.latest_state_u8)) float(np.mean(self.latest_state_u8))
if step is None or step.command_index is None if step is None or step.command_index is None
@@ -1148,10 +1297,8 @@ class L6ThreeCameraCalibrationNode(Node):
target_state: list[float] = [] target_state: list[float] = []
channel_errors: list[float] = [] channel_errors: list[float] = []
if step is not None: if step is not None:
target_state = [float(value) for value in self.baseline_command] target_state = list(self._target_command(step))
if step.command_index is not None: if len(self.latest_state_u8) == self.command_count:
target_state[step.command_index] = float(step.target_u8)
if len(self.latest_state_u8) == 6:
channel_errors = [ channel_errors = [
abs(value - target_state[index]) abs(value - target_state[index])
for index, value in enumerate(self.latest_state_u8) for index, value in enumerate(self.latest_state_u8)
@@ -1186,8 +1333,19 @@ class L6ThreeCameraCalibrationNode(Node):
"direction": None if step is None else step.direction, "direction": None if step is None else step.direction,
"attempt": None if step is None else step.attempt, "attempt": None if step is None else step.attempt,
"motor_index": None if step is None else step.command_index, "motor_index": None if step is None else step.command_index,
"speed_u8": None if step is None else int(step.speed_u8), "command_unit": self.command_unit,
"target_u8": None if step is None else step.target_u8, "speed_u8": (
None if step is None or self.command_unit != "u8" else int(step.speed_u8)
),
"speed_rad_s": (
None if step is None or self.command_unit != "rad" else round(float(step.speed_u8), 6)
),
"target_u8": (
None if step is None or self.command_unit != "u8" else step.target_u8
),
"target_rad": (
None if step is None or self.command_unit != "rad" else round(float(step.target_u8), 9)
),
"current_command_u8": ( "current_command_u8": (
None None
if step is None or not math.isfinite(self.step_requested_u8) if step is None or not math.isfinite(self.step_requested_u8)
@@ -1235,23 +1393,24 @@ class L6ThreeCameraCalibrationNode(Node):
self.fixed_base_maximum_corner_drift_px self.fixed_base_maximum_corner_drift_px
), ),
"feedback_hz": round(self._feedback_hz(), 3), "feedback_hz": round(self._feedback_hz(), 3),
"camera_info_views": sorted(self.camera_matrices),
"state_publisher_count": self.count_publishers(self.state_topic), "state_publisher_count": self.count_publishers(self.state_topic),
"command_publisher_count": self.count_publishers(self.command_topic), "command_publisher_count": self.count_publishers(self.command_topic),
"command_names": list(self.command_names), "command_names": list(self.command_names),
"latest_state_u8": list(self.latest_state_u8), f"latest_state_{self.command_unit}": list(self.latest_state_u8),
"target_state_u8": [round(value, 3) for value in target_state], f"target_state_{self.command_unit}": [round(value, 6) for value in target_state],
"current_command_state_u8": ( f"current_command_state_{self.command_unit}": (
[] []
if self.step_last_command_u8 is None if self.step_last_command_u8 is None
else list(self.step_last_command_u8) else list(self.step_last_command_u8)
), ),
"channel_errors_u8": [round(value, 3) for value in channel_errors], f"channel_errors_{self.command_unit}": [round(value, 6) for value in channel_errors],
"maximum_error_channel": ( "maximum_error_channel": (
None None
if not channel_errors if not channel_errors
else self.command_names[int(np.argmax(channel_errors))] else self.command_names[int(np.argmax(channel_errors))]
), ),
"maximum_error_u8": ( f"maximum_error_{self.command_unit}": (
None if not channel_errors else round(max(channel_errors), 3) None if not channel_errors else round(max(channel_errors), 3)
), ),
"final_json": getattr(self, "final_json", ""), "final_json": getattr(self, "final_json", ""),
@@ -48,6 +48,12 @@ _PHASE_LABELS = {
"prepare": "扫描起点准备", "prepare": "扫描起点准备",
"retry_prepare": "自动重扫起点准备", "retry_prepare": "自动重扫起点准备",
"sweep": "正式扫描", "sweep": "正式扫描",
"clearance": "手指避让",
"clearance_outer": "小指/无名指避让",
"clearance_splay": "侧摆避让",
"return_splay_zero": "侧摆回零",
"return_middle_open": "展开中指",
"return_outer_open": "展开小指/无名指",
} }
_DIRECTION_LABELS = { _DIRECTION_LABELS = {
"decreasing": "递减", "decreasing": "递减",
@@ -207,10 +213,15 @@ def render_six_channel_progress_zh(
cycle_text = "-" if cycle is None else str(int(cycle) + 1) cycle_text = "-" if cycle is None else str(int(cycle) + 1)
direction = status.get("direction") direction = status.get("direction")
direction_text = _DIRECTION_LABELS.get(str(direction), "-") direction_text = _DIRECTION_LABELS.get(str(direction), "-")
target = status.get("target_u8") command_unit = str(status.get("command_unit", "u8"))
target = status.get("target_rad") if command_unit == "rad" else status.get("target_u8")
requested = status.get("current_command_u8", target) requested = status.get("current_command_u8", target)
actual = status.get("actual_u8") actual = status.get("actual_u8")
actual_text = "未知" if actual is None else f"{float(actual):.1f}" if command_unit == "rad":
requested = "未知" if requested is None else f"{float(requested):.3f} rad"
actual_text = "未知" if actual is None else f"{float(actual):.3f} rad"
else:
actual_text = "未知" if actual is None else f"{float(actual):.1f}"
valid = int(status.get("valid_frames", 0) or 0) valid = int(status.get("valid_frames", 0) or 0)
total = int(status.get("total_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) rate = float(status.get("tag_detection_rate", 0.0) or 0.0)
@@ -257,6 +268,15 @@ def render_six_channel_progress_zh(
lines.append( lines.append(
f"运动:速度档 {int(speed_u8)};全行程 {trajectory_seconds:.1f} 秒余弦轨迹" f"运动:速度档 {int(speed_u8)};全行程 {trajectory_seconds:.1f} 秒余弦轨迹"
) )
speed_rad_s = status.get("speed_rad_s")
if speed_rad_s is not None:
trajectory_seconds = float(
status.get("command_trajectory_duration_seconds", 0.0) or 0.0
)
lines.append(
f"运动:峰值 {float(speed_rad_s):.3f} rad/s"
f"本段 {trajectory_seconds:.1f} 秒余弦轨迹"
)
latest_state = status.get("latest_state_u8", []) latest_state = status.get("latest_state_u8", [])
command_names = status.get("command_names", []) command_names = status.get("command_names", [])
if ( if (
@@ -353,7 +373,6 @@ def _launch_command(
"hand_type": config.side, "hand_type": config.side,
"tag_layout": config.tag_layout, "tag_layout": config.tag_layout,
"serial_number": config.serial_number, "serial_number": config.serial_number,
"can_interface": config.can_interface,
"source_urdf_path": str(config.source_urdf), "source_urdf_path": str(config.source_urdf),
"source_urdf_expected_sha256": config.source_urdf_sha256, "source_urdf_expected_sha256": config.source_urdf_sha256,
"camera_extrinsics_file": str(config.camera_extrinsics), "camera_extrinsics_file": str(config.camera_extrinsics),
@@ -362,16 +381,23 @@ def _launch_command(
"calibration_config_expected_sha256": config.calibration_config_sha256, "calibration_config_expected_sha256": config.calibration_config_sha256,
"tag_config": str(config.tag_config), "tag_config": str(config.tag_config),
"tag_config_expected_sha256": config.tag_config_sha256, "tag_config_expected_sha256": config.tag_config_sha256,
"vendor_sdk_config": "" if config.sdk_config is None else str(config.sdk_config),
"sdk_config_expected_sha256": config.sdk_config_sha256,
"output_root": str(config.output_root), "output_root": str(config.output_root),
"session_dir": str(session), "session_dir": str(session),
"corrected_urdf_output_dir": str(session), "corrected_urdf_output_dir": str(session),
"recalibration_scope": "partial", "recalibration_scope": config.calibration_contract.typed_profile.scope.default_scope,
"calibration_speed": str(int(sdk_startup_speed_u8)), "calibration_speed": str(int(sdk_startup_speed_u8)),
"index_roll_calibration_speed": "1", "index_roll_calibration_speed": "1",
"index_flex_calibration_speed": "1", "index_flex_calibration_speed": "1",
"commands_enabled": str(commands_enabled).lower(), "commands_enabled": str(commands_enabled).lower(),
"record_bag": str(record_bag).lower(), "record_bag": str(record_bag).lower(),
} }
# HCAN/ZLG vendor products do not have a Linux SocketCAN interface.
# Omitting the launch override also avoids the invalid token
# ``can_interface:=`` when the reviewed product value is intentionally empty.
if config.can_interface:
arguments["can_interface"] = config.can_interface
for view, camera in config.cameras.items(): for view, camera in config.cameras.items():
arguments[f"{view}_camera_serial"] = camera["serial_number"] arguments[f"{view}_camera_serial"] = camera["serial_number"]
arguments[f"{view}_camera_name"] = camera["camera_name"] arguments[f"{view}_camera_name"] = camera["camera_name"]
@@ -0,0 +1,11 @@
"""Registered O12 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,313 @@
"""Schema-v7 radian-knot runtime artifact for O12."""
from __future__ import annotations
import math
from pathlib import Path
from typing import Any, Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from .fitting import O12FitResult, curve_input_knots_rad, curve_values_at_rad
from .profile import (
ACTIVE_JOINTS,
COMMAND_INDEX_BY_JOINT,
COMMAND_NAMES,
KEY,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
PASSIVE_JOINTS,
SAFE_LOWER_RAD,
SAFE_UPPER_RAD,
SDK_LOWER_RAD,
SDK_TO_URDF_JOINT,
SDK_UPPER_RAD,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
build_typed_profile,
)
ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS)
def _motor_index(joint: str) -> int:
source = str(joint)
while source not in COMMAND_INDEX_BY_JOINT:
source = MIMIC_SOURCE_BY_JOINT[source]
return COMMAND_INDEX_BY_JOINT[source]
def _task_for_joint(joint: str):
target = str(joint)
while True:
if target in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT:
target = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT[target]
elif target not in COMMAND_INDEX_BY_JOINT:
target = MIMIC_SOURCE_BY_JOINT[target]
else:
break
return next(
task for task in build_typed_profile().motion.tasks
if target in task.joints
)
def _source_joint_metadata(source_urdf: str | Path) -> dict[str, dict[str, Any]]:
root = ET.parse(Path(source_urdf)).getroot()
result: dict[str, dict[str, Any]] = {}
for node in root.findall("joint"):
name = str(node.get("name", ""))
if name not in ALL_REVOLUTE_JOINTS:
continue
limit = node.find("limit")
if node.get("type") != "revolute" or limit is None:
raise ValueError(f"invalid O12 revolute joint: {name}")
item: dict[str, Any] = {
"lower": float(limit.get("lower", "nan")),
"upper": float(limit.get("upper", "nan")),
}
mimic = node.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 must contain all 19 O12 revolute joints")
return result
def _round(values: Sequence[float], digits: int = 10) -> list[float]:
array = np.asarray(values, dtype=float)
if array.shape != (65,) or not np.all(np.isfinite(array)):
raise ValueError("O12 knot curve must contain 65 finite values")
return [round(float(value), digits) for value in array]
def _zeroed(values: Sequence[float], inputs: Sequence[float]) -> np.ndarray:
curve = np.asarray(values, dtype=float)
baseline = float(np.interp(0.0, np.asarray(inputs, dtype=float), curve))
return curve - baseline
def build_o12_runtime_payload(
*,
serial_number: str,
source_urdf: str | Path,
result: O12FitResult,
protected_inputs: Mapping[str, str],
passed: bool,
) -> dict[str, Any]:
profile = build_typed_profile()
source = _source_joint_metadata(source_urdf)
curves: dict[str, tuple[np.ndarray, np.ndarray, np.ndarray, list[float]]] = {}
joints: dict[str, dict[str, Any]] = {}
for name in ACTIVE_JOINTS:
donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
fit = result.curves[donor]
inputs = list(curve_input_knots_rad(donor))
decreasing = _zeroed(
curve_values_at_rad(donor, fit, inputs, "decreasing_rad"), inputs
)
increasing = _zeroed(
curve_values_at_rad(donor, fit, inputs, "increasing_rad"), inputs
)
angle = 0.5 * (decreasing + increasing)
curves[name] = (angle, decreasing, increasing, inputs)
motor = COMMAND_INDEX_BY_JOINT[name]
joint: dict[str, Any] = {
"urdf_joint": name,
"sdk_channel": COMMAND_NAMES[motor],
"motor_index": motor,
"passive": False,
"calibration_status": profile.joint_coverage[name],
"curve_input_knots_rad": _round(inputs),
"angle_rad": _round(angle),
"decreasing_rad": _round(decreasing),
"increasing_rad": _round(increasing),
"zero_feedback_rad": 0.0,
"urdf_sign_adapter": (
"sdk_negative_to_urdf_positive"
if SDK_UPPER_RAD[motor] <= 0.0 and SDK_LOWER_RAD[motor] < 0.0
else "identity"
),
"raw_increasing_curve_branch": (
"decreasing"
if _task_for_joint(donor).end_value > _task_for_joint(donor).start_value
else "increasing"
),
}
if donor != name:
joint["transferred_from_joint"] = donor
joint["transfer_policy"] = "pinky_feedback_correction_on_ring_cad"
joints[name] = joint
for name in PASSIVE_JOINTS:
metadata = source[name]
source_name = MIMIC_SOURCE_BY_JOINT[name]
motor = _motor_index(source_name)
if name in MEASURED_PASSIVE_JOINTS:
fit = result.curves[name]
inputs = list(curve_input_knots_rad(name))
decreasing = _zeroed(
curve_values_at_rad(name, fit, inputs, "decreasing_rad"), inputs
) + float(metadata["offset"])
increasing = _zeroed(
curve_values_at_rad(name, fit, inputs, "increasing_rad"), inputs
) + float(metadata["offset"])
angle = 0.5 * (decreasing + increasing)
coupling = result.mimic_fits[name]
coefficients = [float(metadata["offset"]), *coupling.coefficients]
coefficients.extend([0.0] * (6 - len(coefficients)))
coupling_model = coupling.model
multiplier = coupling.urdf_mimic_multiplier
policy = coupling.urdf_mimic_policy
status = profile.joint_coverage[name]
else:
source_curve, source_dec, source_inc, inputs = curves[source_name]
multiplier = float(metadata["multiplier"])
offset = float(metadata["offset"])
angle = offset + multiplier * source_curve
decreasing = offset + multiplier * source_dec
increasing = offset + multiplier * source_inc
coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0]
coupling_model = "linear_mimic"
policy = "cad_nominal_preserved"
status = profile.joint_coverage[name]
curves[name] = (angle, decreasing, increasing, list(inputs))
joints[name] = {
"urdf_joint": name,
"sdk_channel": COMMAND_NAMES[motor],
"motor_index": motor,
"passive": True,
"source_joint": source_name,
"calibration_status": status,
"static_zero_policy": "cad_preserved_tag_mount_ambiguous",
"curve_input_knots_rad": _round(inputs),
"angle_rad": _round(angle),
"decreasing_rad": _round(decreasing),
"increasing_rad": _round(increasing),
"coupling_model": coupling_model,
"coupling_coefficients": [round(float(v), 10) for v in coefficients],
"mimic_multiplier": round(float(multiplier), 10),
"urdf_mimic_policy": policy,
"raw_increasing_curve_branch": (
"decreasing"
if _task_for_joint(name).end_value > _task_for_joint(name).start_value
else "increasing"
),
}
errors = np.abs(np.concatenate([
np.asarray(values, dtype=float)
for values in result.holdout_errors_rad.values()
]))
payload: dict[str, Any] = {
"schema_version": 7,
"profile_id": KEY.profile_id,
"layout_id": KEY.layout,
"model": "O12",
"side": "right",
"serial_number": str(serial_number),
"calibration_scope": "full",
"publication_pointer": "latest_passed",
"angle_unit": "rad",
"curve_input_domain": "feedback_rad",
"runtime_curve_policy": "direction_aware_knots",
"command_names": list(COMMAND_NAMES),
"command_lower_rad": list(SAFE_LOWER_RAD),
"command_upper_rad": list(SAFE_UPPER_RAD),
"baseline_command_rad": [0.0] * 12,
"sdk_mapping": [
{
"motor_index": i,
"sdk_channel": command,
"urdf_joint": urdf,
"sdk_lower_rad": SDK_LOWER_RAD[i],
"sdk_upper_rad": SDK_UPPER_RAD[i],
"calibration_lower_rad": SAFE_LOWER_RAD[i],
"calibration_upper_rad": SAFE_UPPER_RAD[i],
}
for i, (command, urdf) in enumerate(zip(COMMAND_NAMES, SDK_TO_URDF_JOINT))
],
"protected_inputs": dict(protected_inputs),
"joints": joints,
"quality": {
"passed": bool(passed),
"training_cycles": [0, 1, 2],
"holdout_cycle": 3,
"validation_mae_rad": round(float(np.mean(errors)), 10),
"validation_p95_rad": round(float(np.percentile(errors, 95.0)), 10),
"validation_max_rad": round(float(np.max(errors)), 10),
"ring_transfer_source": "pinky_mcp_pitch",
"ring_cad_geometry_and_mimic_preserved": True,
},
}
validate_o12_runtime_payload(payload)
return payload
def validate_o12_runtime_payload(payload: Mapping[str, Any]) -> None:
required = {
"schema_version", "profile_id", "layout_id", "model", "side",
"serial_number", "calibration_scope", "publication_pointer",
"angle_unit", "curve_input_domain", "runtime_curve_policy",
"command_names", "command_lower_rad", "command_upper_rad",
"baseline_command_rad", "sdk_mapping", "protected_inputs", "joints",
"quality",
}
if set(payload) != required:
raise ValueError("schema v7 O12 artifact has unexpected top-level fields")
if (
payload["schema_version"] != 7
or payload["profile_id"] != KEY.profile_id
or payload["model"] != "O12"
or payload["side"] != "right"
or payload["angle_unit"] != "rad"
or payload["curve_input_domain"] != "feedback_rad"
or payload["runtime_curve_policy"] != "direction_aware_knots"
):
raise ValueError("schema v7 O12 artifact identity/domain is invalid")
if tuple(payload["command_names"]) != COMMAND_NAMES:
raise ValueError("O12 SDK channel order differs from fixed contract")
if len(payload["sdk_mapping"]) != 12 or any(
int(row.get("motor_index", -1)) != i
or row.get("sdk_channel") != COMMAND_NAMES[i]
or row.get("urdf_joint") != SDK_TO_URDF_JOINT[i]
or float(row.get("sdk_lower_rad", math.nan)) != SDK_LOWER_RAD[i]
or float(row.get("sdk_upper_rad", math.nan)) != SDK_UPPER_RAD[i]
or float(row.get("calibration_lower_rad", math.nan)) != SAFE_LOWER_RAD[i]
or float(row.get("calibration_upper_rad", math.nan)) != SAFE_UPPER_RAD[i]
for i, row in enumerate(payload["sdk_mapping"])
):
raise ValueError("O12 SDK/URDF mapping differs from fixed contract")
protected = payload["protected_inputs"]
expected = build_typed_profile().artifacts.protected_input_fields
if not isinstance(protected, Mapping) or set(protected) != expected:
raise ValueError("O12 protected input hashes are incomplete")
if any(len(str(v)) != 64 or any(c not in "0123456789abcdef" for c in str(v)) for v in protected.values()):
raise ValueError("O12 protected input hash is invalid")
joints = payload["joints"]
if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS:
raise ValueError("schema v7 must contain all 19 O12 revolute joints")
for name, joint in joints.items():
inputs = np.asarray(joint.get("curve_input_knots_rad"), dtype=float)
if inputs.shape != (65,) or not np.all(np.isfinite(inputs)) or np.any(np.diff(inputs) <= 0.0):
raise ValueError(f"{name} has invalid radian knots")
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
values = np.asarray(joint.get(field), dtype=float)
if values.shape != (65,) or not np.all(np.isfinite(values)):
raise ValueError(f"{name}.{field} must contain 65 finite values")
if int(joint.get("motor_index", -1)) != _motor_index(name):
raise ValueError(f"{name} has invalid O12 motor binding")
if joint.get("raw_increasing_curve_branch") not in {"decreasing", "increasing"}:
raise ValueError(f"{name} has invalid O12 direction binding")
if not bool(payload["quality"].get("passed")):
raise ValueError("failed O12 calibration cannot be published")
__all__ = ["ALL_REVOLUTE_JOINTS", "build_o12_runtime_payload", "validate_o12_runtime_payload"]
@@ -0,0 +1,181 @@
"""Direction-aware fitting for O12 feedback expressed in radians."""
from __future__ import annotations
from dataclasses import dataclass
import math
from pathlib import Path
from typing import Any, Mapping, Sequence
import numpy as np
from ..g20.profile import JointCurveFit
from ..g20.zero_solver import fit_rotation_joint_curve, rotation_curve_holdout_errors
from ..l6.fitting import MimicFit, fit_coupling_model
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
COMMAND_INDEX_BY_JOINT,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
build_typed_profile,
)
@dataclass(frozen=True)
class O12FitResult:
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, ...]]
def _task_by_joint() -> dict[str, Any]:
return {
joint: task
for task in build_typed_profile().motion.tasks
for joint in task.joints
}
def feedback_rad_to_curve_index(joint: str, feedback_rad: float) -> float:
"""Map a physical feedback angle to the fitter's normalized 255..0 axis."""
task = _task_by_joint()[str(joint)]
denominator = task.end_value - task.start_value
if abs(denominator) <= 1.0e-12:
raise ValueError(f"O12 task {task.key} has a degenerate range")
phase = (float(feedback_rad) - task.start_value) / denominator
return 255.0 * (1.0 - float(np.clip(phase, 0.0, 1.0)))
def curve_input_knots_rad(joint: str, count: int = 65) -> tuple[float, ...]:
task = _task_by_joint()[str(joint)]
return tuple(
float(value)
for value in np.linspace(
min(task.start_value, task.end_value),
max(task.start_value, task.end_value),
int(count),
)
)
def curve_values_at_rad(
joint: str, fit: JointCurveFit, inputs_rad: Sequence[float], branch: str
) -> tuple[float, ...]:
values = np.asarray(getattr(fit, branch), dtype=float)
indices = np.arange(256, dtype=float)
return tuple(
float(np.interp(feedback_rad_to_curve_index(joint, value), indices, values))
for value in inputs_rad
)
def _virtual_records(
joint: str, rows: Sequence[Mapping[str, Any]]
) -> list[dict[str, Any]]:
result = []
for source in rows:
row = dict(source)
curve_index = feedback_rad_to_curve_index(joint, float(row["feedback_rad"]))
row["command_u8"] = int(np.clip(round(curve_index), 0, 255))
result.append(row)
return result
def _travel(fit: JointCurveFit) -> float:
return 0.5 * (
float(fit.decreasing_rad[0] - fit.decreasing_rad[255])
+ float(fit.increasing_rad[0] - fit.increasing_rad[255])
)
def fit_o12_session(
source_urdf: str | Path,
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
) -> O12FitResult:
del source_urdf # topology is validated by the immutable product loader
expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
if set(records_by_joint) != expected:
missing = sorted(expected - set(records_by_joint))
extra = sorted(set(records_by_joint) - expected)
raise ValueError(f"O12 records differ from profile: missing={missing} extra={extra}")
curves: dict[str, JointCurveFit] = {}
holdout: dict[str, tuple[float, ...]] = {}
cycle_curves: dict[str, dict[int, JointCurveFit]] = {}
for joint in sorted(expected):
rows = _virtual_records(joint, records_by_joint[joint])
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"{joint} is missing training or holdout records")
fit = fit_rotation_joint_curve(
training, zero_command_u8=255, canonical_zero_direction="decreasing"
)
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"{joint} isolated holdout failed")
if fit.maximum_hysteresis_rad > math.radians(2.0):
raise ValueError(f"{joint} hysteresis exceeds 2 degrees")
curves[joint] = fit
holdout[joint] = tuple(float(value) for value in errors)
cycle_curves[joint] = {
cycle: fit_rotation_joint_curve(
[row for row in training if int(row["cycle"]) == cycle],
zero_command_u8=255,
canonical_zero_direction="decreasing",
)
for cycle in (0, 1, 2)
}
mimic_fits: dict[str, MimicFit] = {}
for target in sorted(MEASURED_PASSIVE_JOINTS):
source = MIMIC_SOURCE_BY_JOINT[target]
cycle_pairs = [
(
tuple(cycle_curves[source][cycle].decreasing_rad)
+ tuple(cycle_curves[source][cycle].increasing_rad),
tuple(cycle_curves[target][cycle].decreasing_rad)
+ tuple(cycle_curves[target][cycle].increasing_rad),
)
for cycle in (0, 1, 2)
]
mimic_fits[target] = fit_coupling_model(
source,
target,
curves[source],
curves[target],
model="quadratic_runtime",
cycle_curve_pairs=cycle_pairs,
minimum_multiplier=0.5,
maximum_multiplier=2.2,
)
travels = {
joint: _travel(curves[joint]) for joint in CALIBRATED_ACTIVE_JOINTS
}
for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items():
travels[target] = travels[donor]
# O12 active feedback is already a physical angle with open/centred zero.
# Calibration publishes the visual transfer curves and measured travel;
# it must not invent a static offset from an arbitrary Tag mounting angle.
offsets = {joint: 0.0 for joint in travels}
return O12FitResult(
curves=curves,
zero_offsets_rad=offsets,
travels_rad=travels,
mimic_fits=mimic_fits,
holdout_errors_rad=holdout,
)
__all__ = [
"O12FitResult", "curve_input_knots_rad", "curve_values_at_rad",
"feedback_rad_to_curve_index", "fit_o12_session",
]
@@ -0,0 +1,76 @@
"""Physical-radian motion helpers for O12 right calibration."""
from __future__ import annotations
import math
from typing import Sequence
from ...core import CalibrationProfile, TaskSpec
def cosine_position_trajectory_rad(
start_rad: float,
target_rad: float,
elapsed_seconds: float,
maximum_speed_rad_s: float,
) -> tuple[float, float, float]:
"""Cosine trajectory whose peak velocity does not exceed the limit."""
speed = float(maximum_speed_rad_s)
if not math.isfinite(speed) or speed <= 0.0:
raise ValueError("maximum_speed_rad_s must be positive")
distance = abs(float(target_rad) - float(start_rad))
if distance <= 0.0:
return float(target_rad), 1.0, 0.0
duration = math.pi * distance / (2.0 * speed)
phase = min(1.0, max(0.0, float(elapsed_seconds) / duration))
blend = 0.5 - 0.5 * math.cos(math.pi * phase)
return (
float(start_rad) + (float(target_rad) - float(start_rad)) * blend,
phase,
duration,
)
def build_calibration_motion_command(
task: TaskSpec,
command_rad: float,
*,
profile: CalibrationProfile,
) -> list[float]:
values = list(profile.command.baseline_values)
for index, value in task.auxiliary_commands:
values[int(index)] = float(value)
values[int(task.command_index)] = float(command_rad)
return values
def build_calibration_preparation_waypoints(
task: TaskSpec,
*,
profile: CalibrationProfile,
current_command: Sequence[float] | None = None,
) -> tuple[tuple[float, ...], ...]:
del current_command
return (tuple(build_calibration_motion_command(task, task.start_value, profile=profile)),)
def build_calibration_return_waypoints(
target_command: Sequence[float] | None = None,
*,
profile: CalibrationProfile,
current_command: Sequence[float] | None = None,
**_: object,
) -> tuple[tuple[float, ...], ...]:
del current_command
target = tuple(
profile.command.baseline_values if target_command is None else target_command
)
if len(target) != profile.command.command_count:
raise ValueError("O12 return command has the wrong channel count")
return (tuple(float(value) for value in target),)
__all__ = [
"build_calibration_motion_command", "build_calibration_preparation_waypoints",
"build_calibration_return_waypoints", "cosine_position_trajectory_rad",
]
@@ -0,0 +1,369 @@
"""Safety-gated online acquisition node for O12 right."""
from __future__ import annotations
import math
import time
import numpy as np
import rclpy
from scipy.spatial.transform import Rotation
from std_msgs.msg import Empty, Int8MultiArray, Int16MultiArray
from ...storage import append_jsonl
from ..l6.node import L6ThreeCameraCalibrationNode, MotionStep
from .pipeline import finalize_o12_session
from .profile import (
INDEX_CLEARANCE_RAD,
PARK_FINGER_RAD,
PARK_MIDDLE_MCP_RAD,
PARK_MIDDLE_PIP_RAD,
build_typed_profile,
)
POSITION_MODE = 0
class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode):
def __init__(self) -> None:
super().__init__(
profile=build_typed_profile(),
finalizer=finalize_o12_session,
sample_kind="o12_joint_sample",
)
self.mode_verified = False
self.error_verified = False
self.temperature_verified = False
self.temperature_fallback_active = False
self.health_check_started_at = time.monotonic()
self.latest_errors: tuple[int, ...] = ()
self.latest_temperatures: tuple[int, ...] = ()
self.last_health_query_at = 0.0
self.preflight_reference_by_task: dict[str, Rotation] = {}
self.preflight_maximum_rotation_by_task: dict[str, float] = {}
self.measured_direction_axis_by_task: dict[str, tuple[float, float, float]] = {}
self.control_mode_publisher = self.create_publisher(
Int8MultiArray, "/o12/right/joint_control_mode_cmd", 10
)
self.error_query_publisher = self.create_publisher(
Empty, "/o12/right/joint_error_cmd", 10
)
self.temperature_query_publisher = self.create_publisher(
Empty, "/o12/right/joint_temperature_cmd", 10
)
self.create_subscription(
Int8MultiArray,
"/o12/right/joint_control_mode_states",
self._mode_callback,
10,
)
self.create_subscription(
Int16MultiArray,
"/o12/right/joint_error_states",
self._error_callback,
10,
)
self.create_subscription(
Int16MultiArray,
"/o12/right/joint_temperature_states",
self._temperature_callback,
10,
)
def _declare_parameters(self) -> None:
super()._declare_parameters()
self.declare_parameter("maximum_temperature_c", 70)
self.declare_parameter("temperature_report_required", False)
self.declare_parameter("temperature_fallback_after_seconds", 5.0)
self.declare_parameter("motion_speed_scale", 1.0)
def _load_parameters(self) -> None:
super()._load_parameters()
self.maximum_temperature_c = int(
self.get_parameter("maximum_temperature_c").value
)
self.temperature_report_required = bool(
self.get_parameter("temperature_report_required").value
)
self.temperature_fallback_after_seconds = float(
self.get_parameter("temperature_fallback_after_seconds").value
)
self.motion_speed_scale = float(
self.get_parameter("motion_speed_scale").value
)
if not 40 <= self.maximum_temperature_c <= 90:
raise ValueError("maximum_temperature_c must be in [40, 90]")
if not 1.0 <= self.temperature_fallback_after_seconds <= 30.0:
raise ValueError("temperature_fallback_after_seconds must be in [1, 30]")
if not 0.5 <= self.motion_speed_scale <= 2.0:
raise ValueError("motion_speed_scale must be in [0.5, 2.0]")
def _temperature_ready(self) -> bool:
return self.temperature_verified or self.temperature_fallback_active
def _activate_temperature_fallback_if_allowed(self, now: float) -> None:
if (
self.temperature_report_required
or self.temperature_verified
or self.temperature_fallback_active
or not self.error_verified
or now - self.health_check_started_at
< self.temperature_fallback_after_seconds
):
return
self.temperature_fallback_active = True
append_jsonl(self.raw_path, {
"kind": "o12_temperature_capability_fallback",
"temperature_report_received": False,
"fallback_protection": "joint_error_states_bit1_overheat",
"sdk_config_sha256": self.protected_inputs["sdk_config_sha256"],
})
self.get_logger().warning(
"O12 temperature report unavailable; continuing with error-code "
"bit1 overheat protection"
)
def _mode_callback(self, message: Int8MultiArray) -> None:
values = tuple(int(value) for value in message.data)
if len(values) != 12 or any(value != POSITION_MODE for value in values):
self._pause("control_mode_is_not_position")
return
self.mode_verified = True
def _error_callback(self, message: Int16MultiArray) -> None:
self.latest_errors = tuple(int(value) for value in message.data)
if len(self.latest_errors) != 12:
self._pause("invalid_error_report_length")
elif any(self.latest_errors):
self._pause("o12_error_report_nonzero")
else:
self.error_verified = True
def _temperature_callback(self, message: Int16MultiArray) -> None:
self.latest_temperatures = tuple(int(value) for value in message.data)
if len(self.latest_temperatures) != 12:
self._pause("invalid_temperature_report_length")
elif any(value >= self.maximum_temperature_c for value in self.latest_temperatures):
self._pause("o12_over_temperature")
else:
self.temperature_verified = True
self.temperature_fallback_active = False
def _query_health(self) -> None:
if not self.commands_enabled:
return
mode = Int8MultiArray()
mode.data = [POSITION_MODE] * 12
self.control_mode_publisher.publish(mode)
self.error_query_publisher.publish(Empty())
self.temperature_query_publisher.publish(Empty())
self.last_health_query_at = time.monotonic()
@staticmethod
def _full_target(**values: float) -> tuple[float, ...]:
index = {
"thumb_roll": 0, "thumb_abad": 1, "thumb_mcp": 2,
"thumb_pip": 3, "index_abad": 4, "index_mcp": 5,
"index_pip": 6, "middle_abad": 7, "middle_mcp": 8,
"middle_pip": 9, "ring_mcp": 10, "pinky_mcp": 11,
}
target = [0.0] * 12
for name, value in values.items():
target[index[name]] = float(value)
return tuple(target)
def _build_steps(self) -> list[MotionStep]:
def scaled(speed: float, maximum: float | None = None) -> float:
value = float(speed) * self.motion_speed_scale
return value if maximum is None else min(value, maximum)
steps: list[MotionStep] = [
MotionStep(
"baseline", None, None, 0.0, scaled(0.10, 0.15),
target_command=self._full_target(),
)
]
previous_group = "thumb"
for task in self.profile.motion.tasks:
group = task.key.split("_", 1)[0]
if group != previous_group:
if group == "middle":
outer = self._full_target(
ring_mcp=PARK_FINGER_RAD,
pinky_mcp=PARK_FINGER_RAD,
)
steps.append(MotionStep("clearance_outer", None, None, 0.0, scaled(0.10, 0.15), target_command=outer))
target = self._full_target(
index_abad=INDEX_CLEARANCE_RAD,
ring_mcp=PARK_FINGER_RAD,
pinky_mcp=PARK_FINGER_RAD,
)
steps.append(MotionStep("clearance_splay", None, None, 0.0, scaled(0.04, 0.08), target_command=target))
elif group == "index":
splay_zero = self._full_target(
ring_mcp=PARK_FINGER_RAD,
pinky_mcp=PARK_FINGER_RAD,
)
steps.append(MotionStep("clearance_splay", None, None, 0.0, scaled(0.04, 0.08), target_command=splay_zero))
target = self._full_target(
middle_mcp=PARK_MIDDLE_MCP_RAD,
middle_pip=PARK_MIDDLE_PIP_RAD,
ring_mcp=PARK_FINGER_RAD,
pinky_mcp=PARK_FINGER_RAD,
)
steps.append(MotionStep("clearance", None, None, 0.0, scaled(0.10, 0.15), target_command=target))
previous_group = group
probe_delta = math.copysign(
min(abs(task.end_value - task.start_value), math.radians(3.0)),
task.end_value - task.start_value,
)
for target in (task.start_value, task.start_value + probe_delta, task.start_value):
steps.append(MotionStep(
"preflight", task.key, task.command_index, target,
scaled(float(task.preflight_speed or 0.02), 0.04),
))
for cycle in (0, 1, 2, 3):
formal_speed = scaled(float(task.formal_speed))
steps.extend((
MotionStep("prepare", task.key, task.command_index, task.start_value, formal_speed, cycle),
MotionStep("sweep", task.key, task.command_index, task.end_value, formal_speed, cycle, "decreasing"),
MotionStep("prepare", task.key, task.command_index, task.end_value, formal_speed, cycle),
MotionStep("sweep", task.key, task.command_index, task.start_value, formal_speed, cycle, "increasing"),
))
# Collision-aware return: side-swing neutral, middle open, then outer pair.
middle_and_outer_parked = self._full_target(
middle_mcp=PARK_MIDDLE_MCP_RAD,
middle_pip=PARK_MIDDLE_PIP_RAD,
ring_mcp=PARK_FINGER_RAD, pinky_mcp=PARK_FINGER_RAD
)
outer_parked = self._full_target(
ring_mcp=PARK_FINGER_RAD, pinky_mcp=PARK_FINGER_RAD
)
steps.append(MotionStep("return_splay_zero", None, None, 0.0, scaled(0.04, 0.08), target_command=middle_and_outer_parked))
steps.append(MotionStep("return_middle_open", None, None, 0.0, scaled(0.10, 0.15), target_command=outer_parked))
steps.append(MotionStep("return_outer_open", None, None, 0.0, scaled(0.10, 0.15), target_command=self._full_target()))
return steps
def _start(self, request, response):
if not (self.mode_verified and self.error_verified and self._temperature_ready()):
response.success = False
response.message = "O12 POSITION mode/error/temperature precheck is incomplete"
return response
return super()._start(request, response)
def _observe_preflight_pose(self, view, selected, step: MotionStep) -> None:
task = self._task(str(step.task_key))
if task.view != view:
return
measurement = self.profile.measurement.measurements[task.joints[0]]
parent = Rotation.from_quat(
selected[str(measurement.parent_role)].quaternion_xyzw
)
child = Rotation.from_quat(
selected[str(measurement.child_role)].quaternion_xyzw
)
relative = parent.inv() * child
if abs(float(step.target_u8) - task.start_value) <= 1.0e-9:
self.preflight_reference_by_task.setdefault(task.key, relative)
return
reference = self.preflight_reference_by_task.get(task.key)
if reference is None:
return
vector = (reference.inv() * relative).as_rotvec()
magnitude = float(np.linalg.norm(vector))
if magnitude > self.preflight_maximum_rotation_by_task.get(task.key, 0.0):
self.preflight_maximum_rotation_by_task[task.key] = magnitude
if magnitude > 1.0e-9:
axis = vector / magnitude
self.measured_direction_axis_by_task[task.key] = tuple(
float(value) for value in axis
)
def _finish_step(self, step: MotionStep) -> None:
previous_task = step.task_key
if step.phase == "preflight" and step.task_key is not None:
task = self._task(step.task_key)
is_probe = abs(float(step.target_u8) - task.start_value) > 1.0e-9
if is_probe:
rotation = self.preflight_maximum_rotation_by_task.get(task.key, 0.0)
if rotation < math.radians(0.25):
self._pause(f"mapping_preflight_no_target_motion:{task.key}")
return
append_jsonl(self.raw_path, {
"kind": "o12_fixed_mapping_preflight",
"task_name": task.key,
"motor_index": task.command_index,
"sdk_channel": self.command_names[task.command_index],
"urdf_joint": task.joints[0],
"command_delta_rad": round(float(step.target_u8) - task.start_value, 9),
"observed_rotation_rad": round(rotation, 9),
"observed_axis_tag_frame_xyz": list(
self.measured_direction_axis_by_task[task.key]
),
"channel_exchange_allowed": False,
})
super()._finish_step(step)
next_step = self._current_step()
if self.state == "RUNNING" and (
next_step is None
or next_step.phase.startswith("clearance")
or (previous_task and next_step.task_key and previous_task.split("_", 1)[0] != next_step.task_key.split("_", 1)[0])
):
self._query_health()
def _tick(self) -> None:
now = time.monotonic()
self._activate_temperature_fallback_if_allowed(now)
if not self.started and self.state not in {"PAUSED", "ABORTED", "PASSED"}:
if now - self.last_health_query_at >= 1.0:
self._query_health()
# The SDK's joint state is command-triggered; a 20 Hz zero command
# supplies the feedback stream used by READY and frequency checks.
self._publish_command(list(self.baseline_command))
super()._tick()
if (
not self.started
and self.state == "READY"
and not (self.mode_verified and self.error_verified and self._temperature_ready())
):
self.state = "WAIT_DEVICES"
self.reason = "waiting_for_o12_position_mode_and_health_reports"
def _status(self):
value = super()._status()
value.update({
"position_mode_verified": self.mode_verified,
"error_report_verified": self.error_verified,
"temperature_report_verified": self.temperature_verified,
"temperature_fallback_active": self.temperature_fallback_active,
"temperature_safety_policy": (
"direct_temperature_report"
if self.temperature_verified
else "error_bitmask_bit1_overheat"
if self.temperature_fallback_active
else "waiting_for_temperature_report"
),
"latest_error_codes": list(self.latest_errors),
"latest_temperatures_c": list(self.latest_temperatures),
"motion_speed_scale": self.motion_speed_scale,
})
return value
def main(args: list[str] | None = None) -> None:
rclpy.init(args=args)
node: O12ThreeCameraCalibrationNode | None = None
try:
node = O12ThreeCameraCalibrationNode()
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
__all__ = ["O12ThreeCameraCalibrationNode", "POSITION_MODE", "main"]
@@ -0,0 +1,145 @@
"""Shared online/offline O12 session finalization."""
from __future__ import annotations
from datetime import datetime
import json
import os
from pathlib import Path
from typing import Any, Mapping, Sequence
from ...product import sha256_file
from ...storage import atomic_write_json
from .artifacts import build_o12_runtime_payload
from .fitting import O12FitResult, fit_o12_session
from .profile import CALIBRATED_ACTIVE_JOINTS, MEASURED_PASSIVE_JOINTS
from .urdf import O12UrdfCorrection, write_o12_corrected_urdf
MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
def accepted_records_by_joint(
records: Sequence[Mapping[str, Any]],
) -> dict[str, list[dict[str, Any]]]:
samples = [
dict(row) for row in records
if row.get("kind") == "o12_joint_sample"
and str(row.get("joint", "")) in MEASURED_JOINTS
]
latest: dict[tuple[str, int, str], int] = {}
for row in samples:
key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"]))
latest[key] = max(latest.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[key]:
continue
accepted: dict[str, Any] = {
"cycle": int(row["cycle"]),
"direction": str(row["direction"]),
"feedback_rad": float(row["feedback_rad"]),
"relative_quaternion_xyzw": list(row["relative_quaternion_xyzw"]),
}
for field in (
"relative_translation_xyz_m", "parent_pose_common",
"child_pose_common", "view_normal_common_xyz",
"camera_center_common_xyz_m", "state_rad",
):
if field in row:
value = row[field]
accepted[field] = dict(value) if isinstance(value, Mapping) else list(value)
result[str(row["joint"])].append(accepted)
return result
def load_o12_raw_samples(path: str | Path) -> list[dict[str, Any]]:
source = Path(path).expanduser().resolve()
if not source.is_file():
raise ValueError(f"raw O12 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:
item = json.loads(line)
except json.JSONDecodeError as error:
raise ValueError(f"invalid O12 JSONL record at line {line_number}") from error
if not isinstance(item, Mapping):
raise ValueError(f"O12 JSONL line {line_number} is not an object")
rows.append(dict(item))
return rows
def _publish(serial_root: Path, session: Path) -> Path:
if session.parent != serial_root or not session.is_dir():
raise ValueError("O12 session must be a direct existing child")
destination = serial_root / "latest_passed"
temporary = serial_root / f".latest_passed.{os.getpid()}.tmp"
if temporary.exists() or temporary.is_symlink():
temporary.unlink()
os.symlink(session.name, temporary, target_is_directory=True)
os.replace(temporary, destination)
return destination
def finalize_o12_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], O12FitResult, O12UrdfCorrection]:
directory = Path(session_dir).expanduser().resolve()
directory.mkdir(parents=True, exist_ok=True)
result = fit_o12_session(source_urdf, accepted_records_by_joint(records))
payload = build_o12_runtime_payload(
serial_number=serial_number,
source_urdf=source_urdf,
result=result,
protected_inputs=protected_inputs,
passed=True,
)
correction = write_o12_corrected_urdf(
source_urdf=source_urdf,
output_directory=directory,
serial_number=serial_number,
result=result,
timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"),
)
json_path = directory / f"o12_right_{serial_number}_calibration.json"
atomic_write_json(json_path, payload)
summary = {
"schema_version": 1,
"profile_id": "O12/right/o12_right_16/v1",
"serial_number": str(serial_number),
"result": "PASS",
"measured_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS),
"measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS),
"ring_transfer": {
"source": "pinky_mcp_pitch",
"target": "ring_mcp_pitch",
"preserved_fields": list(correction.preserved_ring_fields),
},
"artifacts": {
"json": json_path.name,
"urdf": correction.path.name,
"calibration_json_sha256": sha256_file(json_path),
"corrected_urdf_sha256": sha256_file(correction.path),
},
}
atomic_write_json(directory / "calibration_summary_zh.json", summary)
if publish:
_publish(directory.parent, directory)
return payload, result, correction
__all__ = [
"MEASURED_JOINTS", "accepted_records_by_joint", "finalize_o12_session",
"load_o12_raw_samples",
]
@@ -0,0 +1,344 @@
"""Reviewed O12 right-hand 16-Tag calibration profile."""
from __future__ import annotations
import math
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("O12", "right", "o12_right_16", 1)
# sensor_msgs/JointState.position active-angle order from API_CPP_O12.md.
COMMAND_NAMES: tuple[str, ...] = (
"thumb_roll",
"thumb_abad",
"thumb_mcp",
"thumb_pip",
"index_abad",
"index_mcp",
"index_pip",
"middle_abad",
"middle_mcp",
"middle_pip",
"ring_mcp",
"pinky_mcp",
)
SDK_LOWER_RAD = (
0.0, -1.387536755335492, -0.8272860654453121, -1.2915436464758039,
-0.2617993877991494, 0.0, 0.0, -0.2617993877991494,
0.0, 0.0, 0.0, 0.0,
)
SDK_UPPER_RAD = (
0.9424777960769379, 0.0, 0.0, 0.0,
0.2617993877991494, 1.3526301702956054, 1.530653753999027,
0.2617993877991494, 1.3578661580515883, 1.8151424220741028,
1.53588974175501, 1.53588974175501,
)
# Source-URDF-safe intersections. The SDK exposes a larger range on several
# axes, but calibration must never command outside the immutable CAD model.
SAFE_LOWER_RAD = (
0.0, -0.94, -0.8272860654453121, -1.29,
-0.26, 0.0, 0.0, -0.26, 0.0, 0.0, 0.0, 0.0,
)
SAFE_UPPER_RAD = (
0.73, 0.0, 0.0, 0.0,
0.26, 1.33, 1.48, 0.26, 1.33, 1.71, 1.38, 1.38,
)
SDK_TO_URDF_JOINT: tuple[str, ...] = (
"thumb_cmc_roll",
"thumb_cmc_yaw",
"thumb_cmc_pitch",
"thumb_mcp",
"index_mcp_roll",
"index_mcp_pitch",
"index_pip",
"middle_mcp_roll",
"middle_mcp_pitch",
"middle_pip",
"ring_mcp_pitch",
"pinky_mcp_pitch",
)
ACTIVE_JOINTS = SDK_TO_URDF_JOINT
PASSIVE_JOINTS: tuple[str, ...] = (
"thumb_dip",
"index_dip",
"middle_dip",
"ring_pip",
"ring_dip",
"pinky_pip",
"pinky_dip",
)
CALIBRATED_ACTIVE_JOINTS = frozenset(ACTIVE_JOINTS) - {"ring_mcp_pitch"}
MEASURED_PASSIVE_JOINTS = frozenset(
{"thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip"}
)
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = {
"ring_mcp_pitch": "pinky_mcp_pitch",
}
MIMIC_SOURCE_BY_JOINT = {
"thumb_dip": "thumb_mcp",
"index_dip": "index_pip",
"middle_dip": "middle_pip",
"ring_pip": "ring_mcp_pitch",
"ring_dip": "ring_pip",
"pinky_pip": "pinky_mcp_pitch",
"pinky_dip": "pinky_pip",
}
COMMAND_INDEX_BY_JOINT = {
joint: index for index, joint in enumerate(SDK_TO_URDF_JOINT)
}
PARK_FINGER_RAD = 0.65 * 1.38
PARK_MIDDLE_MCP_RAD = 0.65 * 1.33
PARK_MIDDLE_PIP_RAD = 0.65 * 1.71
INDEX_CLEARANCE_RAD = -math.radians(10.0)
def _task(
key: str,
view: str,
index: int,
joints: tuple[str, ...],
*,
start: float,
end: float,
speed: float,
auxiliary: tuple[tuple[int, float], ...] = (),
) -> TaskSpec:
return TaskSpec(
key,
view,
index,
joints,
auxiliary_commands=auxiliary,
start=start,
end=end,
preflight_speed=min(speed, 0.02),
formal_speed=speed,
)
def build_typed_profile() -> CalibrationProfile:
middle_clearance = (
(4, INDEX_CLEARANCE_RAD),
(10, PARK_FINGER_RAD),
(11, PARK_FINGER_RAD),
)
index_clearance = (
(7, 0.0),
(8, PARK_MIDDLE_MCP_RAD),
(9, PARK_MIDDLE_PIP_RAD),
(10, PARK_FINGER_RAD),
(11, PARK_FINGER_RAD),
)
tasks = (
_task("thumb_pitch_front", "front", 2, ("thumb_cmc_pitch",), start=0.0, end=SAFE_LOWER_RAD[2], speed=0.03),
_task("thumb_roll_front", "front", 0, ("thumb_cmc_roll",), start=0.0, end=SAFE_UPPER_RAD[0], speed=0.04),
_task("thumb_mcp_dip_front", "front", 3, ("thumb_mcp", "thumb_dip"), start=0.0, end=SAFE_LOWER_RAD[3], speed=0.08),
_task("thumb_yaw_top", "top", 1, ("thumb_cmc_yaw",), start=0.0, end=SAFE_LOWER_RAD[1], speed=0.04),
_task("pinky_chain_side", "side", 11, ("pinky_mcp_pitch", "pinky_pip", "pinky_dip"), start=0.0, end=SAFE_UPPER_RAD[11], speed=0.08),
_task("middle_roll_front", "front", 7, ("middle_mcp_roll",), start=SAFE_UPPER_RAD[7], end=SAFE_LOWER_RAD[7], speed=0.04, auxiliary=middle_clearance),
_task("middle_mcp_side", "side", 8, ("middle_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[8], speed=0.08, auxiliary=middle_clearance),
_task("middle_pip_dip_side", "side", 9, ("middle_pip", "middle_dip"), start=0.0, end=SAFE_UPPER_RAD[9], speed=0.08, auxiliary=middle_clearance),
_task("index_roll_front", "front", 4, ("index_mcp_roll",), start=SAFE_UPPER_RAD[4], end=SAFE_LOWER_RAD[4], speed=0.04, auxiliary=index_clearance),
_task("index_mcp_side", "side", 5, ("index_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[5], speed=0.08, auxiliary=index_clearance),
_task("index_pip_dip_side", "side", 6, ("index_pip", "index_dip"), start=0.0, end=SAFE_UPPER_RAD[6], speed=0.08, auxiliary=index_clearance),
)
measurements = {
"thumb_cmc_pitch": MeasurementSpec("thumb_cmc_pitch", "relative_rotation", "front", "front_base", "thumb_cmc"),
"thumb_cmc_roll": MeasurementSpec("thumb_cmc_roll", "relative_rotation", "front", "front_base", "thumb_cmc"),
"thumb_mcp": MeasurementSpec("thumb_mcp", "relative_rotation", "front", "thumb_cmc", "thumb_mcp"),
"thumb_dip": MeasurementSpec("thumb_dip", "relative_rotation", "front", "thumb_mcp", "thumb_dip", pose_axis_line_required=False),
"thumb_cmc_yaw": MeasurementSpec("thumb_cmc_yaw", "relative_rotation", "top", "top_base", "thumb_yaw"),
"pinky_mcp_pitch": MeasurementSpec("pinky_mcp_pitch", "relative_rotation", "side", "side_base", "pinky_mcp"),
"pinky_pip": MeasurementSpec("pinky_pip", "relative_rotation", "side", "pinky_mcp", "pinky_pip", pose_axis_line_required=False),
"pinky_dip": MeasurementSpec("pinky_dip", "relative_rotation", "side", "pinky_pip", "pinky_dip", pose_axis_line_required=False),
"middle_mcp_roll": MeasurementSpec("middle_mcp_roll", "relative_rotation", "front", "front_base", "middle_roll"),
"middle_mcp_pitch": MeasurementSpec("middle_mcp_pitch", "relative_rotation", "side", "side_base", "middle_pip"),
"middle_pip": MeasurementSpec("middle_pip", "relative_rotation", "side", "side_base", "middle_pip"),
"middle_dip": MeasurementSpec("middle_dip", "relative_rotation", "side", "middle_pip", "middle_dip", pose_axis_line_required=False),
"index_mcp_roll": MeasurementSpec("index_mcp_roll", "relative_rotation", "front", "front_base", "index_roll"),
"index_mcp_pitch": MeasurementSpec("index_mcp_pitch", "relative_rotation", "side", "side_base", "index_pip"),
"index_pip": MeasurementSpec("index_pip", "relative_rotation", "side", "side_base", "index_pip"),
"index_dip": MeasurementSpec("index_dip", "relative_rotation", "side", "index_pip", "index_dip", pose_axis_line_required=False),
}
active = frozenset(ACTIVE_JOINTS)
passive = frozenset(PASSIVE_JOINTS)
coverage = {
**{
name: (
"transferred_static_dynamic"
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
else "measured_static_dynamic"
)
for name in active
},
**{
name: (
"measured_dynamic_cad_static"
if name in MEASURED_PASSIVE_JOINTS
else "mimic_nominal"
)
for name in passive
},
}
return CalibrationProfile(
key=KEY,
namespace="/o12_calibration",
command=CommandLayout(
names=COMMAND_NAMES,
baseline_u8=(),
baseline=(0.0,) * 12,
# The public command domain is the reviewed SDK/CAD intersection.
lower_bounds=SAFE_LOWER_RAD,
upper_bounds=SAFE_UPPER_RAD,
unit="rad",
feedback_by_index=True,
command_index_by_joint=COMMAND_INDEX_BY_JOINT,
urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS},
),
vision=VisionRigSpec(
views=(
ViewSpec("front", (
TagSpec("front_base", 0, True), TagSpec("thumb_cmc", 1),
TagSpec("thumb_mcp", 2), TagSpec("thumb_dip", 3),
TagSpec("middle_roll", 12), TagSpec("index_roll", 13),
)),
ViewSpec("side", (
TagSpec("side_base", 4, True), TagSpec("pinky_mcp", 5),
TagSpec("pinky_pip", 6), TagSpec("pinky_dip", 7),
TagSpec("middle_pip", 8), TagSpec("middle_dip", 9),
TagSpec("index_pip", 10), TagSpec("index_dip", 11),
)),
ViewSpec("top", (
TagSpec("top_base", 14, True), TagSpec("thumb_yaw", 15),
)),
),
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={
"command_rate_hz": 20.0,
"clearance_flex_rad_s": 0.10,
"clearance_splay_rad_s": 0.04,
"probe_travel_rad": math.radians(3.0),
"probe_speed_rad_s": 0.02,
"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(CALIBRATED_ACTIVE_JOINTS),
post_solve_endpoint_joints=frozenset(),
mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT,
cad_frozen_joints=passive,
endpoint_anchor_by_joint={name: "lower_at_start" for name in CALIBRATED_ACTIVE_JOINTS},
fitted_mimic_joints=MEASURED_PASSIVE_JOINTS,
coupling_model_by_joint={name: "quadratic_runtime" for name in MEASURED_PASSIVE_JOINTS},
),
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={"full": active},
frozen_joints={"full": frozenset()},
default_scope="full",
),
artifacts=ArtifactPolicy(
output_schema_version=7,
calibration_filename="o12_right_{serial_number}_calibration.json",
corrected_urdf_filename="linkerhand_o12_right_{serial_number}_zero_calibrated.urdf",
protected_input_fields=frozenset({
"source_urdf_sha256", "camera_extrinsics_sha256",
"calibration_config_sha256", "tag_config_sha256", "sdk_config_sha256",
}),
publication_pointer="latest_passed",
session_compatibility_tokens=frozenset({"o12_right_16_v1", "feedback_rad_v1"}),
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(
typed,
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_INDEX_BY_JOINT",
"COMMAND_NAMES", "INDEX_CLEARANCE_RAD", "KEY", "MEASURED_PASSIVE_JOINTS",
"MIMIC_SOURCE_BY_JOINT", "PARK_FINGER_RAD", "PARK_MIDDLE_MCP_RAD",
"PARK_MIDDLE_PIP_RAD", "PASSIVE_JOINTS", "SAFE_LOWER_RAD", "SAFE_UPPER_RAD",
"SDK_LOWER_RAD", "SDK_TO_URDF_JOINT", "SDK_UPPER_RAD",
"TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile",
]
@@ -0,0 +1,256 @@
"""One-command O12 right runner with vendor Jazzy overlay loading."""
from __future__ import annotations
import argparse
from datetime import datetime
import json
import os
from pathlib import Path
import subprocess
import time
from typing import Any
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger
from ...product import ProductConfig, load_product_config
from ..l6.runner import (
_ProgressConsole,
_l6_reason_zh,
_launch_command,
_stop_stack,
_wait_until,
render_six_channel_progress_zh,
)
from .pipeline import finalize_o12_session, load_o12_raw_samples
_TASK_LABELS = {
"thumb_pitch_front": "拇指 CMC pitch(正面 ID0→ID1",
"thumb_roll_front": "拇指 CMC roll(正面 ID0→ID1",
"thumb_mcp_dip_front": "拇指 MCP / 被动 DIP(正面 ID1→ID2→ID3",
"thumb_yaw_top": "拇指 CMC yaw(顶部 ID14→ID15",
"pinky_chain_side": "小指 MCP / 被动 PIP/DIP(侧面 ID4→ID5→ID6→ID7",
"middle_roll_front": "中指 MCP roll(正面 ID0→ID12",
"middle_mcp_side": "中指 MCP pitch(侧面 ID4→ID8",
"middle_pip_dip_side": "中指 PIP / 被动 DIP(侧面 ID8→ID9",
"index_roll_front": "食指 MCP roll(正面 ID0→ID13",
"index_mcp_side": "食指 MCP pitch(侧面 ID4→ID10",
"index_pip_dip_side": "食指 PIP / 被动 DIP(侧面 ID10→ID11",
}
def _o12_reason_zh(status):
reason = str(status.get("reason", ""))
if reason.startswith("mapping_preflight_no_target_motion:"):
task = reason.split(":", 1)[1]
return (
"O12-MAPPING-301",
f"固定 SDK 映射点动时没有观察到目标关节运动:{task}",
"检查对应 Tag、通道映射和机械连接;不要交换通道后强行继续。",
)
if reason.startswith("o12_error_report_nonzero"):
return (
"O12-HEALTH-201",
f"O12 返回非零错误码:{status.get('latest_error_codes', [])}",
"检查堵转、过流、过热或通信异常,排除后重新启动。",
)
return _l6_reason_zh(status, model_name="O12")
def render_o12_progress_zh(status, estimator=None) -> str:
"""Render O12 in the same operator-oriented layout as G20 and L6."""
text = render_six_channel_progress_zh(
status,
task_labels=_TASK_LABELS,
reason_renderer=_o12_reason_zh,
estimator=estimator,
)
temperature = (
"直接温度回读"
if status.get("temperature_report_verified")
else "错误码 bit1 过热保护"
if status.get("temperature_fallback_active")
else "等待温度能力确认"
)
return text + (
"\nO12安全:POSITION="
f"{bool(status.get('position_mode_verified'))}"
f"错误码通道={bool(status.get('error_report_verified'))}"
f"温度策略={temperature};速度倍率="
f"{float(status.get('motion_speed_scale', 1.0)):.1f}x"
)
class _Monitor(Node):
def __init__(self, progress: _ProgressConsole) -> None:
super().__init__("o12_calibration_runner")
self.status: dict[str, Any] = {}
self.progress = progress
self.create_subscription(String, "/o12_calibration/status", self._status, 10)
self.start_client = self.create_client(Trigger, "/o12_calibration/start")
self.abort_client = self.create_client(Trigger, "/o12_calibration/abort")
def _status(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 _overlay_environment(setup: Path) -> dict[str, str]:
completed = subprocess.run(
["bash", "-c", 'source "$1" >/dev/null 2>&1; env -0', "bash", str(setup)],
check=True,
stdout=subprocess.PIPE,
)
environment = dict(os.environ)
for item in completed.stdout.split(b"\0"):
if b"=" in item:
key, value = item.split(b"=", 1)
environment[key.decode()] = value.decode(errors="surrogateescape")
return environment
def _run_online(
config: ProductConfig, *, record_bag: bool, commands_enabled: bool
) -> int:
if config.sdk_setup is None or config.sdk_config is None:
raise ValueError("O12 vendor SDK overlay/config are required")
session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S")
while session.exists():
time.sleep(1.0)
session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S")
session.mkdir(parents=True)
log_path = session / "calibration.log"
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
print(f"O12 标定环境正在启动;日志:{log_path}", flush=True)
process = subprocess.Popen(
_launch_command(
config, session, record_bag=record_bag,
commands_enabled=commands_enabled,
),
cwd=config.workspace,
env=_overlay_environment(config.sdk_setup),
stdout=log_stream,
stderr=subprocess.STDOUT,
text=True,
start_new_session=True,
)
rclpy.init()
monitor = _Monitor(_ProgressConsole(renderer=render_o12_progress_zh))
try:
ready = _wait_until(
monitor, process,
lambda status: status.get("state") in {"READY", "PAUSED", "ABORTED"},
timeout=120.0,
)
if not ready or monitor.status.get("state") != "READY":
print(
"O12 启动预检失败:请检查 HCAN、POSITION 模式、错误/温度回读、"
f"12路反馈和三相机。日志:{log_path}", flush=True,
)
return 2
if not monitor.start_client.wait_for_service(timeout_sec=10.0):
print("O12 标定 /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"O12 标定未启动:{getattr(response, 'message', '')}", flush=True)
return 2
print("O12 标定已自动开始:POSITION,20 Hz,弧度余弦轨迹。", flush=True)
finished = _wait_until(
monitor, process,
lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"},
timeout=None,
)
if not finished or monitor.status.get("state") != "PASSED":
print(
"O12 标定已安全停止并保持当前位置:"
+ str(monitor.status.get("reason", "process_exit")),
flush=True,
)
return 3
print("\n".join((
"PASS:O12 右手 11 个实测任务和独立 holdout 已通过。",
f"发布结果:{config.session_root / 'latest_passed'}",
f"JSON{monitor.status.get('final_json')}",
f"URDF{monitor.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="O12 right 16-Tag 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=False,
)
if config.profile_key.profile_id != "O12/right/o12_right_16/v1":
raise ValueError("O12 runner received a different product profile")
if selected.validate_only:
print(f"配置有效:{config.profile_key.profile_id}SDK {config.sdk_config_sha256}")
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_o12_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,
"sdk_config_sha256": config.sdk_config_sha256,
},
records=load_o12_raw_samples(selected.offline_raw),
publish=selected.publish_offline,
)
print(f"离线回放PASSschema {payload['schema_version']}URDF {correction.path}")
return
raise SystemExit(_run_online(
config,
record_bag=selected.record_bag,
commands_enabled=not selected.commands_disabled,
))
__all__ = ["main", "render_o12_progress_zh"]
@@ -0,0 +1,140 @@
"""Auditable O12 URDF correction from radian-domain calibration results."""
from __future__ import annotations
from dataclasses import dataclass
from datetime import datetime
import math
from pathlib import Path
import re
from typing import Mapping
import xml.etree.ElementTree as ET
from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches
from .fitting import O12FitResult
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
MEASURED_PASSIVE_JOINTS,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
)
SPLAY_JOINTS = frozenset(
{"index_mcp_roll", "middle_mcp_roll"}
)
@dataclass(frozen=True)
class O12UrdfCorrection:
path: Path
corrected_limits_rad: Mapping[str, tuple[float, float]]
mimic_multipliers: Mapping[str, float]
preserved_ring_fields: tuple[str, ...]
def write_o12_corrected_urdf(
*,
source_urdf: str | Path,
output_directory: str | Path,
serial_number: str,
result: O12FitResult,
timestamp: str | None = None,
) -> O12UrdfCorrection:
"""Patch only measured travel and dynamic mimic terms.
Tag mounting angle cannot be separated from a static passive-joint zero,
so origins and passive limits stay byte-for-byte CAD. The unobserved ring
keeps its own origin, limits and both CAD mimic ratios.
"""
source = Path(source_urdf).expanduser().resolve()
if not source.is_file():
raise ValueError(f"source URDF does not exist: {source}")
if "calibrated" in source.stem.lower():
raise ValueError("O12 source URDF must be immutable original CAD")
if set(result.travels_rad) != (
CALIBRATED_ACTIVE_JOINTS | set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT)
):
raise ValueError("O12 travel result has the wrong active joint set")
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
raise ValueError("O12 mimic result has the wrong passive joint set")
root = ET.parse(source).getroot()
joints = {
str(node.get("name")): node
for node in root.findall("joint")
if node.get("type") == "revolute"
}
required = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS | {
"ring_mcp_pitch", "ring_pip", "ring_dip"
}
if missing := required - set(joints):
raise ValueError("source URDF is missing O12 joints: " + ",".join(sorted(missing)))
corrected_limits: dict[str, tuple[float, float]] = {}
patches: dict[str, UrdfJointPatch] = {}
for name in sorted(CALIBRATED_ACTIVE_JOINTS):
limit = joints[name].find("limit")
if limit is None:
raise ValueError(f"O12 source active joint has no limit: {name}")
cad_lower = float(limit.get("lower", "nan"))
cad_upper = float(limit.get("upper", "nan"))
travel = abs(float(result.travels_rad[name]))
if not math.isfinite(travel) or travel <= math.radians(2.0):
raise ValueError(f"invalid measured O12 travel: {name}")
if name in SPLAY_JOINTS:
half = min(0.5 * travel, abs(cad_lower), abs(cad_upper))
lower, upper = -half, half
else:
lower, upper = max(0.0, cad_lower), min(cad_upper, travel)
if not lower < upper:
raise ValueError(f"invalid corrected O12 limits: {name}")
corrected_limits[name] = (lower, upper)
patches[name] = UrdfJointPatch(
limit_lower=f"{lower:.15g}", limit_upper=f"{upper:.15g}"
)
mimic_multipliers: dict[str, float] = {}
for name in sorted(MEASURED_PASSIVE_JOINTS):
mimic = joints[name].find("mimic")
if mimic is None:
raise ValueError(f"O12 passive joint has no mimic element: {name}")
multiplier = float(result.mimic_fits[name].urdf_mimic_multiplier)
if not math.isfinite(multiplier) or not 0.5 <= multiplier <= 2.2:
raise ValueError(f"invalid O12 mimic multiplier: {name}")
mimic_multipliers[name] = multiplier
patches[name] = UrdfJointPatch(
mimic_multiplier=f"{multiplier:.15g}"
)
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("O12 URDF 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 = Path(output_directory).expanduser().resolve()
output.mkdir(parents=True, exist_ok=True)
destination = output / f"{source.stem}_calibrated_{safe_serial}_{stamp}.urdf"
write_urdf_patches(
source_urdf=source,
destination_urdf=destination,
patches=UrdfPatchSet(joints=patches),
forbidden_source_stem_patterns=(r"calibrated",),
copy_complete_mesh_directory=True,
)
return O12UrdfCorrection(
path=destination,
corrected_limits_rad=corrected_limits,
mimic_multipliers=mimic_multipliers,
preserved_ring_fields=(
"ring_mcp_pitch.origin", "ring_mcp_pitch.limit",
"ring_pip.origin", "ring_pip.limit", "ring_pip.mimic",
"ring_dip.origin", "ring_dip.limit", "ring_dip.mimic",
),
)
__all__ = ["O12UrdfCorrection", "write_o12_corrected_urdf"]
@@ -79,10 +79,12 @@ def get_default_registry() -> ProfileRegistry:
from .g20 import register_profiles from .g20 import register_profiles
from .l6 import register_profiles as register_l6_profiles from .l6 import register_profiles as register_l6_profiles
from .o6 import register_profiles as register_o6_profiles from .o6 import register_profiles as register_o6_profiles
from .o12 import register_profiles as register_o12_profiles
registry = ProfileRegistry() registry = ProfileRegistry()
register_profiles(registry) register_profiles(registry)
register_l6_profiles(registry) register_l6_profiles(registry)
register_o6_profiles(registry) register_o6_profiles(registry)
register_o12_profiles(registry)
_DEFAULT_REGISTRY = registry _DEFAULT_REGISTRY = registry
return _DEFAULT_REGISTRY return _DEFAULT_REGISTRY
@@ -213,6 +213,11 @@ class ProductConfig:
cameras: Mapping[str, Mapping[str, str]] cameras: Mapping[str, Mapping[str, str]]
required_independent_passes: int required_independent_passes: int
static_repeatability_rad: float static_repeatability_rad: float
sdk_driver: str = "linker_hand_sdk"
sdk_transport: str = "socketcan"
sdk_setup: Path | None = None
sdk_config: Path | None = None
sdk_config_sha256: str = ""
@property @property
def session_root(self) -> Path: def session_root(self) -> Path:
@@ -247,11 +252,39 @@ def load_product_config(
).strip() ).strip()
if not namespace.startswith("/") or namespace.endswith("/"): if not namespace.startswith("/") or namespace.endswith("/"):
raise ValueError("namespace must be an absolute ROS namespace") raise ValueError("namespace must be an absolute ROS namespace")
sdk_raw = _mapping(raw.get("sdk", {}), "sdk")
sdk_driver = str(sdk_raw.get("driver", "linker_hand_sdk")).strip()
sdk_transport = str(sdk_raw.get("transport", "socketcan")).strip().lower()
can_interface = str(raw.get("can_interface", "")).strip() can_interface = str(raw.get("can_interface", "")).strip()
if not can_interface: if sdk_transport == "socketcan":
raise ValueError("can_interface is required") if not can_interface:
if check_can and not (Path("/sys/class/net") / can_interface).exists(): raise ValueError("can_interface is required for SocketCAN products")
raise ValueError(f"CAN interface does not exist: {can_interface}") if check_can and not (Path("/sys/class/net") / can_interface).exists():
raise ValueError(f"CAN interface does not exist: {can_interface}")
elif sdk_transport != "hcan":
raise ValueError("sdk.transport must be socketcan or hcan")
sdk_setup: Path | None = None
sdk_config: Path | None = None
sdk_config_hash = ""
if sdk_transport == "hcan":
sdk_setup = _resolve_path(
sdk_raw.get("setup"), workspace=root, name="sdk.setup"
)
sdk_config = _resolve_path(
sdk_raw.get("config"), workspace=root, name="sdk.config"
)
if not sdk_setup.is_file() or not sdk_config.is_file():
raise ValueError("vendor SDK overlay and config must exist")
sdk_config_hash = str(sdk_raw.get("config_sha256", "")).lower()
if re.fullmatch(r"[0-9a-f]{64}", sdk_config_hash) is None:
raise ValueError("SDK config expected SHA-256 is invalid")
actual_sdk_hash = sha256_file(sdk_config)
if actual_sdk_hash != sdk_config_hash:
raise ValueError(
"SDK config SHA-256 mismatch: "
f"expected={sdk_config_hash} actual={actual_sdk_hash}"
)
artifacts = _mapping(raw.get("artifacts"), "artifacts") artifacts = _mapping(raw.get("artifacts"), "artifacts")
source_urdf = _resolve_path( source_urdf = _resolve_path(
@@ -389,4 +422,9 @@ def load_product_config(
cameras=cameras, cameras=cameras,
required_independent_passes=passes, required_independent_passes=passes,
static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0, static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0,
sdk_driver=sdk_driver,
sdk_transport=sdk_transport,
sdk_setup=sdk_setup,
sdk_config=sdk_config,
sdk_config_sha256=sdk_config_hash,
) )
@@ -145,7 +145,7 @@ def test_runtime_has_no_concrete_model_or_view_assumption() -> None:
def test_every_registered_profile_passes_static_integrity_checks() -> None: def test_every_registered_profile_passes_static_integrity_checks() -> None:
registry = get_default_registry() registry = get_default_registry()
assert len(registry) == 7 assert len(registry) == 8
for registered in registry: for registered in registry:
validate_profile(registered.profile) validate_profile(registered.profile)
assert registered.profile.zero.active_joints assert registered.profile.zero.active_joints
@@ -0,0 +1,284 @@
from __future__ import annotations
import math
from pathlib import Path
from types import SimpleNamespace
import xml.etree.ElementTree as ET
import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from linkerhand_calibration.calibrated_joint_state_bridge import CalibratedCommandMapper
from linkerhand_calibration.models.o12.artifacts import build_o12_runtime_payload
from linkerhand_calibration.models.o12.fitting import fit_o12_session
from linkerhand_calibration.models.o12.motion import cosine_position_trajectory_rad
from linkerhand_calibration.models.o12.node import O12ThreeCameraCalibrationNode
from linkerhand_calibration.models.o12.runner import render_o12_progress_zh
from linkerhand_calibration.models.l6.runner import _launch_command
from linkerhand_calibration.models.o12.profile import (
CALIBRATED_ACTIVE_JOINTS,
COMMAND_NAMES,
MEASURED_PASSIVE_JOINTS,
PARK_FINGER_RAD,
SDK_TO_URDF_JOINT,
build_typed_profile,
)
from linkerhand_calibration.models.o12.urdf import write_o12_corrected_urdf
ROOT = Path(__file__).resolve().parents[1]
SOURCE_URDF = ROOT / "urdf/o12_right/linkerhand_o12_t3_right-0703.urdf"
def _synthetic_records():
profile = build_typed_profile()
task_by_joint = {
joint: task for task in profile.motion.tasks for joint in task.joints
}
ratios = {
"thumb_dip": 1.01,
"index_dip": 0.88,
"middle_dip": 0.77,
"pinky_pip": 1.11,
"pinky_dip": 0.95,
}
source = {
"thumb_dip": "thumb_mcp",
"index_dip": "index_pip",
"middle_dip": "middle_pip",
"pinky_pip": "pinky_mcp_pitch",
"pinky_dip": "pinky_pip",
}
travel: dict[str, float] = {}
for name in CALIBRATED_ACTIVE_JOINTS:
task = task_by_joint[name]
travel[name] = abs(task.end_value - task.start_value)
travel["pinky_pip"] = ratios["pinky_pip"] * travel["pinky_mcp_pitch"]
travel["pinky_dip"] = ratios["pinky_dip"] * travel["pinky_pip"]
for name in ("thumb_dip", "index_dip", "middle_dip"):
travel[name] = ratios[name] * travel[source[name]]
result = {name: [] for name in CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS}
for name, rows in result.items():
task = task_by_joint[name]
for cycle in range(4):
for direction, phases in (
("decreasing", np.linspace(0.0, 1.0, 65)),
("increasing", np.linspace(1.0, 0.0, 65)),
):
for phase in phases:
feedback = task.start_value + phase * (task.end_value - task.start_value)
angle = phase * travel[name]
rows.append({
"cycle": cycle,
"direction": direction,
"feedback_rad": float(feedback),
"relative_quaternion_xyzw": Rotation.from_rotvec(
[0.0, angle, 0.0]
).as_quat().tolist(),
})
return result
def test_o12_fixed_mapping_tags_and_radian_domain() -> None:
profile = build_typed_profile()
assert profile.key.profile_id == "O12/right/o12_right_16/v1"
assert profile.command.unit == "rad"
assert profile.command.feedback_by_index is True
assert profile.command.names == COMMAND_NAMES
assert SDK_TO_URDF_JOINT[:4] == (
"thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp"
)
assert profile.vision.tag_ids == frozenset(range(16))
assert len(profile.motion.tasks) == 11
assert [task.formal_speed for task in profile.motion.tasks] == [
0.03, 0.04, 0.08, 0.04, 0.08, 0.04, 0.08, 0.08, 0.04, 0.08, 0.08
]
def test_o12_cosine_speed_is_bounded() -> None:
speed = 0.03
_, _, duration = cosine_position_trajectory_rad(0.0, -0.8, 0.0, speed)
samples = np.asarray([
cosine_position_trajectory_rad(0.0, -0.8, t, speed)[0]
for t in np.linspace(0.0, duration, 2001)
])
measured = np.max(np.abs(np.diff(samples))) / (duration / 2000.0)
assert measured <= speed + 1.0e-6
def test_o12_hcan_launch_does_not_emit_empty_socketcan_argument(tmp_path: Path) -> None:
profile = build_typed_profile()
config = SimpleNamespace(
model="O12", side="right", tag_layout="o12_right_16",
serial_number="O12_TEST", can_interface="", source_urdf=SOURCE_URDF,
source_urdf_sha256="a" * 64,
camera_extrinsics=tmp_path / "extrinsics.yaml",
camera_extrinsics_sha256="b" * 64,
calibration_config=tmp_path / "calibration.yaml",
calibration_config_sha256="c" * 64,
tag_config=tmp_path / "tags.yaml", tag_config_sha256="d" * 64,
sdk_config=tmp_path / "sdk.yaml", sdk_config_sha256="e" * 64,
output_root=tmp_path, cameras={
view: {
"serial_number": f"camera-{view}",
"camera_name": f"camera_{view}",
"camera_info": str(tmp_path / f"{view}.yaml"),
}
for view in ("front", "side", "top")
},
calibration_contract=SimpleNamespace(typed_profile=profile),
)
command = _launch_command(
config, tmp_path / "session", record_bag=False, commands_enabled=False
)
assert not any(item.startswith("can_interface:=") for item in command)
def test_o12_vendor_node_is_launched_in_documented_namespace() -> None:
launch_text = (ROOT / "launch/three_camera_calibration.launch.py").read_text()
vendor = launch_text.split('package="omnihand_node"', 1)[1].split(")", 1)[0]
assert 'executable="omnihand_pro_2025_node"' in vendor
assert 'namespace="o12"' in vendor
def test_o12_temperature_fallback_requires_verified_error_channel(tmp_path: Path) -> None:
warnings: list[str] = []
fake = SimpleNamespace(
temperature_report_required=False,
temperature_verified=False,
temperature_fallback_active=False,
error_verified=False,
health_check_started_at=10.0,
temperature_fallback_after_seconds=5.0,
raw_path=tmp_path / "raw.jsonl",
protected_inputs={"sdk_config_sha256": "a" * 64},
get_logger=lambda: SimpleNamespace(warning=warnings.append),
)
O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed(
fake, 16.0
)
assert not fake.temperature_fallback_active
fake.error_verified = True
O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed(
fake, 16.0
)
assert fake.temperature_fallback_active
assert warnings == [
"O12 temperature report unavailable; continuing with error-code "
"bit1 overheat protection"
]
raw = (tmp_path / "raw.jsonl").read_text()
assert '"fallback_protection":"joint_error_states_bit1_overheat"' in raw
def test_o12_required_temperature_report_disables_fallback(tmp_path: Path) -> None:
fake = SimpleNamespace(
temperature_report_required=True,
temperature_verified=False,
temperature_fallback_active=False,
error_verified=True,
health_check_started_at=10.0,
temperature_fallback_after_seconds=5.0,
)
O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed(
fake, 30.0
)
assert not fake.temperature_fallback_active
def test_o12_motion_plan_has_collision_clearance_and_safe_return() -> None:
profile = build_typed_profile()
fake = SimpleNamespace(
profile=profile,
motion_speed_scale=2.0,
_full_target=O12ThreeCameraCalibrationNode._full_target,
)
steps = O12ThreeCameraCalibrationNode._build_steps(fake)
clearance = [step for step in steps if step.phase.startswith("clearance")]
assert clearance[0].target_command[10:] == (PARK_FINGER_RAD, PARK_FINGER_RAD)
assert clearance[0].speed_u8 == 0.15
assert clearance[1].target_command[4] == pytest.approx(-math.radians(10.0))
assert clearance[1].speed_u8 == 0.08
assert [step.phase for step in steps[-3:]] == [
"return_splay_zero", "return_middle_open", "return_outer_open"
]
for task in profile.motion.tasks:
probes = [step for step in steps if step.task_key == task.key and step.phase == "preflight"]
assert max(abs(step.target_u8 - task.start_value) for step in probes) <= math.radians(3.0) + 1e-12
sweeps = [step for step in steps if step.task_key == task.key and step.phase == "sweep"]
assert {step.speed_u8 for step in sweeps} == {2.0 * task.formal_speed}
def test_o12_operator_progress_uses_unified_layout_and_radians() -> None:
text = render_o12_progress_zh({
"state": "RUNNING",
"serial_number": "O12_TEST",
"step_index": 20,
"step_count": 190,
"step_fraction": 0.5,
"task_name": "thumb_pitch_front",
"phase": "sweep",
"cycle": 1,
"direction": "decreasing",
"attempt": 1,
"command_unit": "rad",
"target_rad": -0.827,
"current_command_u8": -0.400,
"actual_u8": -0.395,
"speed_rad_s": 0.060,
"command_trajectory_duration_seconds": 21.7,
"valid_frames": 80,
"total_frames": 82,
"tag_detection_rate": 80 / 82,
"joint_frame_rate": 80 / 82,
"recognized_tag_ids": [0, 1],
"unrecognized_tag_ids": [],
"feedback_hz": 20.0,
"position_mode_verified": True,
"error_report_verified": True,
"temperature_fallback_active": True,
"motion_speed_scale": 2.0,
})
assert "[O12_TEST] 标定中" in text
assert "拇指 CMC pitch(正面 ID0→ID1" in text
assert "阶段:正式扫描(第 2/4 轮)" in text
assert "命令/反馈:-0.400 rad/-0.395 rad" in text
assert "峰值 0.060 rad/s;本段 21.7 秒余弦轨迹" in text
assert "温度策略=错误码 bit1 过热保护;速度倍率=2.0x" in text
def test_o12_synthetic_fit_schema7_bridge_and_ring_preservation(tmp_path: Path) -> None:
result = fit_o12_session(SOURCE_URDF, _synthetic_records())
hashes = {name: "a" * 64 for name in build_typed_profile().artifacts.protected_input_fields}
payload = build_o12_runtime_payload(
serial_number="O12_TEST",
source_urdf=SOURCE_URDF,
result=result,
protected_inputs=hashes,
passed=True,
)
assert payload["schema_version"] == 7
assert len(payload["joints"]["thumb_cmc_yaw"]["angle_rad"]) == 65
assert payload["joints"]["thumb_cmc_roll"]["raw_increasing_curve_branch"] == "decreasing"
assert payload["joints"]["thumb_cmc_yaw"]["raw_increasing_curve_branch"] == "increasing"
assert "command_range" not in payload
assert payload["joints"]["ring_mcp_pitch"]["transferred_from_joint"] == "pinky_mcp_pitch"
mapper = CalibratedCommandMapper(payload)
assert len(mapper.map_positions([0.0] * 12, ["wrong"] * 12)) == 19
correction = write_o12_corrected_urdf(
source_urdf=SOURCE_URDF,
output_directory=tmp_path,
serial_number="O12_TEST",
result=result,
timestamp="20260904_120000",
)
before = ET.parse(SOURCE_URDF).getroot()
after = ET.parse(correction.path).getroot()
by_name_before = {node.get("name"): node for node in before.findall("joint")}
by_name_after = {node.get("name"): node for node in after.findall("joint")}
for name in ("ring_mcp_pitch", "ring_pip", "ring_dip"):
assert ET.tostring(by_name_before[name]) == ET.tostring(by_name_after[name])