L6右手标定

This commit is contained in:
lxp
2026-09-02 13:26:32 +08:00
parent 2b7c1f92e7
commit f7aeef87a8
31 changed files with 5584 additions and 68 deletions
+2
View File
@@ -69,6 +69,8 @@ Thumbs.db
*_mapping_quality.json
# Device-specific robot descriptions derived from local calibration runs
# Includes both full and partial timestamped zero-calibration outputs.
/src/linkerhand_calibration/urdf/*/*_zero_calibrated_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
@@ -230,7 +230,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
}
),
"L6": HandConfig(
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
init_pos=[250] * 6,
preset_actions={
@@ -33,6 +33,10 @@ _CANONICAL_COMMAND_NAMES = {
"thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
],
"L6": [
"thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch",
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
],
}
_CANONICAL_COMMAND_BOUNDS = {
@@ -43,6 +47,7 @@ _CANONICAL_COMMAND_BOUNDS = {
*[(0, 255)] * 5,
],
"O6": [(0, 255)] * 6,
"L6": [(0, 255)] * 6,
}
+12
View File
@@ -0,0 +1,12 @@
from gui_control.config.constants import HAND_CONFIGS
def test_l6_gui_uses_the_sdk_channel_order() -> None:
assert HAND_CONFIGS["L6"].joint_names_en == [
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
]
@@ -1,3 +1,5 @@
from collections import deque
import can
import time, sys
import threading
@@ -57,6 +59,14 @@ class LinkerHandL6Can:
self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
self.is_lock = False
self.version = None
# L6 replies to a six-byte 0x01 position command with an immediate
# byte-for-byte echo on the same CAN ID. A zero-payload 0x01 state
# query also replies on that ID, but with the measured positions.
# Keep the two transactions distinct so command echoes never enter
# the published feedback stream used by calibration.
self._position_echo_lock = threading.Lock()
self._pending_position_echoes = deque(maxlen=32)
self._position_echo_timeout_seconds = 0.02
# Start the receiving thread
self.running = True
self.receive_thread = threading.Thread(target=self.receive_response)
@@ -111,6 +121,11 @@ class LinkerHandL6Can:
frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
data = [frame_property_value] + [int(val) for val in data_list]
msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
if frame_property_value == 0x01 and len(data_list) == 6:
with self._position_echo_lock:
self._pending_position_echoes.append(
(time.monotonic(), tuple(int(value) for value in data_list))
)
try:
self.bus.send(msg)
except can.CanError as e:
@@ -201,7 +216,23 @@ class LinkerHandL6Can:
except:
return
if frame_type == 0x01: # 0x01
self.x01 = list(response_data)
response = tuple(int(value) for value in response_data)
now = time.monotonic()
is_position_echo = False
with self._position_echo_lock:
while (
self._pending_position_echoes
and now - self._pending_position_echoes[0][0]
> self._position_echo_timeout_seconds
):
self._pending_position_echoes.popleft()
for pending in tuple(self._pending_position_echoes):
if pending[1] == response:
self._pending_position_echoes.remove(pending)
is_position_echo = True
break
if not is_position_echo:
self.x01 = list(response)
elif frame_type == 0x02: # 0x02
self.x02 = list(response_data)
elif frame_type == 0x05: # Set speed
@@ -391,7 +422,10 @@ class LinkerHandL6Can:
return self.x35
def get_finger_order(self):
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
# L6 channel 1 is the physical CMC roll actuator. Older SDK releases
# exposed the channel as ``thumb_cmc_yaw`` even though the wire order
# and mechanism have always been roll.
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
def show_fun_table(self):
pass
@@ -372,7 +372,7 @@ class LinkerHandL6RS485:
return [0] * 6
def get_finger_order(self):
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
# --------------------------------------------------
# 便捷方法
@@ -0,0 +1,72 @@
import ast
from collections import deque
from pathlib import Path
import sys
import threading
import time
from types import SimpleNamespace
PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core"
EXPECTED = [
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
]
def _finger_order(path: Path, class_name: str) -> list[str]:
module = ast.parse(path.read_text(encoding="utf-8"))
selected = next(
item
for item in module.body
if isinstance(item, ast.ClassDef) and item.name == class_name
)
method = next(
item
for item in selected.body
if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order"
)
returned = next(item for item in method.body if isinstance(item, ast.Return))
return ast.literal_eval(returned.value)
def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None:
assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED
assert _finger_order(
PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485"
) == EXPECTED
def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
linker_hand_root = PACKAGE.parent
sys.path.insert(0, str(linker_hand_root))
try:
from core.can.linker_hand_l6_can import LinkerHandL6Can
finally:
sys.path.remove(str(linker_hand_root))
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
hand.can_id = 0x27
hand.x01 = [10, 20, 30, 40, 50, 60]
hand._position_echo_lock = threading.Lock()
command = (255, 2, 253, 253, 253, 253)
hand._pending_position_echoes = deque(
[(time.monotonic(), command)], maxlen=32
)
hand._position_echo_timeout_seconds = 0.02
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)))
)
assert hand.x01 == [10, 20, 30, 40, 50, 60]
assert not hand._pending_position_echoes
measured = (250, 3, 252, 252, 252, 252)
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)))
)
assert hand.x01 == list(measured)
+74
View File
@@ -1,5 +1,79 @@
# LinkerHand 专业标定包
## L6 右手局部标定(l6_right_8/v1
本版只发布 `rh_thumb_cmc_pitch``rh_thumb_cmc_roll`
`rh_pinky_mcp_pitch` 的静态零位与动态曲线,以及 `rh_thumb_dip`
`rh_pinky_dip` 的视觉动态曲线。拇指 DIP 通过线性 mimic;小指 DIP 使用实测
双方向运行曲线和 MuJoCo 二次 equality,因为它的传动比会随屈曲角变化。生成的
修正 URDF 保留 `rh_pinky_dip``<mimic>`,因此在普通 URDF/RViz 中仍会跟随
MCP 运动;该线性回退精确对齐实测零位和闭合端点。中间行程的准确非线性轨迹由
MuJoCo equality 或下述标定桥提供。根据 L6 四指同机构的实机确认,食指、中指、
无名指的 MCP 零偏、行程、双向反馈曲线及 DIP 耦合从小指迁移;每指自己的
`origin.xyz`、axis、mesh 和惯量保持 CAD,不把迁移结果标成 Tag 实测。四指 MCP
以反馈 255 的展开端作为 CAD lower/zero 锚点;实测行程不会再被强制压回较短的
CAD upper,因此不会向四指 origin 写入系统性的负零偏。
结果指针为 `latest_partial_passed`,不会被当作六路主动关节的完整标定。
拇指 pitch/roll 根据两组已记录六路反馈值的实机/仿真姿态对比,以反馈 255
保持源 CAD joint zero,不叠加端点推断的静态偏置;小指及三指迁移仍以反馈 0
机械闭合姿态对齐源 CAD upper。该策略只改变坐标锚点,不改变视觉实测的双方向
行程曲线。现场姿态对比必须同时记录对应的六路反馈值。
先启动 SDK 和 GUI 做手动检查时使用:
```bash
ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
-p hand_type:=right -p hand_joint:=L6 -p can:=can0 -p topic_prefix:=/l6
ros2 run gui_control gui_control
```
正式一键标定由 runner 自己启动 SDK、三相机、AprilTag 和标定节点,不要同时
运行上面的 SDK/GUI 控制命令:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml
```
只检查 Profile、8 张 16 mm Tag、相机/外参哈希和只读源 URDF:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml --validate-only
```
离线回放与在线发布使用同一拟合/URDF写回路径:
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml \
--offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl
```
运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、
roll、index、middle、ring、pinky;旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路
兼容别名读取,新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。
预检和正式扫描均使用 L6 硬件速度 `1` 作为上限,并由 100 Hz 余弦缓入缓出
轨迹把完整 `255↔0` 行程固定为 `6 s`;短行程按距离同比缩短。SDK 会过滤 L6
在同一 CAN ID 回送的位置命令回显,标定只使用状态查询返回的真实反馈。标定
启动时还会检查重复 SDK/GUI 发布者,避免两个进程同时访问同一只手。
标定完成后,用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节
`JointState`
```bash
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
hand_type:=right \
calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json
```
schema v6 默认订阅 `/l6/cb_right_hand_state`,发布
`/sim/mujoco/l6/right/joint_state`。标准 URDF 的 `<mimic>` 本身只支持线性关系,
所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似;需要实测轨迹时使用该桥
或修正 URDF 内的 MuJoCo equality。
## G20右手正式一键标定
固定三相机和19张Tag安装完成后,用户只运行:
@@ -0,0 +1,59 @@
/l6_calibration/front/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [0, 1, 2]
frames: [front_base, thumb_pitch, thumb_dip]
sizes: [0.016, 0.016, 0.016]
/l6_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [3, 4, 5]
frames: [side_base, pinky_pitch, pinky_dip]
sizes: [0.016, 0.016, 0.016]
/l6_calibration/top/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [6, 7]
frames: [top_base, thumb_roll]
sizes: [0.016, 0.016]
@@ -0,0 +1,37 @@
schema_version: 2
profile_id: L6/right/l6_right_8/v1
model: L6
side: right
tag_layout: l6_right_8
namespace: /l6_calibration
serial_number: L6_RIGHT_001
can_interface: can0
output_root: calibration_output
cameras:
front:
serial_number: DB2163742
camera_name: hikrobot_front_DB2163742
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
side:
serial_number: DB2163749
camera_name: hikrobot_side_DB2163749
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
top:
serial_number: DB2163739
camera_name: hikrobot_top_DB2163739
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
artifacts:
source_urdf: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf
source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f
tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
release:
required_independent_passes: 1
static_repeatability_deg: 1.0
@@ -0,0 +1,67 @@
l6_calibration:
ros__parameters:
command_topic: /l6/cb_right_hand_control_cmd
state_topic: /l6/cb_right_hand_state
setting_topic: /l6/cb_hand_setting_cmd
front_camera_info_topic: /l6_calibration/front/camera/camera_info
front_detections_topic: /l6_calibration/front/apriltag/detections
side_camera_info_topic: /l6_calibration/side/camera/camera_info
side_detections_topic: /l6_calibration/side/apriltag/detections
top_camera_info_topic: /l6_calibration/top/camera/camera_info
top_detections_topic: /l6_calibration/top/apriltag/detections
baseline_command_u8: [255, 255, 255, 255, 255, 255]
# L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10,
# which left fewer than 32 useful feedback bins. Speed 1 is still only a
# firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s.
# A 100 Hz cosine trajectory therefore sets the actual, model-level pace.
preflight_speed_u8: 1
formal_speed_u8: 1
speed_settle_seconds: 0.2
command_trajectory_full_range_seconds: 6.0
torque_u8: 80
repetitions: 4
preflight_checkpoints_u8: [255, 127, 0]
tag_size_m: 0.016
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
minimum_detection_rate: 0.95
# Per-Tag quality remains >=95%. With three independently detected Tags,
# the fully joined frame rate may be 0.95^3 ~= 85.7%.
minimum_joint_frame_rate: 0.85
minimum_feedback_hz: 25.0
maximum_state_image_skew_ms: 50.0
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
pnp_maximum_reprojection_error_px: 1.5
pnp_reprojection_tie_px: 1.5
pnp_maximum_pose_jump_deg: 35.0
pnp_maximum_translation_jump_m: 0.04
pnp_maximum_tag_tilt_deg: 75.0
pnp_tracker_reset_seconds: 5.0
minimum_sweep_frames: 40
minimum_state_span_u8: 240.0
minimum_sweep_bins: 32
maximum_bin_gap: 16
maximum_monotonic_correction_deg: 2.0
passive_maximum_monotonic_correction_deg: 3.0
maximum_validation_mae_deg: 1.0
maximum_validation_p95_deg: 2.0
maximum_validation_error_deg: 3.0
mimic_minimum_multiplier: 0.5
mimic_maximum_multiplier: 1.5
mimic_maximum_cycle_range: 0.03
mimic_maximum_residual_p95_deg: 2.0
endpoint_tolerance_u8: 2.0
endpoint_hold_seconds: 1.0
motor_stall_timeout_seconds: 2.0
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
automatic_sweep_retry_limit: 2
non_target_motion_tolerance_u8: 3.0
fixed_base_maximum_corner_drift_px: 2.0
fixed_base_movement_confirmation_frames: 5
@@ -1,4 +1,4 @@
"""Publish calibrated G20 URDF angles from raw command/feedback u8 values."""
"""Publish profile-calibrated URDF angles from raw command/feedback u8 values."""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
@@ -17,7 +17,7 @@ def generate_launch_description() -> LaunchDescription:
package="linkerhand_calibration",
executable="calibrated_joint_state_bridge",
name=[
"g20_calibrated_joint_state_bridge_",
"calibrated_joint_state_bridge_",
LaunchConfiguration("hand_type"),
],
output="screen",
@@ -1,4 +1,4 @@
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
"""Launch three Hikrobot views and one registered hand calibration owner."""
from __future__ import annotations
@@ -26,11 +26,13 @@ from launch_ros.parameter_descriptions import ParameterValue
VIEWS = ("front", "side", "top")
def _default_source_urdf(hand_type: str) -> Path:
def _default_source_urdf(model: str, hand_type: str) -> Path:
relative = (
Path("urdf")
/ f"g20_{hand_type}"
/ f"linkerhand_g20_{hand_type}.urdf"
Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf"
if model.upper() == "L6" and hand_type == "right"
else Path("urdf")
/ f"{model.lower()}_{hand_type}"
/ f"linkerhand_{model.lower()}_{hand_type}.urdf"
)
package_source_or_share = Path(__file__).resolve().parents[1] / relative
try:
@@ -71,6 +73,8 @@ def _launch_stack(context):
/ (
"three_camera_tags_g20_right_19.yaml"
if tag_layout == "g20_right_19"
else "l6_right_8_tags.yaml"
if tag_layout == "l6_right_8"
else "three_camera_tags_g20_right_15.yaml"
if tag_layout == "g20_right_15"
else "three_camera_tags.yaml"
@@ -86,7 +90,7 @@ def _launch_stack(context):
source_urdf = (
Path(requested_source).expanduser().resolve()
if requested_source
else _default_source_urdf(hand_type).resolve()
else _default_source_urdf(model, hand_type).resolve()
)
if not source_urdf.is_file():
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
@@ -96,7 +100,7 @@ def _launch_stack(context):
if contract.typed_profile.artifacts.publish_corrected_urdf:
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
raise RuntimeError(
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
"this profile requires source_urdf_expected_sha256 confirmed "
"by the CAD/hardware owner"
)
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
@@ -142,12 +146,13 @@ def _launch_stack(context):
raw_topics = []
info_topics = []
detection_topics = []
calibration_namespace = contract.typed_profile.namespace
for view in VIEWS:
namespace = f"/g20_calibration/{view}/camera"
namespace = f"{calibration_namespace}/{view}/camera"
raw_topic = f"{namespace}/image_raw"
info_topic = f"{namespace}/camera_info"
rect_topic = f"{namespace}/image_rect"
detector_namespace = f"/g20_calibration/{view}/apriltag"
detector_namespace = f"{calibration_namespace}/{view}/apriltag"
detection_topic = f"{detector_namespace}/detections"
raw_topics.append(raw_topic)
info_topics.append(info_topic)
@@ -170,7 +175,9 @@ def _launch_stack(context):
"camera_name": LaunchConfiguration(
f"{view}_camera_name"
),
"frame_id": f"g20_calibration_{view}_optical_frame",
"frame_id": (
f"{model.lower()}_calibration_{view}_optical_frame"
),
"image_width": 1624,
"image_height": 1240,
"frame_rate": ParameterValue(
@@ -233,7 +240,7 @@ def _launch_stack(context):
)
vision = ComposableNodeContainer(
name="g20_three_camera_vision",
name=f"{model.lower()}_three_camera_vision",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
@@ -265,10 +272,10 @@ def _launch_stack(context):
# G20 velocity read sends another five synchronous CAN
# queries, so keep it off the trajectory-critical path.
"velocity_poll_rate": 1.0,
# Calibration sends one endpoint command per sweep. Keep
# polling the real motor state during the following motion;
# otherwise the SDK republishes a stale state for 0.2 s and
# creates 17-27 command-unit holes in the trajectory bins.
# G20 sends an endpoint and L6 streams a bounded trajectory.
# Keep polling the real motor state during either command path;
# otherwise the SDK republishes stale state and creates large
# command-unit holes in the trajectory bins.
"defer_state_reads_while_commanding": False,
"repeat_position_commands": False,
"is_touch": False,
@@ -278,7 +285,7 @@ def _launch_stack(context):
calibration = Node(
package="linkerhand_calibration",
executable="three_camera_calibration_node",
name="g20_calibration",
name=f"{model.lower()}_calibration",
output="screen",
emulate_tty=True,
arguments=[
@@ -303,7 +310,7 @@ def _launch_stack(context):
# cb_<side>_hand_info has a subscriber. Calibration only used
# that topic to display a speed diagnostic, while those reads
# created 17-33 command-unit holes in position trajectories.
"info_topic": "/g20_calibration/disabled_hand_info",
"info_topic": f"{calibration_namespace}/disabled_hand_info",
"command_topic": command_topic,
"state_topic": state_topic,
"camera_extrinsics_file": LaunchConfiguration(
@@ -313,6 +320,15 @@ def _launch_stack(context):
"source_urdf_expected_sha256": LaunchConfiguration(
"source_urdf_expected_sha256"
),
"camera_extrinsics_expected_sha256": LaunchConfiguration(
"camera_extrinsics_expected_sha256"
),
"calibration_config_expected_sha256": LaunchConfiguration(
"calibration_config_expected_sha256"
),
"tag_config_expected_sha256": LaunchConfiguration(
"tag_config_expected_sha256"
),
"corrected_urdf_output_dir": LaunchConfiguration(
"corrected_urdf_output_dir"
),
@@ -367,14 +383,14 @@ def _launch_stack(context):
command_topic,
state_topic,
info_topic,
"/g20_calibration/status",
f"{calibration_namespace}/status",
],
output="screen",
)
return [
LogInfo(
msg=(
f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; "
f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; "
f"source_urdf={source_urdf}"
)
),
@@ -482,6 +498,15 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"source_urdf_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"camera_extrinsics_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"calibration_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"tag_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"corrected_urdf_output_dir", default_value=""
),
@@ -117,7 +117,7 @@ def interpolate_state_u8(
*,
maximum_skew_ns: int,
) -> tuple[tuple[float, ...], int] | None:
"""Interpolate the 20-D hand state at an image timestamp.
"""Interpolate a profile-sized hand state at an image timestamp.
The SDK publishes state independently from the camera. Continuous
calibration must therefore use the image timestamp instead of whichever
@@ -151,7 +151,11 @@ def interpolate_state_u8(
fraction = before_gap / denominator
before_values = np.asarray(before.position_u8, dtype=float)
after_values = np.asarray(after.position_u8, dtype=float)
if before_values.shape != (20,) or after_values.shape != (20,):
if (
before_values.ndim != 1
or before_values.size == 0
or after_values.shape != before_values.shape
):
return None
interpolated = before_values + fraction * (after_values - before_values)
return (
@@ -1,4 +1,4 @@
"""Map G20 u8 feedback to URDF joint angles using one calibration JSON.
"""Map model SDK u8 feedback to URDF joint angles using one calibration JSON.
The static encoder-zero corrections in ``zero_angles`` are already baked into
the corrected URDF joint origins. This bridge therefore publishes only the
@@ -26,6 +26,7 @@ from .full_hand import (
infer_compact_payload_layout,
validate_compact_payload,
)
from .models.l6.artifacts import validate_l6_runtime_payload
G20_COMMAND_NAMES: tuple[str, ...] = (
@@ -80,12 +81,16 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = (
class CalibratedCommandMapper:
"""Validated, side-specific lookup from G20 u8 values to URDF radians."""
"""Validated, profile-specific lookup from SDK u8 values to URDF radians."""
def __init__(
self, payload: Mapping[str, Any], *, expected_side: str | None = None
) -> None:
validate_compact_payload(payload)
schema_version = int(payload["schema_version"])
if schema_version == 6:
validate_l6_runtime_payload(payload)
else:
validate_compact_payload(payload)
side = str(payload["side"]).lower()
if expected_side is not None and side != str(expected_side).lower():
raise ValueError(
@@ -95,12 +100,18 @@ class CalibratedCommandMapper:
quality = payload["quality"]
if quality.get("passed") is not True:
raise ValueError("calibration quality.passed must be true")
layout_id = infer_compact_payload_layout(payload)
profile = get_hand_calibration_profile(side, layout_id)
layout_id = (
str(payload["layout_id"])
if schema_version == 6
else infer_compact_payload_layout(payload)
)
self.side = side
self.layout_id = layout_id
self.model = str(payload["model"]).upper()
self.profile_id = str(
payload.get("profile_id", f"G20/{side}/{layout_id}/v1")
)
self.serial_number = str(payload["serial_number"])
schema_version = int(payload["schema_version"])
self.input_domain = str(
payload.get(
"curve_input_domain",
@@ -109,16 +120,29 @@ class CalibratedCommandMapper:
)
if self.input_domain not in {"command_u8", "feedback_u8"}:
raise ValueError("calibration curve_input_domain is invalid")
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in G20_URDF_JOINT_NAMES
}
if schema_version == 6:
self.command_names = tuple(str(value) for value in payload["command_names"])
self.urdf_joint_names = tuple(str(name) for name in payload["joints"])
self._motor_by_joint = {
name: int(payload["joints"][name]["motor_index"])
for name in self.urdf_joint_names
}
self.feedback_name_aliases = {"thumb_cmc_yaw": "thumb_cmc_roll"}
else:
profile = get_hand_calibration_profile(side, layout_id)
self.command_names = G20_COMMAND_NAMES
self.urdf_joint_names = G20_URDF_JOINT_NAMES
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in self.urdf_joint_names
}
self.feedback_name_aliases = {}
self._curves = {
name: tuple(
float(value)
for value in payload["joints"][name]["angle_rad"]
)
for name in G20_URDF_JOINT_NAMES
for name in self.urdf_joint_names
}
self._decreasing_curves = {
name: tuple(
@@ -127,7 +151,7 @@ class CalibratedCommandMapper:
"decreasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
for name in self.urdf_joint_names
}
self._increasing_curves = {
name: tuple(
@@ -136,7 +160,7 @@ class CalibratedCommandMapper:
"increasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
for name in self.urdf_joint_names
}
self._previous_by_motor: dict[int, float] = {}
self._direction_by_motor: dict[int, str] = {}
@@ -161,16 +185,21 @@ class CalibratedCommandMapper:
if len(set(names)) != len(names):
raise ValueError("JointState names must be unique")
by_name = dict(zip((str(name) for name in names), values))
missing = [name for name in G20_COMMAND_NAMES if name not in by_name]
for alias, canonical in self.feedback_name_aliases.items():
if alias in by_name and canonical not in by_name:
by_name[canonical] = by_name[alias]
missing = [name for name in self.command_names if name not in by_name]
if missing:
raise ValueError(
"G20 command is missing named channels: " + ",".join(missing)
f"{self.model} feedback is missing named channels: "
+ ",".join(missing)
)
command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
command = tuple(by_name[name] for name in self.command_names)
else:
if len(values) != len(G20_COMMAND_NAMES):
if len(values) != len(self.command_names):
raise ValueError(
"unnamed G20 command must contain exactly 20 positions"
f"unnamed {self.model} feedback must contain exactly "
f"{len(self.command_names)} positions"
)
command = values
indices = tuple(self._command_index(value) for value in command)
@@ -185,7 +214,7 @@ class CalibratedCommandMapper:
direction = "decreasing"
direction_by_motor[motor] = direction
result: list[float] = []
for name in G20_URDF_JOINT_NAMES:
for name in self.urdf_joint_names:
motor = self._motor_by_joint[name]
direction = direction_by_motor[motor]
curves = (
@@ -214,20 +243,22 @@ def load_calibrated_command_mapper(
return CalibratedCommandMapper(payload, expected_side=expected_side)
def default_input_topic(hand_type: str, input_domain: str) -> str:
def default_input_topic(
hand_type: str, input_domain: str, model: str = "G20"
) -> str:
side = str(hand_type).lower()
if side not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
if input_domain == "feedback_u8":
return f"/g20/cb_{side}_hand_state"
return f"/{str(model).lower()}/cb_{side}_hand_state"
if input_domain == "command_u8":
return f"/g20/cb_{side}_hand_control_cmd"
return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
raise ValueError("calibration curve_input_domain is invalid")
class CalibratedJointStateBridge(Node):
def __init__(self) -> None:
super().__init__("g20_calibrated_joint_state_bridge")
super().__init__("calibrated_joint_state_bridge")
self.declare_parameter("hand_type", "right")
self.declare_parameter("calibration_file", "")
self.declare_parameter("input_topic", "")
@@ -245,10 +276,11 @@ class CalibratedJointStateBridge(Node):
input_topic = str(self.get_parameter("input_topic").value).strip()
output_topic = str(self.get_parameter("output_topic").value).strip()
self.input_topic = input_topic or default_input_topic(
hand_type, self.mapper.input_domain
hand_type, self.mapper.input_domain, self.mapper.model
)
self.output_topic = (
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
output_topic
or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state"
)
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
self.subscription = self.create_subscription(
@@ -256,7 +288,8 @@ class CalibratedJointStateBridge(Node):
)
self._last_error = ""
self.get_logger().info(
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
f"loaded {self.mapper.profile_id} calibration for "
f"{self.mapper.serial_number}: "
f"{self.input_topic} ({self.mapper.input_domain}) -> "
f"{self.output_topic}"
)
@@ -273,7 +306,7 @@ class CalibratedJointStateBridge(Node):
self._last_error = ""
result = JointState()
result.header = command.header
result.name = list(G20_URDF_JOINT_NAMES)
result.name = list(self.mapper.urdf_joint_names)
result.position = list(positions)
self.publisher.publish(result)
@@ -50,6 +50,15 @@ class CommandLayout:
baseline_u8: tuple[int, ...]
command_index_by_joint: Mapping[str, int]
disabled_indices: frozenset[int] = frozenset()
# Calibration names are allowed to stay model-neutral while the source
# URDF keeps any vendor/side prefixes (for example ``rh_``).
urdf_joint_by_joint: Mapping[str, str] = field(default_factory=dict)
# Older SDKs occasionally published a wrong label for a physically stable
# channel. Aliases are accepted only at the declared channel index.
feedback_name_aliases: Mapping[str, str] = field(default_factory=dict)
# SDK speed commands are not necessarily one value per position channel.
# This mapping makes that protocol detail explicit in a profile.
speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict)
@property
def command_count(self) -> int:
@@ -98,6 +107,10 @@ class TaskSpec:
joints: tuple[str, ...]
auxiliary_commands: tuple[tuple[int, int], ...] = ()
validation_only: bool = False
start_u8: int = 255
end_u8: int = 0
preflight_speed_u8: int | None = None
formal_speed_u8: int | None = None
@dataclass(frozen=True)
@@ -148,6 +161,15 @@ class ZeroSolvePolicy:
post_solve_endpoint_joints: frozenset[str]
mimic_source_by_joint: Mapping[str, str]
cad_frozen_joints: frozenset[str]
# ``upper_at_end`` means TaskSpec.end_u8 is the trusted source-URDF upper
# physical endpoint. The measured travel then defines the electrical
# zero and corrected [0, travel] coordinate range.
endpoint_anchor_by_joint: Mapping[str, str] = field(default_factory=dict)
fitted_mimic_joints: frozenset[str] = frozenset()
# Passive coupling is not necessarily representable by the linear URDF
# ``mimic`` element. Profiles must opt in explicitly before a nonlinear
# runtime/MuJoCo relation may be published.
coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict)
@dataclass(frozen=True)
@@ -195,6 +217,11 @@ class CalibrationProfile:
quality: QualityPolicy
scope: ScopePolicy
artifacts: ArtifactPolicy
# Per-URDF-joint provenance used by partial calibration artifacts.
# Known values are: measured_static_dynamic, measured_dynamic_cad_static,
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
# and mimic_nominal.
joint_coverage: Mapping[str, str] = field(default_factory=dict)
class ProfileValidationError(ValueError):
@@ -216,6 +243,24 @@ def validate_profile(profile: CalibrationProfile) -> None:
errors.append("disabled command index is out of range")
if any(index not in indices for index in command.command_index_by_joint.values()):
errors.append("joint command index is out of range")
if command.urdf_joint_by_joint:
if not set(command.command_index_by_joint).issubset(
command.urdf_joint_by_joint
):
errors.append("every commanded joint must map to a URDF joint")
urdf_names = tuple(command.urdf_joint_by_joint.values())
if len(set(urdf_names)) != len(urdf_names):
errors.append("URDF joint mappings must be unique")
if any(
index not in indices or slot < 0
for index, slot in command.speed_slot_by_command_index.items()
):
errors.append("speed-slot mapping is invalid")
if any(
not str(alias).strip() or canonical not in command.names
for alias, canonical in command.feedback_name_aliases.items()
):
errors.append("feedback name alias is not part of the command schema")
view_names = profile.vision.view_names
if not view_names or len(set(view_names)) != len(view_names):
@@ -246,6 +291,13 @@ def validate_profile(profile: CalibrationProfile) -> None:
errors.append(f"task {task.key} references unknown measurements")
if any(index not in indices for index, _ in task.auxiliary_commands):
errors.append(f"task {task.key} auxiliary index is out of range")
if not 0 <= task.start_u8 <= 255 or not 0 <= task.end_u8 <= 255:
errors.append(f"task {task.key} sweep endpoint is out of range")
if task.start_u8 == task.end_u8:
errors.append(f"task {task.key} sweep endpoints must differ")
for speed in (task.preflight_speed_u8, task.formal_speed_u8):
if speed is not None and not 0 <= speed <= 255:
errors.append(f"task {task.key} speed is out of range")
for name, spec in profile.measurement.measurements.items():
if name != spec.joint:
errors.append(f"measurement mapping key differs for {name}")
@@ -273,6 +325,29 @@ def validate_profile(profile: CalibrationProfile) -> None:
errors.append("mimic targets must be passive joints")
if not set(zero.mimic_source_by_joint.values()).issubset(all_joints):
errors.append("mimic sources must be known joints")
if not set(zero.endpoint_anchor_by_joint).issubset(zero.active_joints):
errors.append("endpoint anchors must target active joints")
if not set(zero.endpoint_anchor_by_joint.values()).issubset(
{
"upper_at_end",
"lower_at_start",
"zero_at_start",
"cad_range_center",
}
):
errors.append("endpoint anchor policy is unsupported")
if not zero.fitted_mimic_joints.issubset(zero.passive_joints):
errors.append("fitted mimic targets must be passive joints")
if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint):
errors.append("fitted mimic target has no source mapping")
if not set(zero.coupling_model_by_joint).issubset(
zero.mimic_source_by_joint
):
errors.append("coupling model target has no source mapping")
if not set(zero.coupling_model_by_joint.values()).issubset(
{"linear_mimic", "quadratic_runtime"}
):
errors.append("coupling model policy is unsupported")
scopes = set(profile.scope.calibrate_joints)
if profile.scope.default_scope not in scopes:
@@ -297,6 +372,19 @@ def validate_profile(profile: CalibrationProfile) -> None:
errors.append(f"{label} filename must not contain a directory")
if not profile.namespace.startswith("/"):
errors.append("runtime namespace must be absolute")
if profile.joint_coverage:
valid_coverage = {
"measured_static_dynamic",
"measured_dynamic_cad_static",
"transferred_static_dynamic",
"transferred_dynamic_cad_static",
"cad_nominal",
"mimic_nominal",
}
if set(profile.joint_coverage) != all_joints:
errors.append("joint coverage must describe every profile joint")
if not set(profile.joint_coverage.values()).issubset(valid_coverage):
errors.append("joint coverage contains an unsupported status")
if errors:
raise ProfileValidationError("; ".join(errors))
@@ -67,6 +67,11 @@ class ZeroCalibrationProfile:
# thumb kernel instead uses only the serial thumb chain, so its nuisance
# palm pose cannot be influenced by finger observations.
base_pose_strategy: str = "full_hand"
# A serial-chain model may use a separate palm-root joint axis to fix the
# otherwise free rotation about its primary root axis. G20 retains its
# historical defaults; other model profiles can name the physical anchor
# explicitly without introducing model-specific branches in the solver.
orientation_anchor_joint: str | None = None
@property
def reference_finger(self) -> str:
@@ -3238,7 +3243,7 @@ def solve_urdf_zero_offsets(
np.ptp(np.asarray(curves[name].angle_rad))
),
)
orientation_anchor = (
orientation_anchor = profile.orientation_anchor_joint or (
"thumb_cmc_pitch"
if profile.base_pose_strategy == "thumb_serial"
else f"{profile.reference_finger}_mcp_pitch"
@@ -1 +1,12 @@
"""Future profiles live here; no publishable profile is registered yet."""
"""Registered L6 calibration profiles."""
from ..registry import ProfileRegistry
def register_profiles(registry: ProfileRegistry) -> None:
from .profile import build_profile
registry.register(build_profile())
__all__ = ["register_profiles"]
@@ -0,0 +1,524 @@
"""Schema-v6 runtime artifact and atomic partial-result publication for L6."""
from __future__ import annotations
import json
import hashlib
import math
import os
from pathlib import Path
from typing import Any, Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from .fitting import L6FitResult
from .profile import (
ACTIVE_JOINTS,
CALIBRATED_ACTIVE_JOINTS,
COMMAND_INDEX_BY_JOINT,
COMMAND_NAMES,
COUPLING_MODEL_BY_JOINT,
ENDPOINT_ANCHOR_BY_JOINT,
KEY,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
PASSIVE_JOINTS,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
build_typed_profile,
)
ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS)
def _sha256_file(path: str | Path) -> str:
digest = hashlib.sha256()
with Path(path).open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _source_joint_metadata(
source_urdf: str | Path,
) -> dict[str, dict[str, float | str]]:
root = ET.parse(Path(source_urdf)).getroot()
result: dict[str, dict[str, float | str]] = {}
for joint in root.findall("joint"):
name = str(joint.get("name", ""))
if name not in ALL_REVOLUTE_JOINTS:
continue
if joint.get("type") != "revolute":
raise ValueError(f"L6 profile joint is not revolute: {name}")
limit = joint.find("limit")
if limit is None:
raise ValueError(f"L6 source joint has no limit: {name}")
item: dict[str, float | str] = {
"lower": float(limit.get("lower", "nan")),
"upper": float(limit.get("upper", "nan")),
}
mimic = joint.find("mimic")
if mimic is not None:
item.update(
{
"source_joint": str(mimic.get("joint", "")),
"multiplier": float(mimic.get("multiplier", "nan")),
"offset": float(mimic.get("offset", "0")),
}
)
result[name] = item
if set(result) != ALL_REVOLUTE_JOINTS:
raise ValueError("source URDF does not contain exactly 11 L6 revolute joints")
return result
def _rounded_curve(values: Sequence[float]) -> list[float]:
curve = np.asarray(values, dtype=float)
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
raise ValueError("runtime curve must contain 256 finite values")
return [round(float(value), 8) for value in curve]
def _linear_curve(lower: float, upper: float) -> np.ndarray:
# feedback 255 is the open/lower endpoint and feedback 0 is upper/closed.
return np.linspace(float(upper), float(lower), 256, dtype=float)
def _validate_transfer_topology(
source: Mapping[str, Mapping[str, float | str]],
) -> None:
for transfers in (
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
):
for target, donor in transfers.items():
for field in ("lower", "upper"):
if abs(
float(source[target][field]) - float(source[donor][field])
) > 1.0e-8:
raise ValueError(
f"L6 transfer {target} differs from {donor} {field}"
)
def build_l6_runtime_payload(
*,
serial_number: str,
source_urdf: str | Path,
result: L6FitResult,
protected_inputs: Mapping[str, str],
passed: bool = True,
) -> dict[str, Any]:
"""Build all eleven L6 curves with explicit measured/frozen provenance."""
profile = build_typed_profile()
source = _source_joint_metadata(source_urdf)
_validate_transfer_topology(source)
joints: dict[str, dict[str, Any]] = {}
curves: dict[str, np.ndarray] = {}
for name in ACTIVE_JOINTS:
metadata = source[name]
motor_index = COMMAND_INDEX_BY_JOINT[name]
measurement_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(
name, name
)
if (
name in CALIBRATED_ACTIVE_JOINTS
or name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
):
fit = result.curves[measurement_source]
decreasing = np.asarray(fit.decreasing_rad, dtype=float)
increasing = np.asarray(fit.increasing_rad, dtype=float)
curve = 0.5 * (decreasing + increasing)
curve -= float(curve[255])
decreasing = decreasing - float(decreasing[255])
increasing = increasing - float(increasing[255])
zero_method = result.zero_method_by_joint[measurement_source]
zero_angles: dict[str, Any] = {
"policy": zero_method,
"measured_travel_rad": round(
float(result.travels_rad[measurement_source]), 8
),
"urdf_origin_offset_rad": round(
float(result.zero_offsets_rad[measurement_source]), 8
),
}
if measurement_source in ENDPOINT_ANCHOR_BY_JOINT:
anchor = ENDPOINT_ANCHOR_BY_JOINT[measurement_source]
if anchor == "cad_range_center":
zero_angles.update(
{
"source_lower_rad": round(
float(metadata["lower"]), 8
),
"source_upper_rad": round(
float(metadata["upper"]), 8
),
}
)
else:
endpoint_field = {
"lower_at_start": "source_lower_rad",
"upper_at_end": "source_upper_rad",
"zero_at_start": "source_joint_zero_rad",
}[anchor]
endpoint_value = {
"lower_at_start": metadata["lower"],
"upper_at_end": metadata["upper"],
"zero_at_start": 0.0,
}[anchor]
zero_angles[endpoint_field] = round(
float(endpoint_value), 8
)
if measurement_source in result.zero_fallback_reason_by_joint:
zero_angles["geometry_fallback_reason"] = (
result.zero_fallback_reason_by_joint[measurement_source]
)
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT:
zero_angles.update(
{
"policy": "transferred_from_pinky",
"transferred_from_joint": measurement_source,
}
)
else:
curve = _linear_curve(
float(metadata["lower"]), float(metadata["upper"])
)
decreasing = curve.copy()
increasing = curve.copy()
zero_angles = {
"policy": "cad_nominal",
"source_lower_rad": round(float(metadata["lower"]), 8),
"source_upper_rad": round(float(metadata["upper"]), 8),
}
curves[name] = curve
joint_payload = {
"urdf_joint": name,
"sdk_channel": COMMAND_NAMES[motor_index],
"motor_index": motor_index,
"passive": False,
"calibration_status": profile.joint_coverage[name],
"zero_command_u8": 255,
"zero_angles": zero_angles,
"angle_rad": _rounded_curve(curve),
"decreasing_rad": _rounded_curve(decreasing),
"increasing_rad": _rounded_curve(increasing),
}
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT:
joint_payload["transferred_from_joint"] = measurement_source
joints[name] = joint_payload
for name in PASSIVE_JOINTS:
metadata = source[name]
source_name = MIMIC_SOURCE_BY_JOINT[name]
motor_index = COMMAND_INDEX_BY_JOINT[source_name]
measurement_source = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
name, name
)
if (
name in MEASURED_PASSIVE_JOINTS
or name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT
):
fit = result.curves[measurement_source]
decreasing = np.asarray(fit.decreasing_rad, dtype=float)
increasing = np.asarray(fit.increasing_rad, dtype=float)
curve = 0.5 * (decreasing + increasing)
curve -= float(curve[255])
decreasing = decreasing - float(decreasing[255])
increasing = increasing - float(increasing[255])
offset = float(metadata["offset"])
curve += offset
decreasing += offset
increasing += offset
coupling = result.mimic_fits[measurement_source]
coupling_model = coupling.model
coefficients = list(coupling.mujoco_polycoef)
coefficients[0] = offset
urdf_mimic_multiplier = coupling.urdf_mimic_multiplier
urdf_mimic_policy = coupling.urdf_mimic_policy
else:
multiplier = float(metadata["multiplier"])
offset = float(metadata["offset"])
curve = offset + multiplier * curves[source_name]
decreasing = curve.copy()
increasing = curve.copy()
coupling_model = "linear_mimic"
coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0]
urdf_mimic_multiplier = multiplier
urdf_mimic_policy = "cad_nominal"
joint_payload = {
"urdf_joint": name,
"sdk_channel": COMMAND_NAMES[motor_index],
"motor_index": motor_index,
"passive": True,
"source_joint": source_name,
"mimic_offset_rad": round(float(metadata["offset"]), 8),
"coupling_model": coupling_model,
"coupling_coefficients": [
round(float(value), 10) for value in coefficients
],
"urdf_mimic_enabled": True,
"urdf_mimic_policy": urdf_mimic_policy,
"mimic_multiplier": round(float(urdf_mimic_multiplier), 8),
"calibration_status": profile.joint_coverage[name],
"zero_command_u8": 255,
"zero_angles": {"policy": "cad_static"},
"angle_rad": _rounded_curve(curve),
"decreasing_rad": _rounded_curve(decreasing),
"increasing_rad": _rounded_curve(increasing),
}
joints[name] = joint_payload
if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT:
joint_payload["transferred_from_joint"] = measurement_source
errors = np.abs(
np.concatenate(
[np.asarray(values, dtype=float) for values in result.holdout_errors_rad.values()]
)
)
payload: dict[str, Any] = {
"schema_version": 6,
"profile_id": KEY.profile_id,
"layout_id": KEY.layout,
"model": "L6",
"side": "right",
"serial_number": str(serial_number),
"calibration_scope": "partial",
"publication_pointer": "latest_partial_passed",
"angle_unit": "rad",
"command_range": [0, 255],
"curve_input_domain": "feedback_u8",
"runtime_curve_policy": "direction_aware",
"command_names": list(COMMAND_NAMES),
"baseline_command_u8": [255] * 6,
"protected_inputs": dict(protected_inputs),
"joints": joints,
"quality": {
"passed": bool(passed),
"scope": "partial",
"validation_mae_rad": round(float(np.mean(errors)), 8),
"validation_p95_rad": round(float(np.percentile(errors, 95.0)), 8),
"validation_max_rad": round(float(np.max(errors)), 8),
"thumb_axis_zero": (
None
if result.thumb_zero_result is None
else {
"method": (
"hybrid_axis_geometry_cad_range_center"
if result.zero_fallback_reason_by_joint
else "g20_serial_axis_geometry"
),
"passed": bool(result.thumb_zero_result.passed),
"geometry_fallback_reasons": dict(
sorted(result.zero_fallback_reason_by_joint.items())
),
"axis_line_rms_m": round(
float(result.thumb_zero_result.axis_line_rms_m), 10
),
"offsets_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.direct_offsets_rad.items()
)
},
"cycle_offsets_rad": {
name: [round(float(value), 10) for value in values]
for name, values in sorted(
result.thumb_zero_result.cycle_offsets_rad.items()
)
},
"validation_error_by_joint_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.validation_error_by_joint_rad.items()
)
},
}
),
},
}
validate_l6_runtime_payload(payload)
return payload
def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
required_top = {
"schema_version", "profile_id", "layout_id", "model", "side",
"serial_number", "calibration_scope", "publication_pointer",
"angle_unit", "command_range", "curve_input_domain",
"runtime_curve_policy", "command_names", "baseline_command_u8",
"protected_inputs", "joints", "quality",
}
if set(payload) != required_top:
raise ValueError("schema v6 calibration has unexpected top-level fields")
if (
payload["schema_version"] != 6
or payload["profile_id"] != KEY.profile_id
or payload["model"] != "L6"
or payload["side"] != "right"
or payload["calibration_scope"] != "partial"
):
raise ValueError("schema v6 identity is invalid")
if payload["publication_pointer"] != "latest_partial_passed":
raise ValueError("L6 partial result has the wrong publication pointer")
if payload["curve_input_domain"] != "feedback_u8":
raise ValueError("schema v6 must be indexed by feedback_u8")
if payload["runtime_curve_policy"] != "direction_aware":
raise ValueError("schema v6 must retain both motion directions")
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
raise ValueError("schema v6 units are invalid")
if tuple(payload["command_names"]) != COMMAND_NAMES:
raise ValueError("schema v6 command channel order is invalid")
if payload["baseline_command_u8"] != [255] * 6:
raise ValueError("schema v6 baseline must be six open commands")
protected = payload["protected_inputs"]
expected_hashes = {
"source_urdf_sha256", "camera_extrinsics_sha256",
"calibration_config_sha256", "tag_config_sha256",
}
if not isinstance(protected, Mapping) or set(protected) != expected_hashes:
raise ValueError("schema v6 protected inputs are incomplete")
if any(
len(str(value)) != 64
or any(char not in "0123456789abcdef" for char in str(value))
for value in protected.values()
):
raise ValueError("schema v6 protected input hash is invalid")
joints = payload["joints"]
if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS:
raise ValueError("schema v6 must contain all 11 L6 revolute joints")
profile = build_typed_profile()
for name in ACTIVE_JOINTS + PASSIVE_JOINTS:
joint = joints[name]
motor = COMMAND_INDEX_BY_JOINT[
MIMIC_SOURCE_BY_JOINT.get(name, name)
]
if joint.get("urdf_joint") != name or int(joint.get("motor_index", -1)) != motor:
raise ValueError(f"{name} has an invalid URDF/SDK mapping")
if joint.get("sdk_channel") != COMMAND_NAMES[motor]:
raise ValueError(f"{name} has an invalid SDK channel")
if joint.get("calibration_status") != profile.joint_coverage[name]:
raise ValueError(f"{name} has an invalid coverage status")
if joint.get("passive") is not (name in PASSIVE_JOINTS):
raise ValueError(f"{name} passive flag is invalid")
if joint.get("zero_command_u8") != 255:
raise ValueError(f"{name} zero command must be 255")
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
curve = np.asarray(joint.get(field), dtype=float)
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
raise ValueError(f"{name}.{field} must contain 256 finite values")
if np.any(np.diff(curve) > 1.0e-7):
raise ValueError(f"{name}.{field} must be non-increasing")
if name in PASSIVE_JOINTS:
if joint.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name]:
raise ValueError(f"{name} mimic source is invalid")
model = str(joint.get("coupling_model", ""))
expected_model = COUPLING_MODEL_BY_JOINT.get(
name, "linear_mimic"
)
if model != expected_model:
raise ValueError(f"{name} coupling model is invalid")
coefficients = np.asarray(
joint.get("coupling_coefficients"), dtype=float
)
if coefficients.shape != (6,) or not np.all(
np.isfinite(coefficients)
):
raise ValueError(f"{name} coupling coefficients are invalid")
enabled = joint.get("urdf_mimic_enabled")
if enabled is not True:
raise ValueError(f"{name} URDF mimic policy is invalid")
expected_policy = (
"endpoint_linear_fallback"
if model == "quadratic_runtime"
else "exact_linear"
if name in MEASURED_PASSIVE_JOINTS
else "cad_nominal"
)
# Early schema-v6 linear artifacts predate the explicit policy
# label; their unambiguous model/coverage combination remains
# readable. New writers always materialize the field.
policy = str(
joint.get("urdf_mimic_policy", expected_policy)
)
if policy != expected_policy:
raise ValueError(f"{name} URDF mimic fallback is invalid")
multiplier = float(joint.get("mimic_multiplier", "nan"))
if not math.isfinite(multiplier) or multiplier <= 0.0:
raise ValueError(f"{name} mimic multiplier is invalid")
if model == "linear_mimic":
if abs(multiplier - coefficients[1]) > 1.0e-7:
raise ValueError(f"{name} mimic multiplier is inconsistent")
if np.any(np.abs(coefficients[2:]) > 1.0e-10):
raise ValueError(f"{name} linear mimic is not linear")
if abs(float(coefficients[0]) - float(
joint.get("mimic_offset_rad", "nan")
)) > 1.0e-7:
raise ValueError(f"{name} coupling offset is inconsistent")
transferred_from = (
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name)
or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name)
)
if transferred_from is not None:
if joint.get("transferred_from_joint") != transferred_from:
raise ValueError(f"{name} transfer provenance is invalid")
donor = joints[transferred_from]
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
if joint[field] != donor[field]:
raise ValueError(f"{name} transfer curve differs from donor")
quality = payload["quality"]
if not isinstance(quality, Mapping) or quality.get("scope") != "partial":
raise ValueError("schema v6 quality scope must be partial")
if quality.get("passed") is not True:
raise ValueError("schema v6 quality.passed must be true")
def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> Path:
destination = Path(path).resolve()
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_name(f".{destination.name}.{os.getpid()}.tmp")
try:
with temporary.open("w", encoding="utf-8") as stream:
json.dump(payload, stream, ensure_ascii=False, indent=2, sort_keys=True)
stream.write("\n")
stream.flush()
os.fsync(stream.fileno())
os.replace(temporary, destination)
finally:
if temporary.exists():
temporary.unlink()
return destination
def publish_partial_session(serial_root: str | Path, session: str | Path) -> Path:
parent = Path(serial_root).resolve()
target = Path(session).resolve()
if target.parent != parent or not target.is_dir():
raise ValueError("partial session must be a direct existing child")
destination = parent / "latest_partial_passed"
temporary = parent / f".latest_partial_passed.{os.getpid()}.tmp"
if temporary.exists() or temporary.is_symlink():
temporary.unlink()
os.symlink(target.name, temporary, target_is_directory=True)
os.replace(temporary, destination)
return destination
def artifact_hashes(json_path: str | Path, urdf_path: str | Path) -> dict[str, str]:
return {
"calibration_json_sha256": _sha256_file(json_path),
"corrected_urdf_sha256": _sha256_file(urdf_path),
}
__all__ = [
"ALL_REVOLUTE_JOINTS",
"artifact_hashes",
"atomic_write_json",
"build_l6_runtime_payload",
"publish_partial_session",
"validate_l6_runtime_payload",
]
@@ -0,0 +1,708 @@
"""L6 curve, endpoint-zero, holdout, and mimic fitting."""
from __future__ import annotations
from dataclasses import dataclass
import math
from pathlib import Path
from typing import Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from scipy.optimize import least_squares
from ..g20.profile import (
HandCalibrationProfile,
JointCurveFit,
JointSpec,
)
from ..g20.zero_solver import (
ZeroCalibrationProfile,
ZeroSolveResult,
fit_joint_axis_measurement,
fit_rotation_joint_curve,
rotation_curve_holdout_errors,
solve_urdf_zero_offsets,
with_depth_free_axis_projection,
)
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
COMMAND_INDEX_BY_JOINT,
COMMAND_NAMES,
COUPLING_MODEL_BY_JOINT,
ENDPOINT_ANCHOR_BY_JOINT,
KEY,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
)
@dataclass(frozen=True)
class MimicFit:
source_joint: str
target_joint: str
model: str
coefficients: tuple[float, ...]
urdf_mimic_multiplier: float
urdf_mimic_policy: str
cycle_coefficients: tuple[tuple[float, ...], ...]
maximum_cycle_range: float
maximum_cycle_prediction_range_rad: float
residual_rms_rad: float
residual_p95_rad: float
residual_max_rad: float
@property
def multiplier(self) -> float:
"""Linear term retained for compatible diagnostics and artifacts."""
return float(self.coefficients[0])
@property
def cycle_multipliers(self) -> tuple[float, ...]:
return tuple(float(values[0]) for values in self.cycle_coefficients)
@property
def mujoco_polycoef(self) -> tuple[float, ...]:
"""MuJoCo q_target = p0 + p1*q_source + ... coefficients."""
return (0.0, *self.coefficients, *(0.0,) * (5 - len(self.coefficients)))
@dataclass(frozen=True)
class L6FitResult:
curves: Mapping[str, JointCurveFit]
zero_offsets_rad: Mapping[str, float]
travels_rad: Mapping[str, float]
mimic_fits: Mapping[str, MimicFit]
holdout_errors_rad: Mapping[str, tuple[float, ...]]
zero_method_by_joint: Mapping[str, str]
zero_fallback_reason_by_joint: Mapping[str, str]
thumb_zero_result: ZeroSolveResult | None = None
L6_THUMB_AXIS_JOINTS: tuple[str, ...] = (
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
"rh_thumb_dip",
"rh_pinky_mcp_pitch",
)
def _l6_thumb_zero_profile() -> ZeroCalibrationProfile:
"""Describe the observable L6 thumb/palm axis graph to the G20 solver."""
joint_specs = {
"rh_thumb_cmc_roll": JointSpec(
"rh_thumb_cmc_roll", 1, True, "top", "top_base", "thumb_roll"
),
"rh_thumb_cmc_pitch": JointSpec(
"rh_thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_pitch"
),
"rh_thumb_dip": JointSpec(
"rh_thumb_dip", 0, False, "front", "thumb_pitch", "thumb_dip"
),
# This root axis fixes the palm-frame phase around the thumb roll axis.
# Its electrical zero is irrelevant because rotating a revolute joint
# does not change its own physical screw axis.
"rh_pinky_mcp_pitch": JointSpec(
"rh_pinky_mcp_pitch", 5, True, "side", "side_base", "pinky_pitch"
),
}
hand = HandCalibrationProfile(
side="right",
reference_finger="pinky",
view_tags={},
preflight_view_roles={},
joint_specs=joint_specs,
sweep_specs=(),
image_trajectory_joints=frozenset(),
roll_clearance_commands={},
thumb_pitch_clearance_commands={},
layout_id=KEY.layout,
model="L6",
command_names=COMMAND_NAMES,
baseline_command=(255,) * 6,
directional_zero=True,
isolated_holdout=True,
)
return ZeroCalibrationProfile(
hand=hand,
direct_zero_joints=(
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
),
axis_joints=L6_THUMB_AXIS_JOINTS,
inherited_zero_joints={},
inherited_static_zero_joints={},
constrained_circle_joints=frozenset(L6_THUMB_AXIS_JOINTS),
root_anchor_joints=frozenset({"rh_thumb_cmc_roll"}),
axis_parent_joint={
"rh_thumb_cmc_pitch": "rh_thumb_cmc_roll",
},
phase_parent_joint={
"rh_thumb_dip": "rh_thumb_cmc_pitch",
},
offset_observer_joint={
"rh_thumb_cmc_roll": "rh_thumb_cmc_pitch",
"rh_thumb_cmc_pitch": "rh_thumb_dip",
},
same_view_axis_pair_by_offset={},
fixed_direct_zero_offsets_rad={},
static_output_zero_offsets_rad={},
base_pose_strategy="thumb_serial",
orientation_anchor_joint="rh_pinky_mcp_pitch",
)
def curve_travel_rad(fit: JointCurveFit) -> float:
decreasing = float(fit.decreasing_rad[0] - fit.decreasing_rad[255])
increasing = float(fit.increasing_rad[0] - fit.increasing_rad[255])
travel = 0.5 * (decreasing + increasing)
if not math.isfinite(travel) or travel <= 0.0:
raise ValueError("L6 fitted travel must be finite and positive")
return travel
def derive_endpoint_zero_offsets(
source_urdf: str | Path,
curves: Mapping[str, JointCurveFit],
*,
endpoint_anchor_by_joint: Mapping[str, str] | None = None,
maximum_offset_rad: float = math.radians(15.0),
) -> tuple[dict[str, float], dict[str, float]]:
"""Anchor each measured joint to its profile-selected CAD endpoint.
Every published dynamic curve is zero at feedback 255. ``upper_at_end``
rotates the static frame by ``source_upper - measured_travel`` so feedback
0 lands on the CAD upper endpoint. ``lower_at_start`` rotates it by the
source lower value so feedback 255 lands on the CAD lower endpoint.
``cad_range_center`` splits a source-vs-measured travel discrepancy equally
between the two endpoints when neither source endpoint is a trusted datum.
``zero_at_start`` keeps the CAD joint frame itself at feedback 255. All
policies publish the normalized corrected coordinate [0, measured travel].
"""
joints = {
str(joint.get("name")): joint
for joint in ET.parse(Path(source_urdf)).getroot().findall("joint")
}
offsets: dict[str, float] = {}
travels: dict[str, float] = {}
anchors = endpoint_anchor_by_joint or dict(ENDPOINT_ANCHOR_BY_JOINT)
if not anchors or not set(anchors).issubset(CALIBRATED_ACTIVE_JOINTS):
raise ValueError("L6 endpoint anchors must target measured active joints")
for name in sorted(CALIBRATED_ACTIVE_JOINTS):
if name not in curves:
raise ValueError(f"missing measured L6 curve: {name}")
joint = joints.get(name)
limit = None if joint is None else joint.find("limit")
if (
limit is None
or limit.get("upper") is None
or limit.get("lower") is None
):
raise ValueError(f"source URDF joint has incomplete limits: {name}")
travel = curve_travel_rad(curves[name])
travels[name] = travel
if name not in anchors:
continue
policy = str(anchors[name])
if policy == "upper_at_end":
offset = float(limit.get("upper")) - travel
elif policy == "lower_at_start":
offset = float(limit.get("lower"))
elif policy == "cad_range_center":
offset = 0.5 * (
float(limit.get("lower"))
+ float(limit.get("upper"))
- travel
)
elif policy == "zero_at_start":
offset = 0.0
else:
raise ValueError(f"unsupported L6 endpoint anchor: {policy}")
if not math.isfinite(offset) or abs(offset) > maximum_offset_rad:
raise ValueError(
f"{name} endpoint zero offset exceeds 15 degrees: "
f"{math.degrees(offset):.3f}"
)
offsets[name] = offset
return offsets, travels
def _has_complete_axis_geometry(
records_by_joint: Mapping[str, Sequence[Mapping[str, object]]],
) -> bool:
required = {
"relative_translation_xyz_m",
"parent_pose_common",
"child_pose_common",
"view_normal_common_xyz",
"camera_center_common_xyz_m",
"state_u8",
}
return all(
rows and all(required.issubset(row) for row in rows)
for name in L6_THUMB_AXIS_JOINTS
for rows in (records_by_joint.get(name, ()),)
)
def _fit_l6_thumb_axis_zero(
source_urdf: str | Path,
records_by_joint: Mapping[str, Sequence[Mapping[str, object]]],
curves: Mapping[str, JointCurveFit],
*,
fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None,
require_passed: bool = True,
) -> ZeroSolveResult:
"""Recover thumb roll/pitch zeros from four physical screw axes."""
profile = _l6_thumb_zero_profile()
measurements = []
by_key: dict[tuple[str, int], object] = {}
for cycle in range(4):
# Fit the two root/reference axes before the serial passive observer.
for name in (
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
"rh_pinky_mcp_pitch",
):
rows = records_by_joint[name]
view_normal = rows[0]["view_normal_common_xyz"]
measurement = fit_joint_axis_measurement(
name,
rows,
cycle=cycle,
zero_command_u8=255,
constrained_circle_joints=profile.constrained_circle_joints,
view_normal_common_xyz=view_normal,
canonical_zero_direction="decreasing",
)
measurement = with_depth_free_axis_projection(
measurement,
rows[0]["camera_center_common_xyz_m"],
)
measurements.append(measurement)
by_key[(name, cycle)] = measurement
dip_rows = records_by_joint["rh_thumb_dip"]
pitch_axis = by_key[("rh_thumb_cmc_pitch", cycle)]
dip = fit_joint_axis_measurement(
"rh_thumb_dip",
dip_rows,
cycle=cycle,
zero_command_u8=255,
axis_common_constraint=pitch_axis.axis_common_xyz,
constrained_circle_joints=profile.constrained_circle_joints,
view_normal_common_xyz=dip_rows[0]["view_normal_common_xyz"],
canonical_zero_direction="decreasing",
)
dip = with_depth_free_axis_projection(
dip,
dip_rows[0]["camera_center_common_xyz_m"],
)
measurements.append(dip)
result = solve_urdf_zero_offsets(
source_urdf=source_urdf,
measurements=measurements,
curves=curves,
motor_by_joint={
name: COMMAND_INDEX_BY_JOINT[
MIMIC_SOURCE_BY_JOINT.get(name, name)
]
for name in L6_THUMB_AXIS_JOINTS
},
training_cycles=(0, 1, 2),
validation_cycle=3,
maximum_offset_rad=math.radians(15.0),
finger_maximum_offset_rad=math.radians(15.0),
joint_maximum_offset_rad={
"rh_thumb_cmc_roll": math.radians(15.0),
"rh_thumb_cmc_pitch": math.radians(15.0),
},
maximum_cycle_difference_rad=math.radians(0.75),
minimum_applied_offset_rad=math.radians(0.1),
maximum_validation_mae_rad=math.radians(1.0),
maximum_validation_p95_rad=math.radians(2.0),
maximum_validation_error_rad=math.radians(3.0),
maximum_confidence_half_width_rad=math.radians(1.0),
maximum_pose_axis_line_rms_m=0.0015,
hand_type="right",
tag_layout=KEY.layout,
fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets_rad,
zero_profile=profile,
)
if require_passed and not result.passed:
details = ",".join(
f"{name}={reason}"
for name, reason in sorted(result.failure_reasons.items())
)
raise ValueError("L6 thumb axis zero solve failed:" + details)
return result
def _coupling_regression(
active: Sequence[float], passive: Sequence[float], *, degree: int
) -> tuple[tuple[float, ...], np.ndarray]:
x = np.asarray(active, dtype=float)
y = np.asarray(passive, dtype=float)
if x.shape != y.shape or x.ndim != 1 or x.size < 16:
raise ValueError("mimic curves must be aligned finite vectors")
if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)):
raise ValueError("mimic curves must be finite")
if degree not in {1, 2}:
raise ValueError("L6 coupling degree must be one or two")
if float(x @ x) <= 1.0e-9:
raise ValueError("active mimic source has insufficient travel")
design = np.column_stack([x ** power for power in range(1, degree + 1)])
initial = np.linalg.lstsq(design, y, rcond=None)[0]
scale = max(
math.radians(0.25),
float(np.median(np.abs(y - design @ initial))),
)
fitted = least_squares(
lambda value: y - design @ value,
np.asarray(initial, dtype=float),
loss="soft_l1",
f_scale=scale,
)
if not fitted.success or not np.all(np.isfinite(fitted.x)):
raise ValueError("robust through-origin mimic regression failed")
coefficients = tuple(float(value) for value in fitted.x)
grid = np.linspace(0.0, float(np.max(x)), 256)
derivative = np.full_like(grid, coefficients[0])
if degree == 2:
derivative += 2.0 * coefficients[1] * grid
if float(np.min(derivative)) < -1.0e-7:
raise ValueError("L6 coupling model is not monotonic")
return coefficients, y - design @ fitted.x
def fit_coupling_model(
source_joint: str,
target_joint: str,
active_fit: JointCurveFit,
passive_fit: JointCurveFit,
*,
model: str,
cycle_curve_pairs: Sequence[
tuple[Sequence[float], Sequence[float]]
] = (),
minimum_multiplier: float = 0.5,
maximum_multiplier: float = 1.5,
maximum_cycle_range: float = 0.03,
maximum_cycle_prediction_range_rad: float = math.radians(1.0),
maximum_residual_p95_rad: float = math.radians(2.0),
maximum_residual_rad: float = math.radians(3.0),
) -> MimicFit:
if model not in {"linear_mimic", "quadratic_runtime"}:
raise ValueError(f"unsupported L6 coupling model: {model}")
degree = 1 if model == "linear_mimic" else 2
active = np.concatenate(
(
np.asarray(active_fit.decreasing_rad, dtype=float),
np.asarray(active_fit.increasing_rad, dtype=float),
)
)
passive = np.concatenate(
(
np.asarray(passive_fit.decreasing_rad, dtype=float),
np.asarray(passive_fit.increasing_rad, dtype=float),
)
)
coefficients, residual = _coupling_regression(
active, passive, degree=degree
)
multiplier = coefficients[0]
if not minimum_multiplier <= multiplier <= maximum_multiplier:
raise ValueError(
f"{target_joint} coupling linear term is outside [0.5, 1.5]"
)
cycle_coefficients = tuple(
_coupling_regression(
active_cycle, passive_cycle, degree=degree
)[0]
for active_cycle, passive_cycle in cycle_curve_pairs
)
cycle_multipliers = tuple(values[0] for values in cycle_coefficients)
cycle_range = (
0.0
if len(cycle_multipliers) < 2
else float(max(cycle_multipliers) - min(cycle_multipliers))
)
if cycle_range > maximum_cycle_range:
raise ValueError(
f"{target_joint} coupling linear-term cycle range exceeds 0.03"
)
cycle_prediction_range = 0.0
if len(cycle_coefficients) >= 2:
grid = np.linspace(0.0, float(np.max(active)), 256)
predictions = np.asarray(
[
sum(value * grid ** (index + 1) for index, value in enumerate(values))
for values in cycle_coefficients
]
)
cycle_prediction_range = float(
np.max(np.ptp(predictions, axis=0))
)
if cycle_prediction_range > maximum_cycle_prediction_range_rad:
raise ValueError(
f"{target_joint} coupling cycle prediction range exceeds 1 degree"
)
absolute = np.abs(residual)
rms = float(np.sqrt(np.mean(np.square(residual))))
p95 = float(np.percentile(absolute, 95.0))
maximum = float(np.max(absolute))
if p95 > maximum_residual_p95_rad or maximum > maximum_residual_rad:
raise ValueError(
"coupling_residual_exceeds:"
f"joint={target_joint}:model={model}:"
f"linear_term={multiplier:.6f}:"
f"p95_deg={math.degrees(p95):.3f}:"
f"maximum_deg={math.degrees(maximum):.3f}:"
f"p95_limit_deg={math.degrees(maximum_residual_p95_rad):.3f}:"
f"maximum_limit_deg={math.degrees(maximum_residual_rad):.3f}"
)
return MimicFit(
source_joint=source_joint,
target_joint=target_joint,
model=model,
coefficients=coefficients,
# Standard URDF has only a linear mimic. For a nonlinear coupling,
# preserve the familiar editor/RViz linkage with a fallback line that
# is exact at both the open zero and measured closed endpoint. The
# direction-aware runtime curves and MuJoCo polynomial remain exact in
# between those endpoints.
urdf_mimic_multiplier=(
multiplier
if model == "linear_mimic"
else curve_travel_rad(passive_fit) / curve_travel_rad(active_fit)
),
urdf_mimic_policy=(
"exact_linear"
if model == "linear_mimic"
else "endpoint_linear_fallback"
),
cycle_coefficients=cycle_coefficients,
maximum_cycle_range=cycle_range,
maximum_cycle_prediction_range_rad=cycle_prediction_range,
residual_rms_rad=rms,
residual_p95_rad=p95,
residual_max_rad=maximum,
)
def fit_mimic_multiplier(
source_joint: str,
target_joint: str,
active_fit: JointCurveFit,
passive_fit: JointCurveFit,
*,
cycle_curve_pairs: Sequence[
tuple[Sequence[float], Sequence[float]]
] = (),
minimum_multiplier: float = 0.5,
maximum_multiplier: float = 1.5,
maximum_cycle_range: float = 0.03,
maximum_residual_p95_rad: float = math.radians(2.0),
maximum_residual_rad: float = math.radians(3.0),
) -> MimicFit:
return fit_coupling_model(
source_joint,
target_joint,
active_fit,
passive_fit,
model="linear_mimic",
cycle_curve_pairs=cycle_curve_pairs,
minimum_multiplier=minimum_multiplier,
maximum_multiplier=maximum_multiplier,
maximum_cycle_range=maximum_cycle_range,
maximum_residual_p95_rad=maximum_residual_p95_rad,
maximum_residual_rad=maximum_residual_rad,
)
def fit_l6_session(
source_urdf: str | Path,
records_by_joint: Mapping[str, Sequence[Mapping[str, object]]],
*,
require_thumb_axis_zero: bool = False,
) -> L6FitResult:
"""Fit three training cycles and validate the isolated fourth cycle."""
expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
if set(records_by_joint) != expected:
raise ValueError("L6 records must contain exactly five measured joints")
curves: dict[str, JointCurveFit] = {}
holdout: dict[str, tuple[float, ...]] = {}
cycle_curves: dict[str, dict[int, JointCurveFit]] = {}
for name in sorted(expected):
rows = [dict(row) for row in records_by_joint[name]]
training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}]
validation = [row for row in rows if int(row["cycle"]) == 3]
if not training or not validation:
raise ValueError(f"{name} is missing training or holdout records")
fit = fit_rotation_joint_curve(training, zero_command_u8=255)
errors = rotation_curve_holdout_errors(
fit, validation, zero_command_u8=255
)
absolute = np.abs(np.asarray(errors, dtype=float))
if (
float(np.mean(absolute)) > math.radians(1.0)
or float(np.percentile(absolute, 95.0)) > math.radians(2.0)
or float(np.max(absolute)) > math.radians(3.0)
):
raise ValueError(f"{name} isolated holdout failed")
correction_limit = math.radians(
3.0 if name in MEASURED_PASSIVE_JOINTS else 2.0
)
if fit.maximum_monotonic_correction_rad > correction_limit:
raise ValueError(f"{name} monotonic correction exceeds limit")
if fit.maximum_hysteresis_rad > math.radians(2.0):
raise ValueError(f"{name} hysteresis exceeds 2 degrees")
curves[name] = fit
holdout[name] = errors
cycle_curves[name] = {
cycle: fit_rotation_joint_curve(
[row for row in training if int(row["cycle"]) == cycle],
zero_command_u8=255,
)
for cycle in (0, 1, 2)
}
endpoint_offsets, travels = derive_endpoint_zero_offsets(
source_urdf,
curves,
endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT,
)
thumb_zero_result: ZeroSolveResult | None = None
offsets = {
"rh_thumb_cmc_roll": 0.0,
"rh_thumb_cmc_pitch": endpoint_offsets["rh_thumb_cmc_pitch"],
**endpoint_offsets,
}
pinky_endpoint_method = {
"upper_at_end": "mechanical_upper_endpoint",
"lower_at_start": "mechanical_lower_endpoint",
"cad_range_center": "cad_range_center",
"zero_at_start": "source_joint_zero",
}[ENDPOINT_ANCHOR_BY_JOINT["rh_pinky_mcp_pitch"]]
zero_methods = {
"rh_thumb_cmc_roll": "source_joint_zero_unpublished",
"rh_thumb_cmc_pitch": "cad_range_center_unpublished",
"rh_pinky_mcp_pitch": pinky_endpoint_method,
}
zero_fallback_reasons: dict[str, str] = {}
has_axis_geometry = _has_complete_axis_geometry(records_by_joint)
if require_thumb_axis_zero and not has_axis_geometry:
raise ValueError(
"L6 thumb absolute zero requires G20-compatible common-frame "
"Tag pose trajectories; this session must be reacquired"
)
if has_axis_geometry:
geometric_result = _fit_l6_thumb_axis_zero(
source_urdf,
records_by_joint,
curves,
require_passed=False,
)
thumb_zero_result = geometric_result
pitch_failure = geometric_result.failure_reasons.get(
"rh_thumb_cmc_pitch"
)
endpoint_fallback_reasons = {
"zero_offset_exceeds_configured_limit",
"zero_offset_reached_diagnostic_bound",
}
if (
not geometric_result.passed
and set(geometric_result.failure_reasons)
== {"rh_thumb_cmc_pitch"}
and pitch_failure in endpoint_fallback_reasons
):
# L6_RIGHT_001 demonstrated a stable pitch-to-DIP axis-line phase
# beyond the diagnostic search bound. That phase includes
# physical link geometry and is not a safe encoder-zero observation
# when it contradicts both the reviewed endpoint and the +/-15 deg
# write limit. Keep roll geometric, but freeze pitch to its
# independent measured/CAD range-centre datum. Any roll,
# holdout, cone, confidence, or multi-joint failure remains a hard
# rejection.
zero_fallback_reasons["rh_thumb_cmc_pitch"] = str(pitch_failure)
thumb_zero_result = _fit_l6_thumb_axis_zero(
source_urdf,
records_by_joint,
curves,
fixed_direct_zero_offsets_rad={
"rh_thumb_cmc_pitch": endpoint_offsets[
"rh_thumb_cmc_pitch"
]
},
)
elif not geometric_result.passed:
details = ",".join(
f"{name}={reason}"
for name, reason in sorted(
geometric_result.failure_reasons.items()
)
)
raise ValueError("L6 thumb axis zero solve failed:" + details)
offsets.update(
{
name: float(thumb_zero_result.direct_offsets_rad[name])
for name in (
"rh_thumb_cmc_roll",
"rh_thumb_cmc_pitch",
)
}
)
zero_methods.update(
{
"rh_thumb_cmc_roll": "urdf_serial_axis_geometry",
"rh_thumb_cmc_pitch": (
"cad_range_center_after_geometry_rejection"
if "rh_thumb_cmc_pitch" in zero_fallback_reasons
else "urdf_serial_axis_geometry"
),
}
)
mimic_fits: dict[str, MimicFit] = {}
for target in sorted(MEASURED_PASSIVE_JOINTS):
source = MIMIC_SOURCE_BY_JOINT[target]
cycle_pairs = []
for cycle in (0, 1, 2):
active = cycle_curves[source][cycle]
passive = cycle_curves[target][cycle]
cycle_pairs.append(
(
tuple(active.decreasing_rad) + tuple(active.increasing_rad),
tuple(passive.decreasing_rad) + tuple(passive.increasing_rad),
)
)
mimic_fits[target] = fit_coupling_model(
source,
target,
curves[source],
curves[target],
model=COUPLING_MODEL_BY_JOINT[target],
cycle_curve_pairs=cycle_pairs,
)
return L6FitResult(
curves=curves,
zero_offsets_rad=offsets,
travels_rad=travels,
mimic_fits=mimic_fits,
holdout_errors_rad=holdout,
zero_method_by_joint=zero_methods,
zero_fallback_reason_by_joint=zero_fallback_reasons,
thumb_zero_result=thumb_zero_result,
)
__all__ = [
"L6FitResult",
"MimicFit",
"curve_travel_rad",
"derive_endpoint_zero_offsets",
"L6_THUMB_AXIS_JOINTS",
"fit_coupling_model",
"fit_l6_session",
"fit_mimic_multiplier",
]
@@ -0,0 +1,82 @@
"""Safe six-channel motion helpers for the partial L6 profile."""
from __future__ import annotations
import math
from typing import Sequence
from ...core import CalibrationProfile, TaskSpec
def cosine_position_trajectory_u8(
start_u8: float,
target_u8: float,
elapsed_seconds: float,
full_range_duration_seconds: float,
) -> tuple[float, float, float]:
"""Return a bounded, zero-end-velocity L6 command trajectory sample."""
duration = (
float(full_range_duration_seconds)
* abs(float(target_u8) - float(start_u8))
/ 255.0
)
if full_range_duration_seconds <= 0.0:
raise ValueError("full_range_duration_seconds must be positive")
if duration <= 0.0:
return float(target_u8), 1.0, 0.0
phase = min(1.0, max(0.0, float(elapsed_seconds) / duration))
blend = 0.5 - 0.5 * math.cos(math.pi * phase)
value = float(start_u8) + (float(target_u8) - float(start_u8)) * blend
return value, phase, duration
def build_calibration_motion_command(
task: TaskSpec,
command_u8: int,
*,
profile: CalibrationProfile,
) -> list[int]:
values = list(profile.command.baseline_u8)
for index, value in task.auxiliary_commands:
values[int(index)] = int(value)
values[int(task.command_index)] = int(command_u8)
return values
def build_calibration_preparation_waypoints(
task: TaskSpec,
*,
profile: CalibrationProfile,
current_command: Sequence[int] | None = None,
) -> tuple[tuple[int, ...], ...]:
del current_command
start = build_calibration_motion_command(
task, task.start_u8, profile=profile
)
return (tuple(start),)
def build_calibration_return_waypoints(
target_command: Sequence[int] | None = None,
*,
profile: CalibrationProfile,
current_command: Sequence[int] | None = None,
**_: object,
) -> tuple[tuple[int, ...], ...]:
del current_command
target = (
tuple(int(value) for value in target_command)
if target_command is not None
else tuple(profile.command.baseline_u8)
)
if len(target) != profile.command.command_count:
raise ValueError("L6 return command has the wrong channel count")
return (target,)
__all__ = [
"build_calibration_motion_command",
"build_calibration_preparation_waypoints",
"build_calibration_return_waypoints",
"cosine_position_trajectory_u8",
]
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,267 @@
"""Shared online/offline finalization path for one L6 partial session."""
from __future__ import annotations
from datetime import datetime
import json
import math
from pathlib import Path
from typing import Any, Mapping, Sequence
from .artifacts import (
artifact_hashes,
atomic_write_json,
build_l6_runtime_payload,
publish_partial_session,
)
from .fitting import L6FitResult, fit_l6_session
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
CORRECTED_PASSIVE_JOINTS,
MEASURED_PASSIVE_JOINTS,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
)
from .urdf import L6UrdfCorrection, write_l6_corrected_urdf
MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
ENDPOINT_SNAP_TOLERANCE_U8 = 2
def canonical_feedback_command_u8(feedback_u8: float) -> int:
"""Map a reached L6 feedback endpoint onto the curve's 0/255 domain."""
value = int(round(float(feedback_u8)))
if value <= ENDPOINT_SNAP_TOLERANCE_U8:
return 0
if value >= 255 - ENDPOINT_SNAP_TOLERANCE_U8:
return 255
return value
def accepted_records_by_joint(
records: Sequence[Mapping[str, Any]],
) -> dict[str, list[dict[str, Any]]]:
"""Select the newest attempt for every task/cycle/direction."""
samples = [
dict(row)
for row in records
if row.get("kind") == "l6_joint_sample"
and str(row.get("joint", "")) in MEASURED_JOINTS
]
latest_attempt: dict[tuple[str, int, str], int] = {}
for row in samples:
key = (
str(row["task_name"]),
int(row["cycle"]),
str(row["direction"]),
)
latest_attempt[key] = max(
latest_attempt.get(key, 0), int(row.get("attempt", 1))
)
result = {name: [] for name in MEASURED_JOINTS}
for row in samples:
key = (
str(row["task_name"]),
int(row["cycle"]),
str(row["direction"]),
)
if int(row.get("attempt", 1)) != latest_attempt[key]:
continue
accepted = {
"cycle": int(row["cycle"]),
"direction": str(row["direction"]),
# The reusable fitter calls the independent variable
# command_u8; schema v6 deliberately supplies measured SDK
# feedback here, never the requested controller set-point.
"command_u8": canonical_feedback_command_u8(
float(row["feedback_u8"])
),
"feedback_u8": float(row["feedback_u8"]),
"relative_quaternion_xyzw": list(
row["relative_quaternion_xyzw"]
),
}
# Schema-v6.1 adds the G20-compatible pose trajectory required for
# absolute thumb CMC zero recovery. Keep the projection here so the
# online and offline finalizers consume byte-equivalent fitting rows.
geometric_fields = (
"relative_translation_xyz_m",
"parent_pose_common",
"child_pose_common",
"view_normal_common_xyz",
"camera_center_common_xyz_m",
"state_u8",
)
if any(field in row for field in geometric_fields):
missing = [field for field in geometric_fields if field not in row]
if missing:
raise ValueError(
"L6 geometric sample is incomplete: " + ",".join(missing)
)
for field in geometric_fields:
value = row[field]
accepted[field] = (
dict(value) if isinstance(value, Mapping) else list(value)
)
result[str(row["joint"])].append(accepted)
return result
def load_l6_raw_samples(path: str | Path) -> list[dict[str, Any]]:
source = Path(path).expanduser().resolve()
if not source.is_file():
raise ValueError(f"raw L6 sample file does not exist: {source}")
rows: list[dict[str, Any]] = []
with source.open("r", encoding="utf-8") as stream:
for line_number, line in enumerate(stream, 1):
if not line.strip():
continue
try:
value = json.loads(line)
except json.JSONDecodeError as error:
raise ValueError(
f"invalid L6 JSONL record at line {line_number}"
) from error
if not isinstance(value, Mapping):
raise ValueError(f"L6 JSONL line {line_number} is not an object")
rows.append(dict(value))
return rows
def finalize_l6_session(
*,
session_dir: str | Path,
serial_number: str,
source_urdf: str | Path,
protected_inputs: Mapping[str, str],
records: Sequence[Mapping[str, Any]],
publish: bool = True,
timestamp: str | None = None,
) -> tuple[dict[str, Any], L6FitResult, L6UrdfCorrection]:
directory = Path(session_dir).expanduser().resolve()
directory.mkdir(parents=True, exist_ok=True)
result = fit_l6_session(
source_urdf,
accepted_records_by_joint(records),
require_thumb_axis_zero=True,
)
# Validate the complete runtime schema before materializing any corrected
# URDF. A fit/schema rejection therefore leaves only the node's failure
# diagnostic and the immutable raw samples.
payload = build_l6_runtime_payload(
serial_number=serial_number,
source_urdf=source_urdf,
result=result,
protected_inputs=protected_inputs,
passed=True,
)
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
correction = write_l6_corrected_urdf(
source_urdf=source_urdf,
output_directory=directory,
serial_number=serial_number,
result=result,
timestamp=stamp,
)
json_path = directory / f"l6_right_{serial_number}_partial_calibration.json"
atomic_write_json(json_path, payload)
summary = {
"schema_version": 1,
"profile_id": "L6/right/l6_right_8/v1",
"serial_number": str(serial_number),
"result": "PARTIAL_PASS",
"publication_pointer": "latest_partial_passed",
"calibrated_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS),
"active_zero_methods": dict(sorted(result.zero_method_by_joint.items())),
"active_zero_fallback_reasons": dict(
sorted(result.zero_fallback_reason_by_joint.items())
),
"thumb_axis_zero": (
None
if result.thumb_zero_result is None
else {
"offsets_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.direct_offsets_rad.items()
)
},
"axis_line_rms_m": round(
float(result.thumb_zero_result.axis_line_rms_m), 10
),
"validation_error_by_joint_rad": {
name: round(float(value), 10)
for name, value in sorted(
result.thumb_zero_result.validation_error_by_joint_rad.items()
)
},
}
),
"measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS),
"transferred_active_joints": dict(
sorted(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items())
),
"transferred_passive_joints": dict(
sorted(TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items())
),
"passive_coupling": {
name: {
"model": fit.model,
"transferred_from_joint": (
donor if donor != name else None
),
"coefficients": [
round(float(value), 10) for value in fit.coefficients
],
"urdf_mimic_enabled": True,
"urdf_mimic_multiplier": round(
float(fit.urdf_mimic_multiplier), 10
),
"urdf_mimic_policy": fit.urdf_mimic_policy,
"residual_p95_deg": round(
math.degrees(float(fit.residual_p95_rad)), 6
),
"residual_max_deg": round(
math.degrees(float(fit.residual_max_rad)), 6
),
"cycle_prediction_range_deg": round(
math.degrees(
float(fit.maximum_cycle_prediction_range_rad)
),
6,
),
}
for name, donor in sorted(
{
target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
target, target
)
for target in CORRECTED_PASSIVE_JOINTS
}.items()
)
for fit in (result.mimic_fits[donor],)
},
"explicit_runtime_joints": sorted(
correction.explicit_runtime_joints
),
"artifacts": {
"json": json_path.name,
"urdf": correction.path.name,
**artifact_hashes(json_path, correction.path),
},
}
atomic_write_json(directory / "calibration_summary_zh.json", summary)
if publish:
publish_partial_session(directory.parent, directory)
return payload, result, correction
__all__ = [
"ENDPOINT_SNAP_TOLERANCE_U8",
"MEASURED_JOINTS",
"accepted_records_by_joint",
"canonical_feedback_command_u8",
"finalize_l6_session",
"load_l6_raw_samples",
]
@@ -0,0 +1,392 @@
"""Reviewed partial-calibration profile for the right L6 eight-Tag rig."""
from __future__ import annotations
from ...core import (
ArtifactPolicy,
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
)
from ..registry import EngineBindings, RegisteredProfile
from .motion import (
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
)
KEY = ProfileKey("L6", "right", "l6_right_8", 1)
COMMAND_NAMES: tuple[str, ...] = (
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
)
ACTIVE_JOINTS: tuple[str, ...] = (
"rh_thumb_cmc_pitch",
"rh_thumb_cmc_roll",
"rh_index_mcp_pitch",
"rh_middle_mcp_pitch",
"rh_ring_mcp_pitch",
"rh_pinky_mcp_pitch",
)
PASSIVE_JOINTS: tuple[str, ...] = (
"rh_thumb_dip",
"rh_index_dip",
"rh_middle_dip",
"rh_ring_dip",
"rh_pinky_dip",
)
CALIBRATED_ACTIVE_JOINTS = frozenset(
{
"rh_thumb_cmc_pitch",
"rh_thumb_cmc_roll",
"rh_pinky_mcp_pitch",
}
)
MEASURED_PASSIVE_JOINTS = frozenset(
{"rh_thumb_dip", "rh_pinky_dip"}
)
# The four L6 fingers use the same six-channel mechanism. This profile has
# visual Tags only on the pinky, so the remaining three fingers deliberately
# inherit the pinky's measured travel, feedback curves, and passive coupling
# while retaining their own CAD frames and geometry. The shared MCP zero is
# anchored at the observed open endpoint; it is not inferred by forcing the
# measured closed travel back onto the shorter CAD upper limit.
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = {
"rh_index_mcp_pitch": "rh_pinky_mcp_pitch",
"rh_middle_mcp_pitch": "rh_pinky_mcp_pitch",
"rh_ring_mcp_pitch": "rh_pinky_mcp_pitch",
}
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT = {
"rh_index_dip": "rh_pinky_dip",
"rh_middle_dip": "rh_pinky_dip",
"rh_ring_dip": "rh_pinky_dip",
}
CORRECTED_ACTIVE_JOINTS = frozenset(
CALIBRATED_ACTIVE_JOINTS | TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.keys()
)
CORRECTED_PASSIVE_JOINTS = frozenset(
MEASURED_PASSIVE_JOINTS | TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.keys()
)
ENDPOINT_ANCHOR_BY_JOINT = {
# Thumb pitch normally uses serial-axis geometry. If that observation is
# incompatible with the source link geometry, align the centre of the
# measured physical range with the centre of the source CAD range. Real
# endpoint comparison showed lower_at_start slightly under-corrected and
# upper_at_end over-corrected this serial, so neither endpoint is an
# independently trustworthy absolute datum.
"rh_thumb_cmc_pitch": "cad_range_center",
# Feedback 255 is the repeatable open/lower endpoint. The measured travel
# is about 70.55 deg while the source CAD upper is 65 deg. Anchoring the
# closed endpoint therefore introduced a -5.55 deg offset into all four
# fingers and made intermediate pinch poses systematically under-flexed.
"rh_pinky_mcp_pitch": "lower_at_start",
}
COMMAND_INDEX_BY_JOINT = {
"rh_thumb_cmc_pitch": 0,
"rh_thumb_cmc_roll": 1,
"rh_index_mcp_pitch": 2,
"rh_middle_mcp_pitch": 3,
"rh_ring_mcp_pitch": 4,
"rh_pinky_mcp_pitch": 5,
}
MIMIC_SOURCE_BY_JOINT = {
"rh_thumb_dip": "rh_thumb_cmc_pitch",
"rh_index_dip": "rh_index_mcp_pitch",
"rh_middle_dip": "rh_middle_mcp_pitch",
"rh_ring_dip": "rh_ring_mcp_pitch",
"rh_pinky_dip": "rh_pinky_mcp_pitch",
}
COUPLING_MODEL_BY_JOINT = {
"rh_thumb_dip": "linear_mimic",
# The L6 pinky transmission has a repeatable changing ratio over its
# travel. Keep the exact direction-aware lookup at runtime and use the
# quadratic centre relation only for MuJoCo's equality constraint.
"rh_pinky_dip": "quadratic_runtime",
"rh_index_dip": "quadratic_runtime",
"rh_middle_dip": "quadratic_runtime",
"rh_ring_dip": "quadratic_runtime",
}
def build_typed_profile() -> CalibrationProfile:
active = frozenset(ACTIVE_JOINTS)
passive = frozenset(PASSIVE_JOINTS)
frozen_active = active - CALIBRATED_ACTIVE_JOINTS
measurements = {
"rh_thumb_cmc_roll": MeasurementSpec(
"rh_thumb_cmc_roll",
"relative_rotation",
"top",
"top_base",
"thumb_roll",
),
"rh_thumb_cmc_pitch": MeasurementSpec(
"rh_thumb_cmc_pitch",
"relative_rotation",
"front",
"front_base",
"thumb_pitch",
),
"rh_thumb_dip": MeasurementSpec(
"rh_thumb_dip",
"relative_rotation",
"front",
"thumb_pitch",
"thumb_dip",
),
"rh_pinky_mcp_pitch": MeasurementSpec(
"rh_pinky_mcp_pitch",
"relative_rotation",
"side",
"side_base",
"pinky_pitch",
),
"rh_pinky_dip": MeasurementSpec(
"rh_pinky_dip",
"relative_rotation",
"side",
"pinky_pitch",
"pinky_dip",
),
}
tasks = (
TaskSpec(
"thumb_roll_top",
"top",
1,
("rh_thumb_cmc_roll",),
auxiliary_commands=((0, 255),),
preflight_speed_u8=1,
formal_speed_u8=1,
),
TaskSpec(
"thumb_pitch_dip_front",
"front",
0,
("rh_thumb_cmc_pitch", "rh_thumb_dip"),
auxiliary_commands=((1, 255),),
preflight_speed_u8=1,
formal_speed_u8=1,
),
TaskSpec(
"pinky_pitch_dip_side",
"side",
5,
("rh_pinky_mcp_pitch", "rh_pinky_dip"),
preflight_speed_u8=1,
formal_speed_u8=1,
),
)
coverage = {
**{
name: (
"measured_static_dynamic"
if name in CALIBRATED_ACTIVE_JOINTS
else "transferred_static_dynamic"
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
else "cad_nominal"
)
for name in active
},
**{
name: (
"measured_dynamic_cad_static"
if name in MEASURED_PASSIVE_JOINTS
else "transferred_dynamic_cad_static"
if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT
else "mimic_nominal"
)
for name in passive
},
}
return CalibrationProfile(
key=KEY,
namespace="/l6_calibration",
command=CommandLayout(
names=COMMAND_NAMES,
baseline_u8=(255,) * 6,
command_index_by_joint=COMMAND_INDEX_BY_JOINT,
urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS},
feedback_name_aliases={"thumb_cmc_yaw": "thumb_cmc_roll"},
speed_slot_by_command_index={index: index for index in range(6)},
),
vision=VisionRigSpec(
views=(
ViewSpec(
"front",
(
TagSpec("front_base", 0, fixed_reference=True),
TagSpec("thumb_pitch", 1),
TagSpec("thumb_dip", 2),
),
),
ViewSpec(
"side",
(
TagSpec("side_base", 3, fixed_reference=True),
TagSpec("pinky_pitch", 4),
TagSpec("pinky_dip", 5),
),
),
ViewSpec(
"top",
(
TagSpec("top_base", 6, fixed_reference=True),
TagSpec("thumb_roll", 7),
),
),
),
common_frame="calibration_common",
extrinsic_reference_view="front",
extrinsics_quality_limits={
"reprojection_rms_px": 1.2,
"maximum_rotation_repeatability_deg": 0.3,
"maximum_translation_repeatability_m": 0.0015,
},
minimum_capture_counts={
"front_side_captures": 15,
"front_top_captures": 15,
},
),
motion=MotionPolicy(
tasks=tasks,
precheck_sweeps=True,
steady_command_checkpoints=True,
speed_parameters={
"preflight_u8": 1,
"formal_u8": 1,
"speed_settle_seconds": 0.2,
"command_trajectory_full_range_seconds": 6.0,
"torque_u8": 80,
"endpoint_hold_seconds": 1.0,
"stall_timeout_seconds": 2.0,
},
),
measurement=MeasurementPolicy(
measurements=measurements,
directional_zero=True,
),
zero=ZeroSolvePolicy(
active_joints=active,
passive_joints=passive,
direct_zero_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS)),
axis_joints=tuple(
sorted(CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS)
),
mechanical_endpoint_joints=frozenset(ENDPOINT_ANCHOR_BY_JOINT),
post_solve_endpoint_joints=frozenset(),
mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT,
cad_frozen_joints=passive,
endpoint_anchor_by_joint={
name: ENDPOINT_ANCHOR_BY_JOINT[name]
for name in ENDPOINT_ANCHOR_BY_JOINT
},
fitted_mimic_joints=MEASURED_PASSIVE_JOINTS,
coupling_model_by_joint=COUPLING_MODEL_BY_JOINT,
),
quality=QualityPolicy(
training_cycles=(0, 1, 2),
holdout_cycle=3,
hard_threshold_keys=frozenset(
{
"minimum_detection_rate",
"maximum_state_image_skew_ms",
"maximum_hysteresis_rad",
"maximum_validation_error_rad",
"maximum_mimic_residual_rad",
}
),
isolated_holdout=True,
),
scope=ScopePolicy(
calibrate_joints={"partial": CALIBRATED_ACTIVE_JOINTS},
frozen_joints={"partial": frozen_active},
default_scope="partial",
),
artifacts=ArtifactPolicy(
output_schema_version=6,
calibration_filename=(
"l6_right_{serial_number}_partial_calibration.json"
),
corrected_urdf_filename=(
"linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf"
),
protected_input_fields=frozenset(
{
"source_urdf_sha256",
"camera_extrinsics_sha256",
"calibration_config_sha256",
"tag_config_sha256",
}
),
publication_pointer="latest_partial_passed",
session_compatibility_tokens=frozenset(
{"l6_partial_v1", "feedback_curves_v6"}
),
publish_corrected_urdf=True,
),
joint_coverage=coverage,
)
def _run_cli(args: list[str] | None = None) -> None:
from .runner import main
main(args)
def _run_node(args: list[str] | None = None) -> None:
from .node import main
main(args)
def build_profile() -> RegisteredProfile:
typed = build_typed_profile()
return RegisteredProfile(
profile=typed,
engine=EngineBindings(
hand_profile=typed,
zero_profile=typed.zero,
motion_command=build_calibration_motion_command,
preparation_waypoints=build_calibration_preparation_waypoints,
return_waypoints=build_calibration_return_waypoints,
cli_main=_run_cli,
node_main=_run_node,
),
)
__all__ = [
"ACTIVE_JOINTS",
"CALIBRATED_ACTIVE_JOINTS",
"COMMAND_NAMES",
"KEY",
"MEASURED_PASSIVE_JOINTS",
"MIMIC_SOURCE_BY_JOINT",
"PASSIVE_JOINTS",
"build_profile",
"build_typed_profile",
]
@@ -0,0 +1,622 @@
"""One-command online runner and deterministic offline replay for L6 right."""
from __future__ import annotations
import argparse
from datetime import datetime
import hashlib
import json
import os
from pathlib import Path
import re
import signal
import subprocess
import sys
import time
from typing import Any, Mapping
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger
from ...operator_report import (
ProgressEstimator,
render_compact_progress_header_zh,
)
from ...product import ProductConfig, load_product_config
from ...storage import atomic_write_json
from .pipeline import finalize_l6_session, load_l6_raw_samples
_STATE_LABELS = {
"WAIT_DEVICES": "等待六通道反馈和三相机内参",
"READY": "设备就绪",
"RUNNING": "标定中",
"PASSED": "通过",
"PAUSED": "已暂停",
"ABORTED": "已中止",
}
_TASK_LABELS = {
"thumb_roll_top": "拇指 CMC roll(上方机位,ID6→ID7",
"thumb_pitch_dip_front": "拇指 CMC pitch / DIP(正面机位,ID0→ID1→ID2",
"pinky_pitch_dip_side": "小指 MCP pitch / DIP(侧面机位,ID3→ID4→ID5",
}
_PHASE_LABELS = {
"baseline": "安全恢复基准形态",
"preflight": "任务运动预检",
"prepare": "扫描起点准备",
"retry_prepare": "自动重扫起点准备",
"sweep": "正式扫描",
}
_DIRECTION_LABELS = {
"decreasing": "递减",
"increasing": "递增",
}
def _progress_fraction(status: Mapping[str, Any]) -> tuple[float, int, int]:
state = str(status.get("state", ""))
step_count = max(0, int(status.get("step_count", 0) or 0))
step_index = int(status.get("step_index", -1) or 0)
step_fraction = float(status.get("step_fraction", 0.0) or 0.0)
if state == "PASSED":
overall = 1.0
elif step_count and step_index >= 0:
overall = min(1.0, max(0.0, (step_index + step_fraction) / step_count))
else:
overall = 0.0
current_step = min(step_count, max(0, step_index + 1)) if step_count else 0
return overall, current_step, step_count
def _duration_zh(seconds: float | None) -> str:
if seconds is None or seconds < 0.0:
return "计算中"
value = int(round(seconds))
return f"{value // 60}{value % 60:02d}"
def _l6_reason_zh(status: Mapping[str, Any]) -> tuple[str, str, str]:
reason = str(status.get("reason", "unknown"))
if reason.startswith("fixed_base_tag_moved:"):
fields = reason.split(":")
view = fields[1] if len(fields) > 1 else "unknown"
labels = {"front": "正面", "side": "侧面", "top": "上方"}
match = re.search(r"drift_px=([0-9.]+)", reason)
drift = match.group(1) if match else "未知"
limit = float(status.get("base_corner_drift_limit_px", 2.0) or 2.0)
return (
"OBS-BASE-DRIFT-105",
f"{labels.get(view, view)}机位的掌心固定基准 Tag 相对本方向扫描前"
f"锁定位置连续漂移,最大角点位移 {drift} px,超过 {limit:g} px。",
"检查手掌支架、相机和掌心基准 Tag 是否松动或被碰触;固定后重新开始。"
"程序已禁止发布本次结果。",
)
if reason.startswith("sweep_quality_failed:"):
fields = reason.split(":", 2)
details = fields[2] if len(fields) > 2 else "未提供明细"
return (
"OBS-SWEEP-QUALITY-104",
"当前方向经过自动重扫后仍未满足采集门限;具体未通过项:"
f"{details}",
"查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate;先处理遮挡或"
"反馈采样问题,再重新开始。程序已禁止发布本次结果。",
)
if reason.startswith("non_target_motor_moved:"):
return (
"MOTION-NONTARGET-304",
"扫描期间检测到非目标电机离开保持位置。",
"停止其他控制节点并检查机械耦合或反馈通道顺序,确认后重新开始。",
)
if reason.startswith("multiple_state_publishers:"):
count = status.get("state_publisher_count", "?")
return (
"DEVICE-DUPLICATE-SDK-203",
f"检测到 {count} 个 L6 状态发布者;这通常表示已有 SDK/GUI 未退出。",
"先停止单独启动的 linker_hand_sdk 和 GUI,只保留本标定命令自动拉起的 SDK,"
"再重新开始。程序已禁止同时控制同一只手。",
)
if reason.startswith("multiple_command_publishers:"):
count = status.get("command_publisher_count", "?")
return (
"DEVICE-COMMAND-CONFLICT-204",
f"检测到 {count} 个 L6 控制命令发布者,标定节点之外还有程序在控制手。",
"停止 GUI、手动控制节点或其他标定进程,只保留当前标定命令后重新开始。",
)
if reason.startswith("mechanical_stall:"):
return (
"MOTION-STALL-303",
"目标电机连续两秒没有向目标推进,程序已保持当前位置。",
"检查碰撞、摩擦和机械端点;不要连续重启强推。",
)
if reason.startswith("motion_timeout:"):
return (
"MOTION-TIMEOUT-302",
"当前运动在规定时间内没有到达目标位置。",
"检查 CAN 反馈、电机状态和机械阻挡后重新开始。",
)
if reason.startswith("fit_or_publication_failed:"):
if (
"mimic_residual_exceeds:" in reason
or "coupling_residual_exceeds:" in reason
):
joint_match = re.search(r"joint=([^:]+)", reason)
model_match = re.search(r"model=([^:]+)", reason)
multiplier_match = re.search(
r"(?:multiplier|linear_term)=([0-9.]+)", reason
)
p95_match = re.search(r"p95_deg=([0-9.]+)", reason)
maximum_match = re.search(r"maximum_deg=([0-9.]+)", reason)
joint = joint_match.group(1) if joint_match else "未知关节"
model = model_match.group(1) if model_match else "linear_mimic"
multiplier = multiplier_match.group(1) if multiplier_match else "未知"
p95 = p95_match.group(1) if p95_match else "未知"
maximum = maximum_match.group(1) if maximum_match else "未知"
return (
"FIT-MIMIC-503",
f"{joint}{model} 耦合模型未达到精度门限:"
f"线性项 {multiplier}"
f"残差 P95={p95}°、最大={maximum}°。",
"原始视觉曲线已保留;不要放宽门限或发布错误 URDF。请检查 Tag 刚性、"
"遮挡与机械重复性后重新采集。",
)
detail = reason.split(":", 1)[1] if ":" in reason else "未知"
return (
"FIT-PUBLISH-501",
f"采集完成后的拟合、质量验证或 URDF 安全写回失败:{detail}",
"保留本会话,不要修改源 URDF;复制下方诊断块给开发者。",
)
if reason.startswith("operator_abort"):
return "OPERATOR-ABORT-001", "操作员主动中止了本次标定。", "排除现场问题后重新开始。"
return (
"L6-CAL-500",
"L6 标定因未分类保护条件停止。",
"保留会话目录和运行日志,并复制下方诊断块给开发者。",
)
def render_l6_progress_zh(
status: Mapping[str, Any],
estimator: ProgressEstimator | None = None,
) -> str:
"""Render L6 progress in the same operator-oriented layout as G20."""
state = str(status.get("state", ""))
overall, _current_step, _step_count = _progress_fraction(status)
task = status.get("task_name")
phase = status.get("phase")
if task:
task_text = _TASK_LABELS.get(str(task), str(task))
elif phase == "baseline":
task_text = "全手基准姿态"
elif state == "WAIT_DEVICES":
task_text = "等待设备连接"
elif state == "READY":
task_text = "等待开始"
else:
task_text = ""
eta = _duration_zh(estimator.remaining(overall) if estimator else None)
cycle = status.get("cycle")
cycle_text = "-" if cycle is None else str(int(cycle) + 1)
direction = status.get("direction")
direction_text = _DIRECTION_LABELS.get(str(direction), "-")
target = status.get("target_u8")
requested = status.get("current_command_u8", target)
actual = status.get("actual_u8")
actual_text = "未知" if actual is None else f"{float(actual):.1f}"
valid = int(status.get("valid_frames", 0) or 0)
total = int(status.get("total_frames", 0) or 0)
rate = float(status.get("tag_detection_rate", 0.0) or 0.0)
joint_rate = float(
status.get(
"joint_frame_rate",
(float(valid) / total) if total else 0.0,
)
or 0.0
)
recognized = [int(value) for value in status.get("recognized_tag_ids", [])]
unrecognized = [
int(value) for value in status.get("unrecognized_tag_ids", [])
]
recognized_text = "/".join(f"ID{value}" for value in recognized) or ""
unrecognized_text = "/".join(f"ID{value}" for value in unrecognized) or ""
attempt = int(status.get("attempt", 1) or 1)
lines = render_compact_progress_header_zh(
serial_number=str(status.get("serial_number", "?")),
progress=overall,
eta=eta,
stage=_PHASE_LABELS.get(str(phase), _STATE_LABELS.get(state, state)),
cycle=cycle_text,
repetitions=4,
task=task_text,
requested=requested,
actual=actual_text,
direction=direction_text,
tag_status=(
f"已识别 {recognized_text};未识别/不合格 {unrecognized_text}"
f"本方向各Tag最低 {rate:.1%};联合 {valid}/{total}"
f"{joint_rate:.1%}"
),
ready_cameras=3,
feedback_hz=float(status.get("feedback_hz", 0.0) or 0.0),
valid_frames=valid,
automatic_retry_count=max(0, attempt - 1),
)
if state in {"PAUSED", "ABORTED"}:
_code, problem, suggestion = _l6_reason_zh(status)
lines.extend((f"原因:{problem}", f"建议:{suggestion}"))
return "\n".join(lines)
class _ProgressConsole:
def __init__(self) -> None:
self.last_text = ""
self.estimator = ProgressEstimator.start()
def update(self, status: Mapping[str, Any]) -> None:
text = render_l6_progress_zh(status, self.estimator)
if text == self.last_text:
return
self.last_text = text
if sys.stdout.isatty():
sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
sys.stdout.flush()
else:
print(text, flush=True)
class _Monitor(Node):
def __init__(self, progress: _ProgressConsole) -> None:
super().__init__("l6_calibration_runner")
self.status: dict[str, Any] = {}
self.progress = progress
self.create_subscription(
String, "/l6_calibration/status", self._status_callback, 10
)
self.start_client = self.create_client(Trigger, "/l6_calibration/start")
self.abort_client = self.create_client(Trigger, "/l6_calibration/abort")
def _status_callback(self, message: String) -> None:
try:
value = json.loads(message.data)
except json.JSONDecodeError:
return
if isinstance(value, dict):
self.status = value
self.progress.update(value)
def _launch_command(
config: ProductConfig,
session: Path,
*,
record_bag: bool,
commands_enabled: bool,
) -> list[str]:
arguments = {
"model": config.model,
"hand_type": config.side,
"tag_layout": config.tag_layout,
"serial_number": config.serial_number,
"can_interface": config.can_interface,
"source_urdf_path": str(config.source_urdf),
"source_urdf_expected_sha256": config.source_urdf_sha256,
"camera_extrinsics_file": str(config.camera_extrinsics),
"camera_extrinsics_expected_sha256": config.camera_extrinsics_sha256,
"calibration_config": str(config.calibration_config),
"calibration_config_expected_sha256": config.calibration_config_sha256,
"tag_config": str(config.tag_config),
"tag_config_expected_sha256": config.tag_config_sha256,
"output_root": str(config.output_root),
"session_dir": str(session),
"corrected_urdf_output_dir": str(session),
"recalibration_scope": "partial",
"calibration_speed": "1",
"index_roll_calibration_speed": "1",
"index_flex_calibration_speed": "1",
"commands_enabled": str(commands_enabled).lower(),
"record_bag": str(record_bag).lower(),
}
for view, camera in config.cameras.items():
arguments[f"{view}_camera_serial"] = camera["serial_number"]
arguments[f"{view}_camera_name"] = camera["camera_name"]
arguments[f"{view}_camera_info_url"] = camera["camera_info"]
return [
"ros2", "launch", "linkerhand_calibration",
"three_camera_calibration.launch.py",
*(f"{name}:={value}" for name, value in arguments.items()),
]
def _wait_until(
monitor: _Monitor,
process: subprocess.Popen[Any],
predicate,
*,
timeout: float | None,
) -> bool:
started = time.monotonic()
while rclpy.ok():
if process.poll() is not None:
return False
rclpy.spin_once(monitor, timeout_sec=0.2)
if predicate(monitor.status):
return True
if timeout is not None and time.monotonic() - started > timeout:
return False
return False
def _stop_stack(process: subprocess.Popen[Any]) -> None:
"""Stop the whole launch process group without leaking child-node noise."""
if process.poll() is not None:
return
try:
os.killpg(process.pid, signal.SIGINT)
except ProcessLookupError:
return
try:
process.wait(timeout=15.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGTERM)
except ProcessLookupError:
return
try:
process.wait(timeout=5.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGKILL)
except ProcessLookupError:
return
process.wait(timeout=5.0)
def _l6_failure_report(
config: ProductConfig,
session: Path,
status: Mapping[str, Any],
log_path: Path,
) -> str:
code, problem, suggestion = _l6_reason_zh(status)
task_key = status.get("task_name")
task = "" if task_key is None else _TASK_LABELS.get(str(task_key), str(task_key))
metrics = {
"task": task,
"cycle": status.get("cycle"),
"direction": status.get("direction"),
"valid_frames": status.get("valid_frames", 0),
"detection_frames": status.get("total_frames", 0),
"detection_rate": status.get("tag_detection_rate", 0.0),
"detection_rate_by_role": status.get("tag_detection_rate_by_role", {}),
"tag_seen_rate_by_role": status.get("tag_seen_rate_by_role", {}),
"recognized_tag_ids": status.get("recognized_tag_ids", []),
"unrecognized_tag_ids": status.get("unrecognized_tag_ids", []),
"joint_frame_rate": status.get("joint_frame_rate", 0.0),
"all_tags_quality_rate": status.get("all_tags_quality_rate", 0.0),
"pnp_valid_rate": status.get("pnp_valid_rate", 0.0),
"state_sync_rate": status.get("state_sync_rate", 0.0),
"observation_rejection_counts": status.get(
"observation_rejection_counts", {}
),
"actual_u8": status.get("actual_u8"),
"current_command_u8": status.get("current_command_u8"),
"base_corner_drift_px": status.get("base_corner_drift_px", {}),
"state_publisher_count": status.get("state_publisher_count"),
"command_publisher_count": status.get("command_publisher_count"),
"failure_reason": status.get("reason"),
}
payload = {
"schema_version": 1,
"profile_id": status.get("profile_id"),
"serial_number": config.serial_number,
"result": "FAIL",
"error_code": code,
"stage": status.get("phase")
or ("fit" if str(status.get("reason", "")).startswith(
"fit_or_publication_failed:"
) else status.get("state", "unknown")),
"reason": status.get("reason", "unknown"),
"problem_zh": problem,
"automatic_action_zh": "已停止运动、保持当前位置并禁止发布标定 JSON/URDF",
"suggestion_zh": suggestion,
"metrics": metrics,
"feedback_hz": status.get("feedback_hz", 0.0),
"hashes": {
"product_config_sha256": hashlib.sha256(
config.path.read_bytes()
).hexdigest(),
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"source_urdf_sha256": config.source_urdf_sha256,
},
"session_dir": str(session),
"log_path": str(log_path),
"quality": {"passed": False},
}
atomic_write_json(session / "calibration_summary_zh.json", payload)
return "\n".join(
[
"========== 请复制以下内容给开发者 ==========",
f"会话编号:{config.serial_number}_{session.name}",
"结果:FAIL",
f"错误代码:{code}",
f"失败阶段:{payload['stage']}",
f"问题:{problem}",
f"自动处理:{payload['automatic_action_zh']}",
"关键指标:"
+ json.dumps(metrics, ensure_ascii=False, separators=(",", ":")),
f"反馈状态:{float(payload['feedback_hz'] or 0.0):.1f} Hz",
f"配置哈希:{payload['hashes']['product_config_sha256']}",
f"外参哈希:{config.camera_extrinsics_sha256}",
f"源 URDF 哈希:{config.source_urdf_sha256}",
f"会话目录:{session}",
f"运行日志:{log_path}",
f"建议:{suggestion}",
"========== 复制结束 ==========",
]
)
def _run_online(
config: ProductConfig,
*,
record_bag: bool,
commands_enabled: bool,
) -> int:
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
while session.exists():
time.sleep(1.0)
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
session.mkdir(parents=True)
command = _launch_command(
config, session, record_bag=record_bag, commands_enabled=commands_enabled
)
log_path = session / "calibration.log"
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
process = subprocess.Popen(
command,
cwd=config.workspace,
stdout=log_stream,
stderr=subprocess.STDOUT,
text=True,
start_new_session=True,
)
rclpy.init()
monitor = _Monitor(_ProgressConsole())
try:
ready = _wait_until(
monitor,
process,
lambda status: status.get("state")
in {"READY", "PAUSED", "ABORTED"},
timeout=120.0,
)
if ready and monitor.status.get("state") in {"PAUSED", "ABORTED"}:
print(
_l6_failure_report(config, session, monitor.status, log_path),
flush=True,
)
return 3
if not ready:
print("L6 启动失败:120秒内未收到六通道反馈和三相机内参。", flush=True)
return 2
if not monitor.start_client.wait_for_service(timeout_sec=10.0):
print("L6 标定 /start 服务不可用。", flush=True)
return 2
future = monitor.start_client.call_async(Trigger.Request())
while rclpy.ok() and not future.done():
rclpy.spin_once(monitor, timeout_sec=0.2)
response = future.result()
if response is None or not response.success:
print(f"L6 标定未启动:{getattr(response, 'message', '')}", flush=True)
return 2
print(
"L6右手标定已启动:通道0/1/5,预检/正式速度1,"
"全行程6秒余弦缓入缓出,torque 80。",
flush=True,
)
finished = _wait_until(
monitor,
process,
lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"},
timeout=None,
)
if not finished:
print("L6 标定进程意外退出。", flush=True)
return 2
status = monitor.status
if status.get("state") != "PASSED":
print(_l6_failure_report(config, session, status, log_path), flush=True)
return 3
print(
"\n".join(
[
"PASS:L6右手三主动关节与两条DIP实测通过;"
"小指结果已迁移到食指、中指和无名指。",
f"部分结果:{config.session_root / 'latest_partial_passed'}",
f"JSON{status.get('final_json')}",
f"URDF{status.get('final_urdf')}",
]
),
flush=True,
)
return 0
except KeyboardInterrupt:
if monitor.abort_client.wait_for_service(timeout_sec=2.0):
monitor.abort_client.call_async(Trigger.Request())
rclpy.spin_once(monitor, timeout_sec=1.0)
return 130
finally:
monitor.destroy_node()
if rclpy.ok():
rclpy.shutdown()
_stop_stack(process)
log_stream.flush()
os.fsync(log_stream.fileno())
log_stream.close()
def main(args: list[str] | None = None) -> None:
parser = argparse.ArgumentParser(
description="L6 right partial three-camera calibration"
)
parser.add_argument("--config", required=True)
parser.add_argument("--workspace", default=None)
parser.add_argument("--record-bag", action="store_true")
parser.add_argument("--commands-disabled", action="store_true")
parser.add_argument("--validate-only", action="store_true")
parser.add_argument("--offline-raw", default="")
parser.add_argument("--offline-output", default="")
parser.add_argument("--publish-offline", action="store_true")
selected = parser.parse_args(args)
config = load_product_config(
selected.config,
workspace=selected.workspace,
check_can=not bool(selected.validate_only or selected.offline_raw),
)
if selected.validate_only:
print(
f"配置有效:{config.profile_key.profile_id},源URDF "
f"{config.source_urdf_sha256}",
flush=True,
)
return
if selected.offline_raw:
output = (
Path(selected.offline_output).expanduser().resolve()
if selected.offline_output
else config.session_root
/ (datetime.now().strftime("%Y%m%d_%H%M%S") + "_offline")
)
output.mkdir(parents=True, exist_ok=False)
payload, _fit, correction = finalize_l6_session(
session_dir=output,
serial_number=config.serial_number,
source_urdf=config.source_urdf,
protected_inputs={
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"tag_config_sha256": config.tag_config_sha256,
},
records=load_l6_raw_samples(selected.offline_raw),
publish=selected.publish_offline,
)
print(
f"离线回放PASSschema {payload['schema_version']}URDF {correction.path}",
flush=True,
)
return
raise SystemExit(
_run_online(
config,
record_bag=selected.record_bag,
commands_enabled=not selected.commands_disabled,
)
)
__all__ = ["main", "render_l6_progress_zh"]
@@ -0,0 +1,342 @@
"""Minimal, auditable URDF correction for the partial L6 profile."""
from __future__ import annotations
from dataclasses import dataclass
from datetime import datetime
import math
import os
from pathlib import Path
import re
import shutil
from typing import Mapping
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation
from ..g20.zero_solver import _materialize_relative_mesh_assets
from .fitting import L6FitResult
from .profile import (
CALIBRATED_ACTIVE_JOINTS,
CORRECTED_ACTIVE_JOINTS,
CORRECTED_PASSIVE_JOINTS,
MEASURED_PASSIVE_JOINTS,
MIMIC_SOURCE_BY_JOINT,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
)
@dataclass(frozen=True)
class L6UrdfCorrection:
path: Path
origin_offsets_rad: Mapping[str, float]
corrected_limits_rad: Mapping[str, tuple[float, float]]
mimic_multipliers: Mapping[str, float]
coupling_polycoef: Mapping[str, tuple[float, ...]]
explicit_runtime_joints: frozenset[str]
def _triplet(value: str) -> np.ndarray:
result = np.asarray([float(item) for item in str(value).split()], dtype=float)
if result.shape != (3,) or not np.all(np.isfinite(result)):
raise ValueError(f"invalid URDF vector: {value}")
return result
def _replace_attribute(
block: str, element: str, attribute: str, value: str
) -> str:
pattern = re.compile(
rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])"
rf"(?P<value>[^\"']*)\2",
re.DOTALL,
)
match = pattern.search(block)
if match is None:
raise ValueError(f"{element} has no {attribute} attribute")
start, end = match.span("value")
return block[:start] + value + block[end:]
def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str:
origin = joint.find("origin")
if origin is None or origin.get("rpy") is None:
raise ValueError(f"joint {joint.get('name')} has no origin.rpy")
# Preserve the reviewed CAD spelling exactly when the calibrated open
# endpoint is the source joint zero. Apart from avoiding Euler round-off,
# this makes it explicit that a zero correction must not rotate the frame.
if abs(float(offset)) <= 1.0e-12:
return str(origin.get("rpy"))
axis_node = joint.find("axis")
axis = _triplet(
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
)
norm = float(np.linalg.norm(axis))
if norm <= 1.0e-12:
raise ValueError(f"joint {joint.get('name')} has a degenerate axis")
source = Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0")))
corrected = source * Rotation.from_rotvec(axis / norm * float(offset))
return " ".join(
f"{float(value):.15g}" for value in corrected.as_euler("xyz")
)
def _validate_passive_ranges(
joints: Mapping[str, ET.Element], result: L6FitResult
) -> None:
for target in sorted(CORRECTED_PASSIVE_JOINTS):
source = MIMIC_SOURCE_BY_JOINT[target]
measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(
source, source
)
measured_target = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
target, target
)
joint = joints[target]
mimic = joint.find("mimic")
limit = joint.find("limit")
if mimic is None or limit is None:
raise ValueError(f"passive joint is missing mimic or limit: {target}")
if mimic.get("joint") != source:
raise ValueError(f"passive source differs from profile: {target}")
offset = float(mimic.get("offset", "0"))
coupling = result.mimic_fits[measured_target]
travel = float(result.travels_rad[measured_source])
grid = np.linspace(0.0, travel, 256)
predicted = np.full_like(grid, offset)
for power, coefficient in enumerate(coupling.coefficients, 1):
predicted += float(coefficient) * grid ** power
lower = float(limit.get("lower", "-inf"))
upper = float(limit.get("upper", "inf"))
if (
float(np.min(predicted)) < lower - 1.0e-8
or float(np.max(predicted)) > upper + 1.0e-8
):
raise ValueError(
f"{target} fitted coupling range exceeds preserved passive limit"
)
measured = result.curves[measured_target]
observed = np.concatenate(
(
np.asarray(measured.decreasing_rad, dtype=float),
np.asarray(measured.increasing_rad, dtype=float),
)
) + offset
if float(np.min(observed)) < lower - 1.0e-8 or float(
np.max(observed)
) > upper + 1.0e-8:
raise ValueError(
f"{target} measured curve exceeds preserved passive limit"
)
def write_l6_corrected_urdf(
*,
source_urdf: str | Path,
output_directory: str | Path,
serial_number: str,
result: L6FitResult,
timestamp: str | None = None,
) -> L6UrdfCorrection:
source = Path(source_urdf).expanduser().resolve()
output = Path(output_directory).expanduser().resolve()
if not source.is_file():
raise ValueError(f"source URDF does not exist: {source}")
if "calibrated" in source.stem.lower():
raise ValueError("source URDF must be the immutable original CAD file")
if set(result.zero_offsets_rad) != CALIBRATED_ACTIVE_JOINTS:
raise ValueError("L6 zero result has the wrong active joint set")
if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS:
raise ValueError("L6 travel result has the wrong active joint set")
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
raise ValueError("L6 mimic result has the wrong passive joint set")
tree = ET.parse(source)
root = tree.getroot()
joints = {
str(joint.get("name")): joint for joint in root.findall("joint")
}
required = CORRECTED_ACTIVE_JOINTS | CORRECTED_PASSIVE_JOINTS
missing = required - set(joints)
if missing:
raise ValueError("source URDF is missing L6 targets: " + ",".join(sorted(missing)))
_validate_passive_ranges(joints, result)
active_replacements: dict[str, tuple[str, str, str]] = {}
for name in sorted(CORRECTED_ACTIVE_JOINTS):
measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
offset = float(result.zero_offsets_rad[measured_source])
travel = float(result.travels_rad[measured_source])
if not math.isfinite(offset) or abs(offset) > math.radians(15.0):
raise ValueError(f"invalid L6 zero offset: {name}")
if not math.isfinite(travel) or not 0.0 < travel < math.pi:
raise ValueError(f"invalid L6 travel: {name}")
active_replacements[name] = (
_corrected_origin_rpy(joints[name], offset),
"0",
f"{travel:.15g}",
)
mimic_replacements = {
name: f"{float(result.mimic_fits[donor].urdf_mimic_multiplier):.15g}"
for name, donor in sorted(
{
target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
target, target
)
for target in CORRECTED_PASSIVE_JOINTS
}.items()
)
}
explicit_runtime_joints = frozenset(
name for name in CORRECTED_PASSIVE_JOINTS
if result.mimic_fits[
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
].model == "quadratic_runtime"
)
coupling_polycoef = {
name: tuple(
float(value)
for value in result.mimic_fits[
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
].mujoco_polycoef
)
for name in sorted(CORRECTED_PASSIVE_JOINTS)
}
equality_replacements: dict[str, str] = {}
for equality in root.findall("./mujoco/equality/joint"):
target = str(equality.get("joint1", ""))
source_name = str(equality.get("joint2", ""))
if target not in coupling_polycoef:
continue
if MIMIC_SOURCE_BY_JOINT[target] != source_name:
raise ValueError(f"MuJoCo equality source differs for {target}")
coefficients = [
float(value) for value in str(equality.get("polycoef", "")).split()
]
if len(coefficients) != 6:
raise ValueError(f"MuJoCo equality polycoef is invalid for {target}")
offset = float(joints[target].find("mimic").get("offset", "0"))
coefficients = list(coupling_polycoef[target])
coefficients[0] = offset
equality_name = str(equality.get("name", ""))
if not equality_name:
raise ValueError(f"MuJoCo equality has no name for {target}")
equality_replacements[equality_name] = " ".join(
f"{value:.15g}" for value in coefficients
)
if len(equality_replacements) != len(coupling_polycoef):
raise ValueError("source URDF lacks a MuJoCo equality for fitted coupling")
original = source.read_text(encoding="utf-8")
# The file also contains MuJoCo/transmission ``<joint>`` elements. Requiring
# a URDF ``type`` attribute keeps the surgical block replacement confined
# to the eleven kinematic joints.
joint_pattern = re.compile(
r"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
r".*?</joint>",
re.DOTALL,
)
def replace_joint(match: re.Match[str]) -> str:
name = match.group("name")
block = match.group(0)
if name in active_replacements:
rpy, lower, upper = active_replacements[name]
block = _replace_attribute(block, "origin", "rpy", rpy)
block = _replace_attribute(block, "limit", "lower", lower)
block = _replace_attribute(block, "limit", "upper", upper)
if name in mimic_replacements:
block = _replace_attribute(
block, "mimic", "multiplier", mimic_replacements[name]
)
return block
corrected = joint_pattern.sub(replace_joint, original)
for equality_name, polycoef in equality_replacements.items():
equality_pattern = re.compile(
rf"(<joint\b[^>]*\bname\s*=\s*([\"'])"
rf"{re.escape(equality_name)}\2[^>]*>)",
re.DOTALL,
)
match = equality_pattern.search(corrected)
if match is None:
raise ValueError(f"could not locate MuJoCo equality {equality_name}")
replacement = _replace_attribute(
match.group(0), "joint", "polycoef", polycoef
)
corrected = corrected[: match.start()] + replacement + corrected[match.end() :]
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
if re.fullmatch(r"\d{8}_\d{6}", stamp) is None:
raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS")
safe_serial = "".join(
char if char.isalnum() or char in "_.-" else "_"
for char in str(serial_number)
)
if not safe_serial:
raise ValueError("serial number must not be empty")
output.mkdir(parents=True, exist_ok=True)
destination = output / (
f"{source.stem}_partial_zero_calibrated_{safe_serial}_{stamp}.urdf"
)
if destination.exists() or destination == source:
raise ValueError(f"refusing to overwrite URDF: {destination}")
_materialize_relative_mesh_assets(source=source, output=output, urdf_root=root)
# Keep the complete vendor mesh bundle beside the generated URDF, including
# auxiliary meshes not referenced by this particular XML revision.
source_meshes = source.parent / "meshes"
if source_meshes.is_dir():
destination_meshes = output / "meshes"
destination_meshes.mkdir(parents=True, exist_ok=True)
for mesh in source_meshes.iterdir():
if mesh.is_file():
shutil.copy2(mesh, destination_meshes / mesh.name)
temporary = destination.with_suffix(".urdf.tmp")
try:
with temporary.open("w", encoding="utf-8") as stream:
stream.write(corrected)
stream.flush()
os.fsync(stream.fileno())
os.replace(temporary, destination)
finally:
if temporary.exists():
temporary.unlink()
return L6UrdfCorrection(
path=destination,
origin_offsets_rad={
name: float(
result.zero_offsets_rad[
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
]
)
for name in sorted(CORRECTED_ACTIVE_JOINTS)
},
corrected_limits_rad={
name: (
0.0,
float(
result.travels_rad[
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
]
),
)
for name in sorted(CORRECTED_ACTIVE_JOINTS)
},
mimic_multipliers={
name: float(
result.mimic_fits[
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
].urdf_mimic_multiplier
)
for name in sorted(mimic_replacements)
},
coupling_polycoef=coupling_polycoef,
explicit_runtime_joints=explicit_runtime_joints,
)
__all__ = ["L6UrdfCorrection", "write_l6_corrected_urdf"]
@@ -77,8 +77,10 @@ def get_default_registry() -> ProfileRegistry:
global _DEFAULT_REGISTRY
if _DEFAULT_REGISTRY is None:
from .g20 import register_profiles
from .l6 import register_profiles as register_l6_profiles
registry = ProfileRegistry()
register_profiles(registry)
register_l6_profiles(registry)
_DEFAULT_REGISTRY = registry
return _DEFAULT_REGISTRY
@@ -21,6 +21,37 @@ def _duration(seconds: float | None) -> str:
return f"{value // 60}{value % 60:02d}"
def render_compact_progress_header_zh(
*,
serial_number: str,
progress: float,
eta: str,
stage: str,
cycle: object,
repetitions: object,
task: str,
requested: object,
actual: object,
direction: str,
tag_status: str,
ready_cameras: int,
feedback_hz: float,
valid_frames: int,
automatic_retry_count: int,
) -> list[str]:
"""Render the model-independent five-line operator progress header."""
return [
f"[{serial_number}] 标定中 {float(progress) * 100:5.1f}% "
f"预计剩余 {eta}",
f"阶段:{stage}(第 {cycle}/{repetitions} 轮)",
f"任务:{task} 命令/反馈:{requested}/{actual} 方向:{direction}",
f"Tag{tag_status} 相机:{int(ready_cameras)}/3 正常 "
f"反馈:{float(feedback_hz):.1f} Hz",
f"质量:有效帧 {int(valid_frames)} 已自动重扫 "
f"{int(automatic_retry_count)}",
]
def _task_tag_id_status(views: Mapping[str, Any]) -> str:
"""Render every currently required Tag ID with its live visibility."""
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
@@ -273,13 +304,23 @@ def render_progress_zh(
== "waiting_for_task_tags_at_sweep_start"
else _duration(estimator.remaining(progress))
)
lines = [
f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}",
f"阶段:{stage}(第 {cycle}/{repetitions} 轮)",
f"任务:{joint} 命令/反馈:{requested}/{actual} 方向:{direction}",
f"Tag{tag_status}{task_tag_ids} 相机:{ready_cameras}/3 正常 反馈:{feedback_hz:.1f} Hz",
f"质量:有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry}",
]
lines = render_compact_progress_header_zh(
serial_number=serial_number,
progress=progress,
eta=eta,
stage=stage,
cycle=cycle,
repetitions=repetitions,
task=joint,
requested=requested,
actual=actual,
direction=direction,
tag_status=tag_status + task_tag_ids,
ready_cameras=ready_cameras,
feedback_hz=feedback_hz,
valid_frames=int(active.get("valid_frames", 0)),
automatic_retry_count=retry,
)
if str(status.get("reason", "")) == "waiting_for_task_tags_at_sweep_start":
pnp_wait = _pnp_wait_status(views)
if pnp_wait:
@@ -27,13 +27,19 @@ def build_session_controller(
def main(args: list[str] | None = None) -> None:
"""Select the profile first, then delegate to its reviewed CLI strategy."""
selector = argparse.ArgumentParser(add_help=False)
selector.add_argument(
"--config", default=str(default_product_config_path())
)
# Resolve the installed default lazily. A caller that supplies --config
# must also work directly from a source workspace before the package has
# been installed.
selector.add_argument("--config", default=None)
selector.add_argument("--workspace", default=None)
selected, _ = selector.parse_known_args(args)
config_path = (
str(default_product_config_path())
if selected.config is None
else selected.config
)
product = load_product_config(
selected.config,
config_path,
workspace=selected.workspace,
check_can=False,
)
@@ -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:
registry = get_default_registry()
assert len(registry) == 3
assert len(registry) == 4
for registered in registry:
validate_profile(registered.profile)
assert registered.profile.zero.active_joints
@@ -0,0 +1,828 @@
from __future__ import annotations
from pathlib import Path
import re
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,
default_input_topic,
)
from linkerhand_calibration.core import validate_profile
from linkerhand_calibration.models.l6.artifacts import (
build_l6_runtime_payload,
validate_l6_runtime_payload,
)
from linkerhand_calibration.models.l6.fitting import fit_l6_session
from linkerhand_calibration.models.l6.motion import cosine_position_trajectory_u8
from linkerhand_calibration.models.l6.node import (
L6ThreeCameraCalibrationNode,
MotionStep,
)
from linkerhand_calibration.models.l6.pipeline import (
accepted_records_by_joint,
canonical_feedback_command_u8,
finalize_l6_session,
)
from linkerhand_calibration.models.l6.profile import (
ACTIVE_JOINTS,
CALIBRATED_ACTIVE_JOINTS,
COMMAND_NAMES,
ENDPOINT_ANCHOR_BY_JOINT,
KEY,
PASSIVE_JOINTS,
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
build_typed_profile,
)
from linkerhand_calibration.models.l6.runner import render_l6_progress_zh
from linkerhand_calibration.models.l6.urdf import write_l6_corrected_urdf
from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix
from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel
from linkerhand_calibration.product import load_product_config, sha256_file
PACKAGE = Path(__file__).resolve().parents[1]
SOURCE = PACKAGE / "urdf/l6_right/linkerhand_l6v3.1_right.urdf"
PRODUCT = PACKAGE / "config/l6_right_product.yaml"
TRAVELS = {
"rh_thumb_cmc_roll": 1.34,
"rh_thumb_cmc_pitch": 0.78,
"rh_pinky_mcp_pitch": 1.10,
}
MULTIPLIERS = {"rh_thumb_dip": 1.20, "rh_pinky_dip": 1.10}
THUMB_ZERO_OFFSETS = {
"rh_thumb_cmc_roll": 0.04,
"rh_thumb_cmc_pitch": -0.05,
}
KINEMATIC_MODEL = UrdfKinematicModel(SOURCE)
def _pose(xyz: list[float], rpy: list[float]) -> np.ndarray:
return transform_matrix(xyz, Rotation.from_euler("xyz", rpy).as_quat())
def _geometric_observation(
joint: str,
feedback: int,
*,
thumb_zero_offsets: dict[str, float] | None = None,
) -> dict[str, object]:
model = KINEMATIC_MODEL
state = [255.0] * 6
motor = {
"rh_thumb_cmc_pitch": 0,
"rh_thumb_dip": 0,
"rh_thumb_cmc_roll": 1,
"rh_pinky_mcp_pitch": 5,
"rh_pinky_dip": 5,
}[joint]
state[motor] = float(feedback)
active_angles = {
name: travel * (255.0 - state[index]) / 255.0
for name, travel, index in (
("rh_thumb_cmc_pitch", TRAVELS["rh_thumb_cmc_pitch"], 0),
("rh_thumb_cmc_roll", TRAVELS["rh_thumb_cmc_roll"], 1),
("rh_pinky_mcp_pitch", TRAVELS["rh_pinky_mcp_pitch"], 5),
)
}
joint_angles = {
**active_angles,
"rh_thumb_dip": active_angles["rh_thumb_cmc_pitch"]
* MULTIPLIERS["rh_thumb_dip"],
"rh_pinky_dip": active_angles["rh_pinky_mcp_pitch"]
* MULTIPLIERS["rh_pinky_dip"],
}
zero_offsets = {
**(
THUMB_ZERO_OFFSETS
if thumb_zero_offsets is None
else thumb_zero_offsets
),
"rh_pinky_mcp_pitch": 0.0,
}
base_common = _pose([0.11, -0.04, 0.72], [0.11, -0.08, 0.17])
base_mounts = {
"front": _pose([0.0, 0.008, 0.055], [0.2, -0.1, 0.3]),
"side": _pose([-0.01, 0.002, 0.045], [-0.1, 0.2, -0.2]),
"top": _pose([0.012, -0.006, 0.05], [0.1, 0.3, 0.15]),
}
link_mounts = {
"rh_thumb_cmc_roll": _pose([0.012, 0.019, 0.006], [0.2, 0.1, -0.1]),
"rh_thumb_cmc_pitch": _pose([0.009, -0.028, 0.011], [-0.2, 0.1, 0.25]),
"rh_thumb_dip": _pose([0.006, -0.025, 0.018], [0.15, -0.1, 0.2]),
"rh_pinky_mcp_pitch": _pose([0.006, 0.004, 0.021], [-0.1, 0.2, 0.1]),
"rh_pinky_dip": _pose([0.004, 0.003, 0.019], [0.12, 0.08, -0.2]),
}
view = {
"rh_thumb_cmc_roll": "top",
"rh_thumb_cmc_pitch": "front",
"rh_thumb_dip": "front",
"rh_pinky_mcp_pitch": "side",
"rh_pinky_dip": "side",
}[joint]
if joint in {"rh_thumb_dip", "rh_pinky_dip"}:
parent_joint = {
"rh_thumb_dip": "rh_thumb_cmc_pitch",
"rh_pinky_dip": "rh_pinky_mcp_pitch",
}[joint]
parent = base_common @ model.link_transform(
parent_joint,
zero_offsets=zero_offsets,
joint_angles=joint_angles,
independent_mimic_angles=True,
) @ link_mounts[parent_joint]
else:
parent = base_common @ base_mounts[view]
child = base_common @ model.link_transform(
joint,
zero_offsets=zero_offsets,
joint_angles=joint_angles,
independent_mimic_angles=True,
) @ link_mounts[joint]
relative = np.linalg.inv(parent) @ child
return {
"state_u8": state,
"relative_translation_xyz_m": relative[:3, 3].tolist(),
"relative_quaternion_xyzw": Rotation.from_matrix(
relative[:3, :3]
).as_quat().tolist(),
"parent_pose_common": matrix_payload(parent),
"child_pose_common": matrix_payload(child),
"view_normal_common_xyz": [0.577350269, 0.577350269, 0.577350269],
"camera_center_common_xyz_m": [0.0, 0.0, -0.5],
}
def _synthetic_records(
*, thumb_zero_offsets: dict[str, float] | None = None
) -> list[dict]:
result = []
task_by_joint = {
"rh_thumb_cmc_roll": "thumb_roll_top",
"rh_thumb_cmc_pitch": "thumb_pitch_dip_front",
"rh_thumb_dip": "thumb_pitch_dip_front",
"rh_pinky_mcp_pitch": "pinky_pitch_dip_side",
"rh_pinky_dip": "pinky_pitch_dip_side",
}
source_by_passive = {
"rh_thumb_dip": "rh_thumb_cmc_pitch",
"rh_pinky_dip": "rh_pinky_mcp_pitch",
}
for name, task in task_by_joint.items():
travel = (
TRAVELS[name]
if name in TRAVELS
else TRAVELS[source_by_passive[name]] * MULTIPLIERS[name]
)
for cycle in range(4):
for direction, commands in (
("decreasing", range(255, -1, -4)),
("increasing", range(0, 256, 4)),
):
values = list(commands)
if values[-1] not in {0, 255}:
values.append(0 if direction == "decreasing" else 255)
for feedback in values:
angle = travel * (255 - feedback) / 255.0
geometry = _geometric_observation(
name,
feedback,
thumb_zero_offsets=thumb_zero_offsets,
)
result.append(
{
"kind": "l6_joint_sample",
"task_name": task,
"joint": name,
"cycle": cycle,
"direction": direction,
"attempt": 1,
"feedback_u8": float(feedback),
**geometry,
}
)
return result
def _synthetic_nonlinear_pinky_records() -> list[dict]:
records = _synthetic_records()
mounted = Rotation.from_euler("xyz", [0.2, -0.1, 0.3])
for row in records:
if row["joint"] != "rh_pinky_dip":
continue
active = TRAVELS["rh_pinky_mcp_pitch"] * (
255.0 - float(row["feedback_u8"])
) / 255.0
angle = 1.2 * active - 0.2 * active * active
observed = mounted * Rotation.from_rotvec([angle, 0.0, 0.0])
row["relative_quaternion_xyzw"] = observed.as_quat().tolist()
return records
def _joint_block(text: str, name: str) -> str:
pattern = re.compile(
rf"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname=\"{re.escape(name)}\""
rf"[^>]*>.*?</joint>",
re.DOTALL,
)
match = pattern.search(text)
assert match is not None
return match.group(0)
def test_l6_profile_declares_six_channels_eight_tags_and_partial_scope() -> None:
profile = build_typed_profile()
validate_profile(profile)
assert profile.key == KEY
assert profile.namespace == "/l6_calibration"
assert profile.command.names == COMMAND_NAMES
assert profile.command.baseline_u8 == (255,) * 6
assert profile.command.feedback_name_aliases == {
"thumb_cmc_yaw": "thumb_cmc_roll"
}
assert profile.motion.speed_parameters["preflight_u8"] == 1
assert profile.motion.speed_parameters["formal_u8"] == 1
assert profile.motion.speed_parameters["speed_settle_seconds"] == 0.2
assert (
profile.motion.speed_parameters["command_trajectory_full_range_seconds"]
== 6.0
)
assert all(task.preflight_speed_u8 == 1 for task in profile.motion.tasks)
assert all(task.formal_speed_u8 == 1 for task in profile.motion.tasks)
assert profile.vision.tag_ids == frozenset(range(8))
assert profile.scope.selected_joints("partial") == CALIBRATED_ACTIVE_JOINTS
assert len(profile.joint_coverage) == 11
assert profile.artifacts.publication_pointer == "latest_partial_passed"
assert profile.zero.coupling_model_by_joint == {
"rh_thumb_dip": "linear_mimic",
"rh_pinky_dip": "quadratic_runtime",
"rh_index_dip": "quadratic_runtime",
"rh_middle_dip": "quadratic_runtime",
"rh_ring_dip": "quadratic_runtime",
}
assert profile.zero.endpoint_anchor_by_joint == ENDPOINT_ANCHOR_BY_JOINT
def test_l6_operator_progress_names_current_task_round_and_direction() -> None:
text = render_l6_progress_zh(
{
"state": "RUNNING",
"serial_number": "L6_TEST",
"step_index": 42,
"step_count": 61,
"step_fraction": 0.5,
"task_name": "pinky_pitch_dip_side",
"phase": "sweep",
"cycle": 3,
"direction": "decreasing",
"attempt": 2,
"target_u8": 0,
"current_command_u8": 130.0,
"actual_u8": 127.5,
"valid_frames": 50,
"total_frames": 52,
"tag_detection_rate": 50 / 52,
"joint_frame_rate": 50 / 52,
"recognized_tag_ids": [3, 4],
"unrecognized_tag_ids": [5],
"feedback_hz": 29.8,
"formal_speed_u8": 1,
}
)
assert "[L6_TEST] 标定中" in text
assert "小指 MCP pitch / DIP(侧面机位,ID3→ID4→ID5" in text
assert "阶段:正式扫描(第 4/4 轮)" in text
assert "方向:递减" in text
assert "已识别 ID3/ID4" in text
assert "未识别/不合格 ID5" in text
assert "已自动重扫 1 次" in text
assert "步骤" not in text
def test_l6_six_second_cosine_trajectory_is_monotonic_and_smooth() -> None:
start, start_phase, duration = cosine_position_trajectory_u8(
255.0, 0.0, 0.0, 6.0
)
middle, middle_phase, _ = cosine_position_trajectory_u8(
255.0, 0.0, 3.0, 6.0
)
end, end_phase, _ = cosine_position_trajectory_u8(
255.0, 0.0, 6.0, 6.0
)
assert (start, start_phase, duration) == pytest.approx((255.0, 0.0, 6.0))
assert (middle, middle_phase) == pytest.approx((127.5, 0.5))
assert (end, end_phase) == pytest.approx((0.0, 1.0))
commands = [
round(cosine_position_trajectory_u8(255.0, 0.0, tick / 100.0, 6.0)[0])
for tick in range(601)
]
assert all(right <= left for left, right in zip(commands, commands[1:]))
assert max(abs(right - left) for left, right in zip(commands, commands[1:])) <= 1
assert cosine_position_trajectory_u8(255.0, 127.5, 1.5, 6.0)[2] == pytest.approx(3.0)
def test_l6_node_streams_the_six_second_trajectory_without_command_jumps() -> None:
published: list[list[int]] = []
fake = SimpleNamespace(
step_started_at=0.0,
step_start_state_u8=(255.0,) * 6,
command_trajectory_full_range_seconds=6.0,
step_last_command_u8=None,
step_trajectory_phase=0.0,
step_requested_u8=255.0,
_publish_command=lambda values: published.append(values),
)
step = MotionStep("sweep", "thumb_pitch_dip_front", 0, 0, 1, 0, "decreasing")
for tick in range(601):
L6ThreeCameraCalibrationNode._advance_step_trajectory(
fake, step, tick / 100.0
)
channel = [command[0] for command in published]
assert channel[0] == 255
assert channel[-1] == 0
assert max(abs(right - left) for left, right in zip(channel, channel[1:])) <= 1
assert all(command[1:] == [255] * 5 for command in published)
def _l6_sweep_quality_fake(tmp_path: Path) -> tuple[SimpleNamespace, MotionStep]:
task_key = "pinky_pitch_dip_side"
feedback = np.linspace(255.0, 0.0, 188)
fake = SimpleNamespace(
raw_records=[
{
"task_name": task_key,
"cycle": 3,
"direction": "decreasing",
"attempt": 3,
"joint": "rh_pinky_mcp_pitch",
"feedback_u8": float(value),
}
for value in feedback
],
raw_path=tmp_path / "raw_samples.jsonl",
step_required_roles=("side_base", "pinky_pitch", "pinky_dip"),
step_total_frames=211,
step_tag_seen_frames={
"side_base": 211,
"pinky_pitch": 210,
"pinky_dip": 208,
},
# Every Tag independently clears 95%, while the fully joined frames
# match the real 20260901_181435 sweep at about 89.1%.
step_tag_quality_frames={
"side_base": 209,
"pinky_pitch": 205,
"pinky_dip": 202,
},
step_all_tags_quality_frames=190,
step_pnp_valid_frames=189,
step_state_sync_frames=188,
step_valid_frames=188,
step_rejection_counts={},
minimum_sweep_frames=40,
minimum_state_span_u8=240.0,
minimum_sweep_bins=32,
maximum_bin_gap=16,
minimum_detection_rate=0.95,
minimum_joint_frame_rate=0.85,
minimum_feedback_hz=25.0,
profile=build_typed_profile(),
_task=lambda _key: SimpleNamespace(
key=task_key,
joints=("rh_pinky_mcp_pitch", "rh_pinky_dip"),
),
_feedback_hz=lambda: 58.9,
)
fake._step_observation_metrics = lambda: (
L6ThreeCameraCalibrationNode._step_observation_metrics(fake)
)
step = MotionStep(
"sweep", task_key, 5, 0, 1, 3, "decreasing", attempt=3
)
return fake, step
def test_l6_quality_gates_each_tag_separately_from_joined_frames(
tmp_path: Path,
) -> None:
fake, step = _l6_sweep_quality_fake(tmp_path)
metrics = L6ThreeCameraCalibrationNode._step_observation_metrics(fake)
assert metrics["tag_detection_rate"] == pytest.approx(202 / 211)
assert metrics["joint_frame_rate"] == pytest.approx(188 / 211)
# This is a valid sweep: every individual Tag is >=95%, the joined frame
# rate is >=85%, and all existing trajectory coverage gates still pass.
L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step)
def test_l6_quality_failure_names_the_specific_tag_id(tmp_path: Path) -> None:
fake, step = _l6_sweep_quality_fake(tmp_path)
fake.step_tag_quality_frames["pinky_dip"] = 190
with pytest.raises(ValueError, match=r"tag_rate\[ID5/pinky_dip\]=0\.900"):
L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step)
def test_l6_operator_progress_shows_exact_failed_sweep_metric() -> None:
text = render_l6_progress_zh(
{
"state": "PAUSED",
"serial_number": "L6_TEST",
"reason": "sweep_quality_failed:thumb_roll_top:maximum_gap=22",
"phase": "sweep",
}
)
assert "具体未通过项:maximum_gap=22" in text
def test_l6_operator_progress_explains_nonlinear_mimic_failure() -> None:
text = render_l6_progress_zh(
{
"state": "PAUSED",
"serial_number": "L6_TEST",
"reason": (
"fit_or_publication_failed:mimic_residual_exceeds:"
"joint=rh_pinky_dip:multiplier=1.008793:"
"p95_deg=4.715:maximum_deg=6.476"
),
}
)
assert "rh_pinky_dip 的 linear_mimic 耦合模型未达到精度门限" in text
assert "P95=4.715°、最大=6.476°" in text
def test_l6_product_config_and_immutable_source_hash_are_valid() -> None:
product = load_product_config(PRODUCT, workspace=PACKAGE.parents[1], check_can=False)
assert product.profile_key == KEY
assert product.source_urdf == SOURCE.resolve()
assert sha256_file(SOURCE) == (
"298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666"
)
assert product.camera_extrinsics_sha256 == (
"dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80"
)
def test_l6_synthetic_fit_recovers_travel_zero_and_mimic() -> None:
records = accepted_records_by_joint(_synthetic_records())
result = fit_l6_session(SOURCE, records)
for name, expected in TRAVELS.items():
assert result.travels_rad[name] == pytest.approx(expected, abs=2.0e-4)
source_limits = {
joint.get("name"): {
field: float(joint.find("limit").get(field))
for field in ("lower", "upper")
}
for joint in ET.parse(SOURCE).getroot().findall("joint")
if joint.find("limit") is not None
}
for name, expected_offset in THUMB_ZERO_OFFSETS.items():
assert result.zero_offsets_rad[name] == pytest.approx(
expected_offset, abs=2.0e-4
)
assert result.zero_method_by_joint[name] == (
"urdf_serial_axis_geometry"
)
assert result.zero_fallback_reason_by_joint == {}
assert result.zero_offsets_rad["rh_pinky_mcp_pitch"] == pytest.approx(
source_limits["rh_pinky_mcp_pitch"]["lower"],
abs=2.0e-4,
)
assert result.zero_method_by_joint["rh_pinky_mcp_pitch"] == (
"mechanical_lower_endpoint"
)
for name, expected in MULTIPLIERS.items():
assert result.mimic_fits[name].multiplier == pytest.approx(expected, abs=2.0e-4)
assert result.mimic_fits[name].maximum_cycle_range < 1.0e-6
def test_l6_pitch_geometry_bound_centres_measured_range_in_cad_range() -> None:
records = accepted_records_by_joint(
_synthetic_records(
thumb_zero_offsets={
"rh_thumb_cmc_roll": THUMB_ZERO_OFFSETS[
"rh_thumb_cmc_roll"
],
"rh_thumb_cmc_pitch": 1.2,
}
)
)
result = fit_l6_session(SOURCE, records)
source_pitch = next(
joint
for joint in ET.parse(SOURCE).getroot().findall("joint")
if joint.get("name") == "rh_thumb_cmc_pitch"
)
expected = 0.5 * (
float(source_pitch.find("limit").get("lower"))
+ float(source_pitch.find("limit").get("upper"))
- TRAVELS["rh_thumb_cmc_pitch"]
)
assert result.zero_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx(
expected, abs=2.0e-4
)
assert result.zero_method_by_joint["rh_thumb_cmc_pitch"] == (
"cad_range_center_after_geometry_rejection"
)
assert result.zero_fallback_reason_by_joint == {
"rh_thumb_cmc_pitch": "zero_offset_reached_diagnostic_bound"
}
# A pitch-only fallback must not replace the independently observed roll.
assert result.zero_offsets_rad["rh_thumb_cmc_roll"] == pytest.approx(
THUMB_ZERO_OFFSETS["rh_thumb_cmc_roll"], abs=2.0e-4
)
def test_l6_fit_recovers_stable_nonlinear_pinky_coupling(
tmp_path: Path,
) -> None:
result = fit_l6_session(
SOURCE, accepted_records_by_joint(_synthetic_nonlinear_pinky_records())
)
coupling = result.mimic_fits["rh_pinky_dip"]
assert coupling.model == "quadratic_runtime"
assert coupling.coefficients == pytest.approx((1.2, -0.2), abs=2.0e-4)
assert coupling.maximum_cycle_prediction_range_rad < 1.0e-6
assert coupling.residual_p95_rad < 2.0e-5
correction = write_l6_corrected_urdf(
source_urdf=SOURCE,
output_directory=tmp_path,
serial_number="L6_NONLINEAR_TEST",
result=result,
timestamp="20260901_120002",
)
root = ET.parse(correction.path).getroot()
pinky = next(
joint
for joint in root.findall("joint")
if joint.get("name") == "rh_pinky_dip"
)
pinky_mimic = pinky.find("mimic")
assert pinky_mimic is not None
assert float(pinky_mimic.get("multiplier")) == pytest.approx(
(1.2 * TRAVELS["rh_pinky_mcp_pitch"]
- 0.2 * TRAVELS["rh_pinky_mcp_pitch"] ** 2)
/ TRAVELS["rh_pinky_mcp_pitch"],
abs=2.0e-4,
)
equality = next(
joint
for joint in root.findall("./mujoco/equality/joint")
if joint.get("joint1") == "rh_pinky_dip"
)
assert [float(value) for value in equality.get("polycoef").split()] == (
pytest.approx([0.0, 1.2, -0.2, 0.0, 0.0, 0.0], abs=2.0e-4)
)
for finger in ("index", "middle", "ring"):
transferred = next(
joint
for joint in root.findall("./mujoco/equality/joint")
if joint.get("joint1") == f"rh_{finger}_dip"
)
assert [
float(value) for value in transferred.get("polycoef").split()
] == pytest.approx(
[0.0, 1.2, -0.2, 0.0, 0.0, 0.0], abs=2.0e-4
)
def test_l6_feedback_endpoint_deadband_is_canonicalized_for_fitting() -> None:
assert [canonical_feedback_command_u8(value) for value in (0, 1, 2)] == [0] * 3
assert canonical_feedback_command_u8(3) == 3
assert canonical_feedback_command_u8(252) == 252
assert [canonical_feedback_command_u8(value) for value in (253, 254, 255)] == [255] * 3
records = _synthetic_records()
for row in records:
if row["feedback_u8"] == 0.0:
row["feedback_u8"] = 1.0
elif row["feedback_u8"] == 255.0:
row["feedback_u8"] = 254.0
result = fit_l6_session(SOURCE, accepted_records_by_joint(records))
for name, expected in TRAVELS.items():
assert result.travels_rad[name] == pytest.approx(expected, abs=2.0e-4)
def test_l6_urdf_writer_changes_only_authorized_joint_fields(tmp_path: Path) -> None:
result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records()))
correction = write_l6_corrected_urdf(
source_urdf=SOURCE,
output_directory=tmp_path,
serial_number="L6_TEST",
result=result,
timestamp="20260901_120000",
)
original = SOURCE.read_text(encoding="utf-8")
corrected = correction.path.read_text(encoding="utf-8")
original_root = ET.parse(SOURCE).getroot()
original_joints = {
joint.get("name"): joint for joint in original_root.findall("joint")
}
root = ET.parse(correction.path).getroot()
joints = {joint.get("name"): joint for joint in root.findall("joint")}
# The already reviewed DIP geometry and limits stay byte-equivalent; only
# the measured mimic multiplier and matching MuJoCo equality may change.
for passive_name in PASSIVE_JOINTS:
passive = joints[passive_name]
source_passive = original_joints[passive_name]
for element in ("origin", "axis", "limit", "parent", "child"):
assert passive.find(element).attrib == source_passive.find(element).attrib
assert passive.find("mimic").get("offset") == (
source_passive.find("mimic").get("offset")
)
for finger in ("index", "middle", "ring"):
active_name = f"rh_{finger}_mcp_pitch"
passive_name = f"rh_{finger}_dip"
active = joints[active_name]
source_active = original_joints[active_name]
assert active.find("origin").get("xyz") == source_active.find("origin").get("xyz")
assert active.find("origin").get("rpy") == source_active.find("origin").get("rpy")
assert active.find("axis").attrib == source_active.find("axis").attrib
assert active.find("parent").attrib == source_active.find("parent").attrib
assert active.find("child").attrib == source_active.find("child").attrib
assert float(active.find("limit").get("lower")) == 0.0
assert float(active.find("limit").get("upper")) == pytest.approx(
TRAVELS["rh_pinky_mcp_pitch"], abs=2.0e-4
)
for field in ("effort", "velocity"):
assert active.find("limit").get(field) == source_active.find("limit").get(field)
passive = joints[passive_name]
source_passive = original_joints[passive_name]
for element in ("origin", "axis", "limit", "parent", "child"):
assert passive.find(element).attrib == source_passive.find(element).attrib
assert passive.find("mimic").get("joint") == active_name
assert float(passive.find("mimic").get("multiplier")) == pytest.approx(
MULTIPLIERS["rh_pinky_dip"], abs=2.0e-4
)
assert correction.origin_offsets_rad[active_name] == pytest.approx(
correction.origin_offsets_rad["rh_pinky_mcp_pitch"]
)
assert correction.origin_offsets_rad[active_name] == pytest.approx(0.0)
pinky = joints["rh_pinky_mcp_pitch"]
source_pinky = original_joints["rh_pinky_mcp_pitch"]
assert pinky.find("origin").get("rpy") == source_pinky.find("origin").get("rpy")
assert correction.origin_offsets_rad["rh_pinky_mcp_pitch"] == pytest.approx(0.0)
assert result.zero_method_by_joint["rh_pinky_mcp_pitch"] == (
"mechanical_lower_endpoint"
)
roll = joints["rh_thumb_cmc_roll"]
assert float(roll.find("limit").get("lower")) == 0.0
assert float(roll.find("limit").get("upper")) == pytest.approx(1.34, abs=2.0e-4)
assert correction.origin_offsets_rad["rh_thumb_cmc_roll"] == pytest.approx(
THUMB_ZERO_OFFSETS["rh_thumb_cmc_roll"], abs=2.0e-4
)
assert correction.origin_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx(
THUMB_ZERO_OFFSETS["rh_thumb_cmc_pitch"], abs=2.0e-4
)
assert roll.find("origin").get("xyz") == "0.0078133 0.030812 0.025678"
assert float(
joints["rh_thumb_dip"].find("mimic").get("multiplier")
) == pytest.approx(MULTIPLIERS["rh_thumb_dip"])
assert joints["rh_pinky_dip"].find("mimic") is not None
assert correction.explicit_runtime_joints == frozenset(
{"rh_index_dip", "rh_middle_dip", "rh_ring_dip", "rh_pinky_dip"}
)
equalities = {
joint.get("joint1"): joint
for joint in root.findall("./mujoco/equality/joint")
}
assert float(
equalities["rh_thumb_dip"].get("polycoef").split()[1]
) == pytest.approx(MULTIPLIERS["rh_thumb_dip"])
pinky_polycoef = [
float(value)
for value in equalities["rh_pinky_dip"].get("polycoef").split()
]
assert pinky_polycoef == pytest.approx(
[0.0, MULTIPLIERS["rh_pinky_dip"], 0.0, 0.0, 0.0, 0.0],
abs=2.0e-4,
)
assert len(list((tmp_path / "meshes").iterdir())) == 13
assert sha256_file(SOURCE) == (
"298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666"
)
def test_l6_schema_v6_bridge_uses_feedback_and_rh_joint_names() -> None:
result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records()))
hashes = {
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": "1" * 64,
"calibration_config_sha256": "2" * 64,
"tag_config_sha256": "3" * 64,
}
payload = build_l6_runtime_payload(
serial_number="L6_TEST",
source_urdf=SOURCE,
result=result,
protected_inputs=hashes,
)
validate_l6_runtime_payload(payload)
assert set(payload["joints"]) == set(ACTIVE_JOINTS + PASSIVE_JOINTS)
assert payload["joints"]["rh_thumb_dip"]["urdf_mimic_enabled"] is True
assert payload["joints"]["rh_pinky_dip"]["urdf_mimic_enabled"] is True
assert payload["joints"]["rh_pinky_dip"]["coupling_model"] == (
"quadratic_runtime"
)
assert payload["joints"]["rh_pinky_dip"]["urdf_mimic_policy"] == (
"endpoint_linear_fallback"
)
assert payload["joints"]["rh_pinky_dip"]["mimic_multiplier"] == (
pytest.approx(MULTIPLIERS["rh_pinky_dip"], abs=2.0e-4)
)
for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items():
assert payload["joints"][target]["calibration_status"] == (
"transferred_static_dynamic"
)
assert payload["joints"][target]["transferred_from_joint"] == donor
assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"]
for target, donor in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items():
assert payload["joints"][target]["calibration_status"] == (
"transferred_dynamic_cad_static"
)
assert payload["joints"][target]["transferred_from_joint"] == donor
assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"]
mapper = CalibratedCommandMapper(payload, expected_side="right")
assert mapper.profile_id == KEY.profile_id
assert mapper.input_domain == "feedback_u8"
mapped = dict(zip(mapper.urdf_joint_names, mapper.map_positions([0] * 6)))
assert mapped["rh_thumb_cmc_roll"] == pytest.approx(TRAVELS["rh_thumb_cmc_roll"], abs=2.0e-4)
# Accept one-release feedback from an older SDK that mislabeled channel 2.
names = list(COMMAND_NAMES)
names[1] = "thumb_cmc_yaw"
assert mapper.map_positions([255] * 6, names) == mapper.map_positions([255] * 6)
assert default_input_topic("right", "feedback_u8", "L6") == (
"/l6/cb_right_hand_state"
)
def test_l6_online_and_offline_finalization_are_identical(tmp_path: Path) -> None:
records = _synthetic_records()
hashes = {
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": "1" * 64,
"calibration_config_sha256": "2" * 64,
"tag_config_sha256": "3" * 64,
}
online = tmp_path / "online"
offline = tmp_path / "offline"
first, _, first_urdf = finalize_l6_session(
session_dir=online,
serial_number="L6_TEST",
source_urdf=SOURCE,
protected_inputs=hashes,
records=records,
publish=False,
timestamp="20260901_120001",
)
second, _, second_urdf = finalize_l6_session(
session_dir=offline,
serial_number="L6_TEST",
source_urdf=SOURCE,
protected_inputs=hashes,
records=records,
publish=False,
timestamp="20260901_120001",
)
assert first == second
assert first_urdf.path.read_bytes() == second_urdf.path.read_bytes()
def test_l6_legacy_relative_only_session_cannot_publish_thumb_zero(
tmp_path: Path,
) -> None:
geometric_fields = {
"relative_translation_xyz_m",
"parent_pose_common",
"child_pose_common",
"view_normal_common_xyz",
"camera_center_common_xyz_m",
"state_u8",
}
records = [
{key: value for key, value in row.items() if key not in geometric_fields}
for row in _synthetic_records()
]
with pytest.raises(ValueError, match="session must be reacquired"):
finalize_l6_session(
session_dir=tmp_path / "legacy",
serial_number="L6_LEGACY",
source_urdf=SOURCE,
protected_inputs={
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": "1" * 64,
"calibration_config_sha256": "2" * 64,
"tag_config_sha256": "3" * 64,
},
records=records,
publish=False,
timestamp="20260901_120003",
)