L6右手标定
This commit is contained in:
@@ -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,
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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",
|
||||
]
|
||||
+36
-2
@@ -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
|
||||
|
||||
+1
-1
@@ -372,7 +372,7 @@ class LinkerHandL6RS485:
|
||||
return [0] * 6
|
||||
|
||||
def get_finger_order(self):
|
||||
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
|
||||
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
|
||||
|
||||
# --------------------------------------------------
|
||||
# 便捷方法
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
import ast
|
||||
from collections import deque
|
||||
from pathlib import Path
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core"
|
||||
EXPECTED = [
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
]
|
||||
|
||||
|
||||
def _finger_order(path: Path, class_name: str) -> list[str]:
|
||||
module = ast.parse(path.read_text(encoding="utf-8"))
|
||||
selected = next(
|
||||
item
|
||||
for item in module.body
|
||||
if isinstance(item, ast.ClassDef) and item.name == class_name
|
||||
)
|
||||
method = next(
|
||||
item
|
||||
for item in selected.body
|
||||
if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order"
|
||||
)
|
||||
returned = next(item for item in method.body if isinstance(item, ast.Return))
|
||||
return ast.literal_eval(returned.value)
|
||||
|
||||
|
||||
def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None:
|
||||
assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED
|
||||
assert _finger_order(
|
||||
PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485"
|
||||
) == EXPECTED
|
||||
|
||||
|
||||
def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
|
||||
linker_hand_root = PACKAGE.parent
|
||||
sys.path.insert(0, str(linker_hand_root))
|
||||
try:
|
||||
from core.can.linker_hand_l6_can import LinkerHandL6Can
|
||||
finally:
|
||||
sys.path.remove(str(linker_hand_root))
|
||||
|
||||
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
|
||||
hand.can_id = 0x27
|
||||
hand.x01 = [10, 20, 30, 40, 50, 60]
|
||||
hand._position_echo_lock = threading.Lock()
|
||||
command = (255, 2, 253, 253, 253, 253)
|
||||
hand._pending_position_echoes = deque(
|
||||
[(time.monotonic(), command)], maxlen=32
|
||||
)
|
||||
hand._position_echo_timeout_seconds = 0.02
|
||||
|
||||
hand.process_response(
|
||||
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)))
|
||||
)
|
||||
assert hand.x01 == [10, 20, 30, 40, 50, 60]
|
||||
assert not hand._pending_position_echoes
|
||||
|
||||
measured = (250, 3, 252, 252, 252, 252)
|
||||
hand.process_response(
|
||||
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)))
|
||||
)
|
||||
assert hand.x01 == list(measured)
|
||||
@@ -1,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 (
|
||||
|
||||
+60
-27
@@ -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"离线回放PASS:schema {payload['schema_version']},URDF {correction.path}",
|
||||
flush=True,
|
||||
)
|
||||
return
|
||||
raise SystemExit(
|
||||
_run_online(
|
||||
config,
|
||||
record_bag=selected.record_bag,
|
||||
commands_enabled=not selected.commands_disabled,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["main", "render_l6_progress_zh"]
|
||||
@@ -0,0 +1,342 @@
|
||||
"""Minimal, auditable URDF correction for the partial L6 profile."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import shutil
|
||||
from typing import Mapping
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from ..g20.zero_solver import _materialize_relative_mesh_assets
|
||||
from .fitting import L6FitResult
|
||||
from .profile import (
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
CORRECTED_ACTIVE_JOINTS,
|
||||
CORRECTED_PASSIVE_JOINTS,
|
||||
MEASURED_PASSIVE_JOINTS,
|
||||
MIMIC_SOURCE_BY_JOINT,
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class L6UrdfCorrection:
|
||||
path: Path
|
||||
origin_offsets_rad: Mapping[str, float]
|
||||
corrected_limits_rad: Mapping[str, tuple[float, float]]
|
||||
mimic_multipliers: Mapping[str, float]
|
||||
coupling_polycoef: Mapping[str, tuple[float, ...]]
|
||||
explicit_runtime_joints: frozenset[str]
|
||||
|
||||
|
||||
def _triplet(value: str) -> np.ndarray:
|
||||
result = np.asarray([float(item) for item in str(value).split()], dtype=float)
|
||||
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
||||
raise ValueError(f"invalid URDF vector: {value}")
|
||||
return result
|
||||
|
||||
|
||||
def _replace_attribute(
|
||||
block: str, element: str, attribute: str, value: str
|
||||
) -> str:
|
||||
pattern = re.compile(
|
||||
rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])"
|
||||
rf"(?P<value>[^\"']*)\2",
|
||||
re.DOTALL,
|
||||
)
|
||||
match = pattern.search(block)
|
||||
if match is None:
|
||||
raise ValueError(f"{element} has no {attribute} attribute")
|
||||
start, end = match.span("value")
|
||||
return block[:start] + value + block[end:]
|
||||
|
||||
|
||||
def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str:
|
||||
origin = joint.find("origin")
|
||||
if origin is None or origin.get("rpy") is None:
|
||||
raise ValueError(f"joint {joint.get('name')} has no origin.rpy")
|
||||
# Preserve the reviewed CAD spelling exactly when the calibrated open
|
||||
# endpoint is the source joint zero. Apart from avoiding Euler round-off,
|
||||
# this makes it explicit that a zero correction must not rotate the frame.
|
||||
if abs(float(offset)) <= 1.0e-12:
|
||||
return str(origin.get("rpy"))
|
||||
axis_node = joint.find("axis")
|
||||
axis = _triplet(
|
||||
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
|
||||
)
|
||||
norm = float(np.linalg.norm(axis))
|
||||
if norm <= 1.0e-12:
|
||||
raise ValueError(f"joint {joint.get('name')} has a degenerate axis")
|
||||
source = Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0")))
|
||||
corrected = source * Rotation.from_rotvec(axis / norm * float(offset))
|
||||
return " ".join(
|
||||
f"{float(value):.15g}" for value in corrected.as_euler("xyz")
|
||||
)
|
||||
|
||||
|
||||
def _validate_passive_ranges(
|
||||
joints: Mapping[str, ET.Element], result: L6FitResult
|
||||
) -> None:
|
||||
for target in sorted(CORRECTED_PASSIVE_JOINTS):
|
||||
source = MIMIC_SOURCE_BY_JOINT[target]
|
||||
measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(
|
||||
source, source
|
||||
)
|
||||
measured_target = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
|
||||
target, target
|
||||
)
|
||||
joint = joints[target]
|
||||
mimic = joint.find("mimic")
|
||||
limit = joint.find("limit")
|
||||
if mimic is None or limit is None:
|
||||
raise ValueError(f"passive joint is missing mimic or limit: {target}")
|
||||
if mimic.get("joint") != source:
|
||||
raise ValueError(f"passive source differs from profile: {target}")
|
||||
offset = float(mimic.get("offset", "0"))
|
||||
coupling = result.mimic_fits[measured_target]
|
||||
travel = float(result.travels_rad[measured_source])
|
||||
grid = np.linspace(0.0, travel, 256)
|
||||
predicted = np.full_like(grid, offset)
|
||||
for power, coefficient in enumerate(coupling.coefficients, 1):
|
||||
predicted += float(coefficient) * grid ** power
|
||||
lower = float(limit.get("lower", "-inf"))
|
||||
upper = float(limit.get("upper", "inf"))
|
||||
if (
|
||||
float(np.min(predicted)) < lower - 1.0e-8
|
||||
or float(np.max(predicted)) > upper + 1.0e-8
|
||||
):
|
||||
raise ValueError(
|
||||
f"{target} fitted coupling range exceeds preserved passive limit"
|
||||
)
|
||||
measured = result.curves[measured_target]
|
||||
observed = np.concatenate(
|
||||
(
|
||||
np.asarray(measured.decreasing_rad, dtype=float),
|
||||
np.asarray(measured.increasing_rad, dtype=float),
|
||||
)
|
||||
) + offset
|
||||
if float(np.min(observed)) < lower - 1.0e-8 or float(
|
||||
np.max(observed)
|
||||
) > upper + 1.0e-8:
|
||||
raise ValueError(
|
||||
f"{target} measured curve exceeds preserved passive limit"
|
||||
)
|
||||
|
||||
|
||||
def write_l6_corrected_urdf(
|
||||
*,
|
||||
source_urdf: str | Path,
|
||||
output_directory: str | Path,
|
||||
serial_number: str,
|
||||
result: L6FitResult,
|
||||
timestamp: str | None = None,
|
||||
) -> L6UrdfCorrection:
|
||||
source = Path(source_urdf).expanduser().resolve()
|
||||
output = Path(output_directory).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"source URDF does not exist: {source}")
|
||||
if "calibrated" in source.stem.lower():
|
||||
raise ValueError("source URDF must be the immutable original CAD file")
|
||||
if set(result.zero_offsets_rad) != CALIBRATED_ACTIVE_JOINTS:
|
||||
raise ValueError("L6 zero result has the wrong active joint set")
|
||||
if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS:
|
||||
raise ValueError("L6 travel result has the wrong active joint set")
|
||||
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
|
||||
raise ValueError("L6 mimic result has the wrong passive joint set")
|
||||
|
||||
tree = ET.parse(source)
|
||||
root = tree.getroot()
|
||||
joints = {
|
||||
str(joint.get("name")): joint for joint in root.findall("joint")
|
||||
}
|
||||
required = CORRECTED_ACTIVE_JOINTS | CORRECTED_PASSIVE_JOINTS
|
||||
missing = required - set(joints)
|
||||
if missing:
|
||||
raise ValueError("source URDF is missing L6 targets: " + ",".join(sorted(missing)))
|
||||
_validate_passive_ranges(joints, result)
|
||||
|
||||
active_replacements: dict[str, tuple[str, str, str]] = {}
|
||||
for name in sorted(CORRECTED_ACTIVE_JOINTS):
|
||||
measured_source = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
offset = float(result.zero_offsets_rad[measured_source])
|
||||
travel = float(result.travels_rad[measured_source])
|
||||
if not math.isfinite(offset) or abs(offset) > math.radians(15.0):
|
||||
raise ValueError(f"invalid L6 zero offset: {name}")
|
||||
if not math.isfinite(travel) or not 0.0 < travel < math.pi:
|
||||
raise ValueError(f"invalid L6 travel: {name}")
|
||||
active_replacements[name] = (
|
||||
_corrected_origin_rpy(joints[name], offset),
|
||||
"0",
|
||||
f"{travel:.15g}",
|
||||
)
|
||||
|
||||
mimic_replacements = {
|
||||
name: f"{float(result.mimic_fits[donor].urdf_mimic_multiplier):.15g}"
|
||||
for name, donor in sorted(
|
||||
{
|
||||
target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(
|
||||
target, target
|
||||
)
|
||||
for target in CORRECTED_PASSIVE_JOINTS
|
||||
}.items()
|
||||
)
|
||||
}
|
||||
explicit_runtime_joints = frozenset(
|
||||
name for name in CORRECTED_PASSIVE_JOINTS
|
||||
if result.mimic_fits[
|
||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
].model == "quadratic_runtime"
|
||||
)
|
||||
coupling_polycoef = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in result.mimic_fits[
|
||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
].mujoco_polycoef
|
||||
)
|
||||
for name in sorted(CORRECTED_PASSIVE_JOINTS)
|
||||
}
|
||||
equality_replacements: dict[str, str] = {}
|
||||
for equality in root.findall("./mujoco/equality/joint"):
|
||||
target = str(equality.get("joint1", ""))
|
||||
source_name = str(equality.get("joint2", ""))
|
||||
if target not in coupling_polycoef:
|
||||
continue
|
||||
if MIMIC_SOURCE_BY_JOINT[target] != source_name:
|
||||
raise ValueError(f"MuJoCo equality source differs for {target}")
|
||||
coefficients = [
|
||||
float(value) for value in str(equality.get("polycoef", "")).split()
|
||||
]
|
||||
if len(coefficients) != 6:
|
||||
raise ValueError(f"MuJoCo equality polycoef is invalid for {target}")
|
||||
offset = float(joints[target].find("mimic").get("offset", "0"))
|
||||
coefficients = list(coupling_polycoef[target])
|
||||
coefficients[0] = offset
|
||||
equality_name = str(equality.get("name", ""))
|
||||
if not equality_name:
|
||||
raise ValueError(f"MuJoCo equality has no name for {target}")
|
||||
equality_replacements[equality_name] = " ".join(
|
||||
f"{value:.15g}" for value in coefficients
|
||||
)
|
||||
if len(equality_replacements) != len(coupling_polycoef):
|
||||
raise ValueError("source URDF lacks a MuJoCo equality for fitted coupling")
|
||||
|
||||
original = source.read_text(encoding="utf-8")
|
||||
# The file also contains MuJoCo/transmission ``<joint>`` elements. Requiring
|
||||
# a URDF ``type`` attribute keeps the surgical block replacement confined
|
||||
# to the eleven kinematic joints.
|
||||
joint_pattern = re.compile(
|
||||
r"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
|
||||
r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
|
||||
r".*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
|
||||
def replace_joint(match: re.Match[str]) -> str:
|
||||
name = match.group("name")
|
||||
block = match.group(0)
|
||||
if name in active_replacements:
|
||||
rpy, lower, upper = active_replacements[name]
|
||||
block = _replace_attribute(block, "origin", "rpy", rpy)
|
||||
block = _replace_attribute(block, "limit", "lower", lower)
|
||||
block = _replace_attribute(block, "limit", "upper", upper)
|
||||
if name in mimic_replacements:
|
||||
block = _replace_attribute(
|
||||
block, "mimic", "multiplier", mimic_replacements[name]
|
||||
)
|
||||
return block
|
||||
|
||||
corrected = joint_pattern.sub(replace_joint, original)
|
||||
for equality_name, polycoef in equality_replacements.items():
|
||||
equality_pattern = re.compile(
|
||||
rf"(<joint\b[^>]*\bname\s*=\s*([\"'])"
|
||||
rf"{re.escape(equality_name)}\2[^>]*>)",
|
||||
re.DOTALL,
|
||||
)
|
||||
match = equality_pattern.search(corrected)
|
||||
if match is None:
|
||||
raise ValueError(f"could not locate MuJoCo equality {equality_name}")
|
||||
replacement = _replace_attribute(
|
||||
match.group(0), "joint", "polycoef", polycoef
|
||||
)
|
||||
corrected = corrected[: match.start()] + replacement + corrected[match.end() :]
|
||||
|
||||
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
if re.fullmatch(r"\d{8}_\d{6}", stamp) is None:
|
||||
raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS")
|
||||
safe_serial = "".join(
|
||||
char if char.isalnum() or char in "_.-" else "_"
|
||||
for char in str(serial_number)
|
||||
)
|
||||
if not safe_serial:
|
||||
raise ValueError("serial number must not be empty")
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
destination = output / (
|
||||
f"{source.stem}_partial_zero_calibrated_{safe_serial}_{stamp}.urdf"
|
||||
)
|
||||
if destination.exists() or destination == source:
|
||||
raise ValueError(f"refusing to overwrite URDF: {destination}")
|
||||
_materialize_relative_mesh_assets(source=source, output=output, urdf_root=root)
|
||||
# Keep the complete vendor mesh bundle beside the generated URDF, including
|
||||
# auxiliary meshes not referenced by this particular XML revision.
|
||||
source_meshes = source.parent / "meshes"
|
||||
if source_meshes.is_dir():
|
||||
destination_meshes = output / "meshes"
|
||||
destination_meshes.mkdir(parents=True, exist_ok=True)
|
||||
for mesh in source_meshes.iterdir():
|
||||
if mesh.is_file():
|
||||
shutil.copy2(mesh, destination_meshes / mesh.name)
|
||||
temporary = destination.with_suffix(".urdf.tmp")
|
||||
try:
|
||||
with temporary.open("w", encoding="utf-8") as stream:
|
||||
stream.write(corrected)
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
os.replace(temporary, destination)
|
||||
finally:
|
||||
if temporary.exists():
|
||||
temporary.unlink()
|
||||
return L6UrdfCorrection(
|
||||
path=destination,
|
||||
origin_offsets_rad={
|
||||
name: float(
|
||||
result.zero_offsets_rad[
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
]
|
||||
)
|
||||
for name in sorted(CORRECTED_ACTIVE_JOINTS)
|
||||
},
|
||||
corrected_limits_rad={
|
||||
name: (
|
||||
0.0,
|
||||
float(
|
||||
result.travels_rad[
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
]
|
||||
),
|
||||
)
|
||||
for name in sorted(CORRECTED_ACTIVE_JOINTS)
|
||||
},
|
||||
mimic_multipliers={
|
||||
name: float(
|
||||
result.mimic_fits[
|
||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
].urdf_mimic_multiplier
|
||||
)
|
||||
for name in sorted(mimic_replacements)
|
||||
},
|
||||
coupling_polycoef=coupling_polycoef,
|
||||
explicit_runtime_joints=explicit_runtime_joints,
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["L6UrdfCorrection", "write_l6_corrected_urdf"]
|
||||
@@ -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",
|
||||
)
|
||||
Reference in New Issue
Block a user