O6gui预设动作修改、O12标定初始化提交
This commit is contained in:
@@ -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"],
|
||||||
|
|||||||
@@ -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}
|
||||||
|
|||||||
@@ -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,
|
||||||
|
]
|
||||||
@@ -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"离线回放PASS:schema {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])
|
||||||
Reference in New Issue
Block a user