Compare commits
16 Commits
o30
...
8de69c34a1
| Author | SHA1 | Date | |
|---|---|---|---|
| 8de69c34a1 | |||
| d6b7bd6209 | |||
| 8a749a3687 | |||
| 08fe190b3a | |||
| f7aeef87a8 | |||
| 2b7c1f92e7 | |||
| 1ed36ecdd8 | |||
| 7f84225ba8 | |||
| ba9f1b25e8 | |||
| 0d606c2ba2 | |||
| 06c050e446 | |||
| 286581bcba | |||
| 4dadfb954b | |||
| 83c69b48c2 | |||
| 4e594ddb09 | |||
| ef65681230 |
+5
-1
@@ -62,13 +62,15 @@ Thumbs.db
|
||||
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
||||
/profiles/
|
||||
/calibration_output/
|
||||
/config/g20_three_camera_extrinsics.yaml
|
||||
/config/*_three_camera_extrinsics.yaml
|
||||
*.wear_check.json
|
||||
*.checkpoint.json
|
||||
*.verification.json
|
||||
*_mapping_quality.json
|
||||
|
||||
# Device-specific robot descriptions derived from local calibration runs
|
||||
# Includes full/partial zero-calibration outputs and local copies.
|
||||
/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
|
||||
@@ -92,3 +94,5 @@ candump-*
|
||||
# Local Codex/agent workspace metadata
|
||||
/.agents/
|
||||
/.codex/
|
||||
/.codebuddy/
|
||||
/.zcode/
|
||||
|
||||
@@ -1,120 +0,0 @@
|
||||
g20_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /g20/cb_left_hand_control_cmd
|
||||
state_topic: /g20/cb_left_hand_state
|
||||
info_topic: /g20/cb_left_hand_info
|
||||
setting_topic: /g20/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /g20_calibration/front/camera/camera_info
|
||||
front_detections_topic: /g20_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /g20_calibration/side/camera/camera_info
|
||||
side_detections_topic: /g20_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /g20_calibration/top/camera/camera_info
|
||||
top_detections_topic: /g20_calibration/top/apriltag/detections
|
||||
|
||||
# /start先下发并确认这个20通道基准姿态,稳定后才进入第一条扫描。
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
normal_calibration_speed: 15
|
||||
index_roll_calibration_speed: 5
|
||||
index_flex_calibration_speed: 10
|
||||
speed_setting_settle_seconds: 0.25
|
||||
|
||||
# tag36h11检测角点围成的黑色正方形实测为16 mm;不包含外围白边。
|
||||
tag_size_m: 0.016
|
||||
repetitions: 3
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_detection_hz: 15.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
|
||||
# 标定任务不再用第一帧决定平面Tag的IPPE分支;静止端点联合8帧选择整组最稳定解。
|
||||
pnp_group_initialization_frames: 8
|
||||
# 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致;用此先验消除静态IPPE镜像双解。
|
||||
pnp_group_normal_alignment_scale_deg: 5.0
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
top_pnp_invalid_reset_seconds: 1.0
|
||||
# 三维位姿必须与实测20通道状态严格按时间戳配对。
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
|
||||
axis_maximum_plane_rms_m: 0.003
|
||||
# 被动耦合轴只用轨迹确定轴线位置,允许更大的轴向深度噪声;径向和跨轮
|
||||
# 轴线一致性仍沿用严格检查。
|
||||
passive_axis_maximum_plane_rms_m: 0.004
|
||||
axis_maximum_radial_rms_m: 0.003
|
||||
# 整段相对SE(3)运动拟合轴线点;端视关节会投影掉单目PnP光轴深度。
|
||||
axis_maximum_pose_line_rms_m: 0.001
|
||||
# 仅用于运动平面在三维中可观测的斜视关节;近图像平面关节使用姿态轴
|
||||
# 约束三维圆,不让单目平面Tag的深度噪声自由决定转轴方向。
|
||||
axis_maximum_rotation_circle_difference_deg: 1.0
|
||||
# 单轴模型残差与跨轮重复误差分开判定:主动刚性关节要求更严;被动耦合
|
||||
# 关节允许可重复的非理想单轴分量,但仍须通过0.75°跨轮轴差及第三轮留出。
|
||||
active_maximum_rotation_orthogonal_rms_deg: 2.5
|
||||
passive_maximum_rotation_orthogonal_rms_deg: 7.5
|
||||
zero_maximum_axis_cycle_difference_deg: 0.75
|
||||
# 零位无法改变父子轴夹角;超过该值属于CAD/PnP几何错误,不能吸收到零位。
|
||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||
zero_maximum_offset_deg: 20.0
|
||||
# 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线,静态零位固定为CAD 0。
|
||||
zero_finger_maximum_offset_deg: 3.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 电机10在命令0时实测会稳定反馈为4;该0端使用±4。
|
||||
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
|
||||
# 右手电机10在命令255时多次实测稳定反馈为250;仅右手该端点使用±5。
|
||||
right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
|
||||
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5;仅其0端使用±5。
|
||||
pinky_pip_zero_endpoint_tolerance_u8: 5.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
baseline_hold_seconds: 0.5
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
# 反馈在远离目标时连续8秒没有至少1个u8的进展,按机械卡滞立即暂停;
|
||||
# 这类故障不进入遮挡/超时的三次自动重扫。
|
||||
motor_stall_timeout_seconds: 8.0
|
||||
motor_stall_minimum_progress_u8: 1.0
|
||||
invalid_timeout_seconds: 3.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
# 可恢复的采样失败自动重扫当前方向;超过次数才暂停等待人工处理。
|
||||
automatic_sweep_retry_limit: 3
|
||||
# 轨迹拟合失败优先只重扫失败轮次;零位/URDF模型失败不重复运动。
|
||||
automatic_fit_retry_limit: 2
|
||||
automatic_motion_retry_limit: 2
|
||||
# 过程检查允许25%的黄色预警带,最终验收仍使用下面的严格门限。
|
||||
provisional_warning_ratio: 1.25
|
||||
retry_minimum_speed: 3
|
||||
retry_speed_scales: [0.8, 0.6, 0.5]
|
||||
retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
|
||||
|
||||
trajectory_maximum_plane_rms_m: 0.004
|
||||
trajectory_maximum_radial_rms_m: 0.004
|
||||
trajectory_minimum_radius_m: 0.003
|
||||
trajectory_minimum_arc_deg: 15.0
|
||||
# 以下二维参数只供旧轨迹工具兼容,三机位v4零位不使用二维投影。
|
||||
image_trajectory_maximum_radial_rms_px: 2.0
|
||||
image_trajectory_maximum_radial_p95_px: 3.5
|
||||
image_trajectory_minimum_radius_px: 20.0
|
||||
trajectory_maximum_cycle_travel_difference_deg: 3.0
|
||||
passive_maximum_cycle_travel_difference_deg: 10.0
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
maximum_hysteresis_deg: 5.0
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 7.5
|
||||
|
||||
# 默认无额外随机动作;第三轮扫描始终作为不可关闭的留出验证。
|
||||
validation_enabled: false
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
@@ -1,5 +0,0 @@
|
||||
"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb."""
|
||||
|
||||
from .core import BASELINE_COMMAND, COMMAND_NAMES
|
||||
|
||||
__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"]
|
||||
-218
@@ -1,218 +0,0 @@
|
||||
"""Map G20 u8 commands 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
|
||||
dynamic ``angle_rad`` values and never adds the static offsets a second time.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .full_hand import get_hand_calibration_profile, validate_compact_payload
|
||||
|
||||
|
||||
G20_COMMAND_NAMES: tuple[str, ...] = (
|
||||
"thumb_cmc_pitch",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"reserved_11",
|
||||
"reserved_12",
|
||||
"reserved_13",
|
||||
"reserved_14",
|
||||
"thumb_mcp",
|
||||
"index_pip",
|
||||
"middle_pip",
|
||||
"ring_pip",
|
||||
"pinky_pip",
|
||||
)
|
||||
|
||||
# Match the stable ordering used by the existing MuJoCo bridge. JointState
|
||||
# consumers must use names, but retaining the ordering also keeps logs and
|
||||
# direct comparisons deterministic.
|
||||
G20_URDF_JOINT_NAMES: tuple[str, ...] = (
|
||||
"index_dip",
|
||||
"index_mcp_pitch",
|
||||
"index_mcp_roll",
|
||||
"index_pip",
|
||||
"middle_dip",
|
||||
"middle_mcp_pitch",
|
||||
"middle_mcp_roll",
|
||||
"middle_pip",
|
||||
"pinky_dip",
|
||||
"pinky_mcp_pitch",
|
||||
"pinky_mcp_roll",
|
||||
"pinky_pip",
|
||||
"ring_dip",
|
||||
"ring_mcp_pitch",
|
||||
"ring_mcp_roll",
|
||||
"ring_pip",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_ip",
|
||||
"thumb_mcp",
|
||||
)
|
||||
|
||||
|
||||
class CalibratedCommandMapper:
|
||||
"""Validated, side-specific lookup from a G20 command to URDF radians."""
|
||||
|
||||
def __init__(
|
||||
self, payload: Mapping[str, Any], *, expected_side: str | None = None
|
||||
) -> None:
|
||||
validate_compact_payload(payload)
|
||||
side = str(payload["side"]).lower()
|
||||
if expected_side is not None and side != str(expected_side).lower():
|
||||
raise ValueError(
|
||||
f"calibration side {side!r} does not match requested side "
|
||||
f"{str(expected_side).lower()!r}"
|
||||
)
|
||||
quality = payload["quality"]
|
||||
if quality.get("passed") is not True:
|
||||
raise ValueError("calibration quality.passed must be true")
|
||||
profile = get_hand_calibration_profile(side)
|
||||
self.side = side
|
||||
self.serial_number = str(payload["serial_number"])
|
||||
self._motor_by_joint = {
|
||||
name: int(profile.joint_specs[name].motor_index)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def _command_index(value: float) -> int:
|
||||
command = float(value)
|
||||
if not math.isfinite(command):
|
||||
raise ValueError("G20 command positions must be finite")
|
||||
return max(0, min(255, int(math.floor(command + 0.5))))
|
||||
|
||||
def map_positions(
|
||||
self, positions: Sequence[float], names: Sequence[str] = ()
|
||||
) -> tuple[float, ...]:
|
||||
values = tuple(float(value) for value in positions)
|
||||
if names:
|
||||
if len(names) != len(values):
|
||||
raise ValueError(
|
||||
"JointState names and positions must have equal length"
|
||||
)
|
||||
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]
|
||||
if missing:
|
||||
raise ValueError(
|
||||
"G20 command is missing named channels: " + ",".join(missing)
|
||||
)
|
||||
command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
|
||||
else:
|
||||
if len(values) != len(G20_COMMAND_NAMES):
|
||||
raise ValueError(
|
||||
"unnamed G20 command must contain exactly 20 positions"
|
||||
)
|
||||
command = values
|
||||
indices = tuple(self._command_index(value) for value in command)
|
||||
return tuple(
|
||||
self._curves[name][indices[self._motor_by_joint[name]]]
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
)
|
||||
|
||||
|
||||
def load_calibrated_command_mapper(
|
||||
calibration_file: str | Path, *, expected_side: str | None = None
|
||||
) -> CalibratedCommandMapper:
|
||||
path = Path(calibration_file).expanduser().resolve()
|
||||
if not path.is_file():
|
||||
raise ValueError(f"calibration JSON does not exist: {path}")
|
||||
payload = json.loads(path.read_text(encoding="utf-8"))
|
||||
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
||||
|
||||
|
||||
class CalibratedJointStateBridge(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibrated_joint_state_bridge")
|
||||
self.declare_parameter("hand_type", "right")
|
||||
self.declare_parameter("calibration_file", "")
|
||||
self.declare_parameter("input_topic", "")
|
||||
self.declare_parameter("output_topic", "")
|
||||
|
||||
hand_type = str(self.get_parameter("hand_type").value).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
calibration_file = str(self.get_parameter("calibration_file").value)
|
||||
if not calibration_file:
|
||||
raise ValueError("calibration_file is required")
|
||||
self.mapper = load_calibrated_command_mapper(
|
||||
calibration_file, expected_side=hand_type
|
||||
)
|
||||
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 f"/cb_{hand_type}_hand_control_cmd"
|
||||
self.output_topic = (
|
||||
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
|
||||
)
|
||||
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState, self.input_topic, self._command_callback, 10
|
||||
)
|
||||
self._last_error = ""
|
||||
self.get_logger().info(
|
||||
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
|
||||
f"{self.input_topic} -> {self.output_topic}"
|
||||
)
|
||||
|
||||
def _command_callback(self, command: JointState) -> None:
|
||||
try:
|
||||
positions = self.mapper.map_positions(command.position, command.name)
|
||||
except ValueError as error:
|
||||
message = str(error)
|
||||
if message != self._last_error:
|
||||
self.get_logger().error(message)
|
||||
self._last_error = message
|
||||
return
|
||||
self._last_error = ""
|
||||
result = JointState()
|
||||
result.header = command.header
|
||||
result.name = list(G20_URDF_JOINT_NAMES)
|
||||
result.position = list(positions)
|
||||
self.publisher.publish(result)
|
||||
|
||||
|
||||
def main(args: Sequence[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node: CalibratedJointStateBridge | None = None
|
||||
try:
|
||||
node = CalibratedJointStateBridge()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,878 +0,0 @@
|
||||
"""Pure three-camera G20 calibration model and compact runtime schema.
|
||||
|
||||
The hardware node records one parent/child AprilTag trajectory for each
|
||||
directly observable joint. This module deliberately contains no ROS imports:
|
||||
curve fitting, baseline centring, inheritance, and schema validation
|
||||
remain deterministic and unit-testable without connected cameras or a hand.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .trajectory import (
|
||||
_angle_for_circle,
|
||||
_fit_circle_with_axis,
|
||||
_fit_joint_curve,
|
||||
_fit_plane_axis,
|
||||
_orient_circle_positive,
|
||||
)
|
||||
from .zero_calibration import (
|
||||
_fit_circle,
|
||||
_trajectory_arc_rad,
|
||||
circular_median_rad,
|
||||
maximum_pairwise_angle_difference_rad,
|
||||
wrap_angle_rad,
|
||||
)
|
||||
|
||||
|
||||
THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = (
|
||||
255, 255, 255, 255, 255, 255,
|
||||
127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointSpec:
|
||||
name: str
|
||||
motor_index: int
|
||||
active: bool
|
||||
view: str | None
|
||||
parent_role: str | None
|
||||
child_role: str | None
|
||||
source_joint: str | None = None
|
||||
zero_kind: str | None = None
|
||||
|
||||
@property
|
||||
def measured(self) -> bool:
|
||||
return self.source_joint is None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SweepSpec:
|
||||
view: str
|
||||
motor_index: int
|
||||
joints: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointCurveFit:
|
||||
angle_rad: tuple[float, ...]
|
||||
decreasing_rad: tuple[float, ...]
|
||||
increasing_rad: tuple[float, ...]
|
||||
circle: Mapping[str, Any]
|
||||
maximum_monotonic_correction_rad: float
|
||||
maximum_hysteresis_rad: float
|
||||
quality: Mapping[str, float]
|
||||
zero_offset_rad: float = 0.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HandCalibrationProfile:
|
||||
side: str
|
||||
reference_finger: str
|
||||
view_tags: Mapping[str, Mapping[str, int]]
|
||||
preflight_view_roles: Mapping[str, tuple[str, ...]]
|
||||
joint_specs: Mapping[str, JointSpec]
|
||||
sweep_specs: tuple[SweepSpec, ...]
|
||||
image_trajectory_joints: frozenset[str]
|
||||
roll_clearance_commands: Mapping[int, int]
|
||||
thumb_pitch_clearance_commands: Mapping[int, int]
|
||||
|
||||
@property
|
||||
def measured_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if spec.measured
|
||||
)
|
||||
|
||||
@property
|
||||
def active_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if spec.active
|
||||
)
|
||||
|
||||
@property
|
||||
def passive_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if not spec.active
|
||||
)
|
||||
|
||||
@property
|
||||
def reference_roll_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_pitch_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_pip_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_speed_slot(self) -> int:
|
||||
return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[
|
||||
self.reference_finger
|
||||
]
|
||||
|
||||
|
||||
_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky")
|
||||
_MOTOR_BY_JOINT: dict[str, int] = {
|
||||
**{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)},
|
||||
**{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)},
|
||||
**{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)},
|
||||
}
|
||||
|
||||
|
||||
def _build_hand_profile(side: str) -> HandCalibrationProfile:
|
||||
hand_side = str(side).lower()
|
||||
if hand_side not in {"left", "right"}:
|
||||
raise ValueError("hand side must be left or right")
|
||||
reference = "index" if hand_side == "left" else "pinky"
|
||||
roll_role = f"{reference}_roll"
|
||||
mcp_role = f"{reference}_mcp"
|
||||
pip_role = f"{reference}_pip"
|
||||
dip_role = f"{reference}_dip"
|
||||
view_tags: dict[str, dict[str, int]] = {
|
||||
"front": {
|
||||
"front_base": 0,
|
||||
"thumb_cmc": 1,
|
||||
"thumb_mcp": 2,
|
||||
"thumb_ip": 3,
|
||||
roll_role: 10,
|
||||
},
|
||||
"side": {
|
||||
"side_base": 4,
|
||||
mcp_role: 5,
|
||||
pip_role: 6,
|
||||
dip_role: 7,
|
||||
},
|
||||
"top": {"top_base": 8, "thumb_yaw": 9},
|
||||
}
|
||||
preflight_view_roles = {
|
||||
view: tuple(tags) for view, tags in view_tags.items()
|
||||
}
|
||||
joint_specs: dict[str, JointSpec] = {}
|
||||
# Both hands use the front base/Tag 1 pair for motor 0. Keeping the
|
||||
# complete thumb root chain in one calibrated view avoids introducing a
|
||||
# cross-camera axis-line phase into the URDF zero solve.
|
||||
thumb_pitch_view = "front"
|
||||
thumb_pitch_parent = "front_base"
|
||||
joint_specs["thumb_cmc_pitch"] = JointSpec(
|
||||
"thumb_cmc_pitch",
|
||||
0,
|
||||
True,
|
||||
thumb_pitch_view,
|
||||
thumb_pitch_parent,
|
||||
"thumb_cmc",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_mcp_pitch"
|
||||
source = None if finger == reference else f"{reference}_mcp_pitch"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"side" if source is None else None,
|
||||
"side_base" if source is None else None,
|
||||
mcp_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_cmc_roll"] = JointSpec(
|
||||
"thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_mcp_roll"
|
||||
source = None if finger == reference else f"{reference}_mcp_roll"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"front" if source is None else None,
|
||||
"front_base" if source is None else None,
|
||||
roll_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_cmc_yaw"] = JointSpec(
|
||||
"thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
joint_specs["thumb_mcp"] = JointSpec(
|
||||
"thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_pip"
|
||||
source = None if finger == reference else f"{reference}_pip"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"side" if source is None else None,
|
||||
mcp_role if source is None else None,
|
||||
pip_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_ip"] = JointSpec(
|
||||
"thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_dip"
|
||||
source = None if finger == reference else f"{reference}_dip"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[f"{finger}_pip"],
|
||||
False,
|
||||
"side" if source is None else None,
|
||||
pip_role if source is None else None,
|
||||
dip_role if source is None else None,
|
||||
source_joint=source,
|
||||
)
|
||||
sweep_specs = (
|
||||
SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)),
|
||||
SweepSpec("front", 5, ("thumb_cmc_roll",)),
|
||||
SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
|
||||
SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], (
|
||||
f"{reference}_mcp_roll",
|
||||
)),
|
||||
SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], (
|
||||
f"{reference}_mcp_pitch",
|
||||
)),
|
||||
SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], (
|
||||
f"{reference}_pip", f"{reference}_dip",
|
||||
)),
|
||||
SweepSpec("top", 10, ("thumb_cmc_yaw",)),
|
||||
)
|
||||
image_joints = frozenset(
|
||||
{
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
f"{reference}_mcp_roll",
|
||||
f"{reference}_mcp_pitch",
|
||||
f"{reference}_pip",
|
||||
}
|
||||
)
|
||||
measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"]
|
||||
# The roll channels are mirrored mechanically between hands. Command 0
|
||||
# moves the three non-reference fingers to the camera-right clearance pose
|
||||
# on the left hand, while the same physical pose is command 255 on the
|
||||
# right hand. Using 0 for both sides made the right index/middle/ring
|
||||
# fingers lean left and could obstruct or load the pinky roll sweep.
|
||||
roll_clearance_command = 0 if hand_side == "left" else 255
|
||||
clearance = {
|
||||
_MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command
|
||||
for finger in _FINGERS
|
||||
if finger != reference
|
||||
}
|
||||
assert measured_roll not in clearance
|
||||
# Motor 0 is observed from the front with both other thumb-root channels
|
||||
# fully open. Store the commands explicitly for the right hand so a
|
||||
# caller-provided baseline cannot silently restore the obsolete 50/120
|
||||
# pose and hide Tag 1 from the front camera.
|
||||
thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {}
|
||||
return HandCalibrationProfile(
|
||||
side=hand_side,
|
||||
reference_finger=reference,
|
||||
view_tags=view_tags,
|
||||
preflight_view_roles=preflight_view_roles,
|
||||
joint_specs=joint_specs,
|
||||
sweep_specs=sweep_specs,
|
||||
image_trajectory_joints=image_joints,
|
||||
roll_clearance_commands=clearance,
|
||||
thumb_pitch_clearance_commands=thumb_pitch_clearance,
|
||||
)
|
||||
|
||||
|
||||
LEFT_HAND_PROFILE = _build_hand_profile("left")
|
||||
RIGHT_HAND_PROFILE = _build_hand_profile("right")
|
||||
|
||||
|
||||
def get_hand_calibration_profile(side: str) -> HandCalibrationProfile:
|
||||
value = str(side).lower()
|
||||
if value == "left":
|
||||
return LEFT_HAND_PROFILE
|
||||
if value == "right":
|
||||
return RIGHT_HAND_PROFILE
|
||||
raise ValueError("hand side must be left or right")
|
||||
|
||||
|
||||
# Backwards-compatible aliases keep the existing left-hand API stable.
|
||||
VIEW_TAGS = LEFT_HAND_PROFILE.view_tags
|
||||
JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs
|
||||
SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs
|
||||
MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints
|
||||
ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints
|
||||
PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints
|
||||
SPLAY_JOINTS: tuple[str, ...] = (
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
)
|
||||
|
||||
# These directly actuated motions have a fixed parent link during their sweep
|
||||
# and a motion plane that is close to the active camera's image plane. Their
|
||||
# projected circles are substantially more repeatable than the difference of
|
||||
# two independently estimated planar-Tag PnP depths. thumb_ip remains projected
|
||||
# because its front-view PnP depth is not repeatable enough for a 3-D fit. The
|
||||
# side-view index_dip and oblique top-view yaw remain parent-relative full
|
||||
# SE(3) measurements; their circle directions are constrained by the more
|
||||
# repeatable relative-orientation screw axis in urdf_zero.py.
|
||||
IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints
|
||||
|
||||
# Keep the three unmeasured finger-roll motors away from the front camera's
|
||||
# line of sight while index_mcp_roll is measured.
|
||||
INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands
|
||||
|
||||
# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is
|
||||
# measured. This keeps the moving top-view tag sufficiently front-facing.
|
||||
THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = {
|
||||
5: 145,
|
||||
}
|
||||
|
||||
|
||||
def calibration_auxiliary_commands(
|
||||
spec: SweepSpec,
|
||||
*,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> dict[int, int]:
|
||||
"""Return motors that must remain fixed throughout one calibration task."""
|
||||
if spec.motor_index == 0:
|
||||
return dict(profile.thumb_pitch_clearance_commands)
|
||||
if spec.motor_index == profile.reference_roll_motor:
|
||||
return dict(profile.roll_clearance_commands)
|
||||
if spec.motor_index == 10:
|
||||
return dict(THUMB_YAW_CLEARANCE_COMMANDS)
|
||||
return {}
|
||||
|
||||
|
||||
def build_full_hand_command(
|
||||
motor_index: int,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 values")
|
||||
motor = int(motor_index)
|
||||
if motor not in {spec.motor_index for spec in profile.joint_specs.values()}:
|
||||
raise ValueError("motor_index is not a controlled G20 calibration channel")
|
||||
command = int(command_u8)
|
||||
if not 0 <= command <= 255:
|
||||
raise ValueError("command_u8 must be in [0, 255]")
|
||||
result = [int(value) for value in baseline]
|
||||
if any(not 0 <= value <= 255 for value in result):
|
||||
raise ValueError("baseline values must be in [0, 255]")
|
||||
result[motor] = command
|
||||
return result
|
||||
|
||||
|
||||
def build_calibration_motion_command(
|
||||
spec: SweepSpec,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
"""Build a sweep command, including any required clearance pose."""
|
||||
result = build_full_hand_command(
|
||||
spec.motor_index,
|
||||
command_u8,
|
||||
baseline=baseline,
|
||||
profile=profile,
|
||||
)
|
||||
for motor_index, auxiliary_command in calibration_auxiliary_commands(
|
||||
spec, profile=profile
|
||||
).items():
|
||||
result[motor_index] = auxiliary_command
|
||||
return result
|
||||
|
||||
|
||||
def build_calibration_speed_profile(
|
||||
spec: SweepSpec,
|
||||
*,
|
||||
normal_speed: int,
|
||||
index_roll_speed: int,
|
||||
index_flex_speed: int,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
"""Return G20's five per-finger speeds for one calibration task."""
|
||||
normal = int(normal_speed)
|
||||
index_roll = int(index_roll_speed)
|
||||
index_flex = int(index_flex_speed)
|
||||
if not all(
|
||||
0 <= speed <= 255 for speed in (normal, index_roll, index_flex)
|
||||
):
|
||||
raise ValueError("calibration speeds must be in [0, 255]")
|
||||
speeds = [normal] * 5
|
||||
if spec.motor_index == profile.reference_roll_motor:
|
||||
speeds[profile.reference_speed_slot] = index_roll
|
||||
elif spec.motor_index in {
|
||||
profile.reference_pitch_motor,
|
||||
profile.reference_pip_motor,
|
||||
}:
|
||||
speeds[profile.reference_speed_slot] = index_flex
|
||||
return speeds
|
||||
|
||||
|
||||
def _record_vector(record: Mapping[str, Any]) -> np.ndarray:
|
||||
value = np.asarray(record.get("relative_translation_xyz_m"), dtype=float)
|
||||
if value.shape != (3,) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("record relative_translation_xyz_m must contain 3 values")
|
||||
return value
|
||||
|
||||
|
||||
def _record_image_vector(record: Mapping[str, Any]) -> np.ndarray:
|
||||
value = np.asarray(record.get("image_relative_xy_px"), dtype=float)
|
||||
if value.shape != (2,) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("record image_relative_xy_px must contain 2 values")
|
||||
return value
|
||||
|
||||
|
||||
def fit_joint_center_curve(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_plane_rms_m: float = 0.004,
|
||||
maximum_radial_rms_m: float = 0.004,
|
||||
minimum_radius_m: float = 0.003,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
) -> JointCurveFit:
|
||||
"""Fit one command-indexed curve from a parent-frame centre trajectory."""
|
||||
samples = [dict(record) for record in records]
|
||||
if len(samples) < 12:
|
||||
raise ValueError("joint trajectory requires at least 12 samples")
|
||||
points = np.asarray([_record_vector(record) for record in samples])
|
||||
axis, common_plane_rms = _fit_plane_axis([points])
|
||||
circle = _fit_circle_with_axis(points, axis)
|
||||
circle = _orient_circle_positive(circle, samples, list(points))
|
||||
values = [_angle_for_circle(point, circle) for point in points]
|
||||
curves, correction, hysteresis = _fit_joint_curve(samples, values)
|
||||
quality = {
|
||||
"plane_rms_m": max(
|
||||
float(common_plane_rms), float(circle["plane_rms_m"])
|
||||
),
|
||||
"radial_rms_m": float(circle["radial_rms_m"]),
|
||||
"radius_m": float(circle["radius_m"]),
|
||||
"arc_rad": float(circle["observed_arc_rad"]),
|
||||
}
|
||||
failures: list[str] = []
|
||||
if quality["plane_rms_m"] > float(maximum_plane_rms_m):
|
||||
failures.append("plane_rms")
|
||||
if quality["radial_rms_m"] > float(maximum_radial_rms_m):
|
||||
failures.append("radial_rms")
|
||||
if quality["radius_m"] < float(minimum_radius_m):
|
||||
failures.append("radius")
|
||||
if quality["arc_rad"] < float(minimum_arc_rad):
|
||||
failures.append("arc")
|
||||
if failures:
|
||||
raise ValueError("joint_trajectory_quality_failed:" + ",".join(failures))
|
||||
return JointCurveFit(
|
||||
angle_rad=tuple(float(value) for value in curves["angle_rad"]),
|
||||
decreasing_rad=tuple(
|
||||
float(value) for value in curves["decreasing_rad"]
|
||||
),
|
||||
increasing_rad=tuple(
|
||||
float(value) for value in curves["increasing_rad"]
|
||||
),
|
||||
circle=dict(circle),
|
||||
maximum_monotonic_correction_rad=float(correction),
|
||||
maximum_hysteresis_rad=float(hysteresis),
|
||||
quality=quality,
|
||||
)
|
||||
|
||||
|
||||
def _signed_image_circle_angle(
|
||||
point_xy_px: Sequence[float], circle: Mapping[str, Any]
|
||||
) -> float:
|
||||
point = np.asarray(point_xy_px, dtype=float)
|
||||
centre = np.asarray(circle["center_xy_px"], dtype=float)
|
||||
reference = np.asarray(circle["reference_xy_px"], dtype=float)
|
||||
vector = point - centre
|
||||
if point.shape != (2,) or not np.all(np.isfinite(point)):
|
||||
raise ValueError("image point must contain 2 finite values")
|
||||
if float(np.linalg.norm(vector)) < 1.0e-9:
|
||||
raise ValueError("image point lies at the fitted circle centre")
|
||||
return float(circle["orientation_sign"]) * math.atan2(
|
||||
float(reference[0] * vector[1] - reference[1] * vector[0]),
|
||||
float(np.dot(reference, vector)),
|
||||
)
|
||||
|
||||
|
||||
def fit_joint_image_curve(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_radial_rms_px: float = 2.0,
|
||||
maximum_radial_p95_px: float = 3.5,
|
||||
minimum_radius_px: float = 20.0,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
) -> JointCurveFit:
|
||||
"""Fit angular phase from a stable projected parent/child circle."""
|
||||
samples = [dict(record) for record in records]
|
||||
if len(samples) < 12:
|
||||
raise ValueError("joint image trajectory requires at least 12 samples")
|
||||
points = np.asarray([_record_image_vector(record) for record in samples])
|
||||
centre, radius = _fit_circle(points)
|
||||
radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
|
||||
quality = {
|
||||
"radial_rms_px": float(np.sqrt(np.mean(np.square(radial_error)))),
|
||||
"radial_p95_px": float(np.percentile(radial_error, 95.0)),
|
||||
"radius_px": float(radius),
|
||||
"arc_rad": float(_trajectory_arc_rad(points, centre)),
|
||||
}
|
||||
failures: list[str] = []
|
||||
if quality["radial_rms_px"] > float(maximum_radial_rms_px):
|
||||
failures.append("radial_rms")
|
||||
if quality["radial_p95_px"] > float(maximum_radial_p95_px):
|
||||
failures.append("radial_p95")
|
||||
if quality["radius_px"] < float(minimum_radius_px):
|
||||
failures.append("radius")
|
||||
if quality["arc_rad"] < float(minimum_arc_rad):
|
||||
failures.append("arc")
|
||||
if failures:
|
||||
raise ValueError(
|
||||
"joint_image_trajectory_quality_failed:" + ",".join(failures)
|
||||
)
|
||||
|
||||
endpoint_points = np.asarray(
|
||||
[
|
||||
point
|
||||
for record, point in zip(samples, points)
|
||||
if int(record["command_u8"]) == 255
|
||||
]
|
||||
)
|
||||
if endpoint_points.size == 0:
|
||||
raise ValueError("joint image trajectory is missing command 255")
|
||||
reference = np.median(endpoint_points, axis=0) - centre
|
||||
reference_norm = float(np.linalg.norm(reference))
|
||||
if reference_norm < 1.0e-9:
|
||||
raise ValueError("joint image trajectory endpoint is degenerate")
|
||||
reference /= reference_norm
|
||||
circle: dict[str, Any] = {
|
||||
"space": "image_2d",
|
||||
"center_xy_px": [float(value) for value in centre],
|
||||
"reference_xy_px": [float(value) for value in reference],
|
||||
"orientation_sign": 1.0,
|
||||
"radius_px": float(radius),
|
||||
}
|
||||
values = [_signed_image_circle_angle(point, circle) for point in points]
|
||||
command_zero_values = [
|
||||
value
|
||||
for record, value in zip(samples, values)
|
||||
if int(record["command_u8"]) == 0
|
||||
]
|
||||
if not command_zero_values:
|
||||
raise ValueError("joint image trajectory is missing command 0")
|
||||
if float(np.median(command_zero_values)) < 0.0:
|
||||
circle["orientation_sign"] = -1.0
|
||||
values = [-value for value in values]
|
||||
curves, correction, hysteresis = _fit_joint_curve(samples, values)
|
||||
return JointCurveFit(
|
||||
angle_rad=tuple(float(value) for value in curves["angle_rad"]),
|
||||
decreasing_rad=tuple(
|
||||
float(value) for value in curves["decreasing_rad"]
|
||||
),
|
||||
increasing_rad=tuple(
|
||||
float(value) for value in curves["increasing_rad"]
|
||||
),
|
||||
circle=circle,
|
||||
maximum_monotonic_correction_rad=float(correction),
|
||||
maximum_hysteresis_rad=float(hysteresis),
|
||||
quality=quality,
|
||||
)
|
||||
|
||||
|
||||
def fit_measured_joint_curve(
|
||||
joint_name: str,
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_plane_rms_m: float = 0.004,
|
||||
maximum_radial_rms_m: float = 0.004,
|
||||
minimum_radius_m: float = 0.003,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
image_maximum_radial_rms_px: float = 2.0,
|
||||
image_maximum_radial_p95_px: float = 3.5,
|
||||
image_minimum_radius_px: float = 20.0,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> JointCurveFit:
|
||||
"""Select the stable trajectory representation for one measured joint."""
|
||||
if str(joint_name) in profile.image_trajectory_joints:
|
||||
return fit_joint_image_curve(
|
||||
records,
|
||||
maximum_radial_rms_px=image_maximum_radial_rms_px,
|
||||
maximum_radial_p95_px=image_maximum_radial_p95_px,
|
||||
minimum_radius_px=image_minimum_radius_px,
|
||||
minimum_arc_rad=minimum_arc_rad,
|
||||
)
|
||||
return fit_joint_center_curve(
|
||||
records,
|
||||
maximum_plane_rms_m=maximum_plane_rms_m,
|
||||
maximum_radial_rms_m=maximum_radial_rms_m,
|
||||
minimum_radius_m=minimum_radius_m,
|
||||
minimum_arc_rad=minimum_arc_rad,
|
||||
)
|
||||
|
||||
|
||||
def center_splay_curve(fit: JointCurveFit) -> tuple[JointCurveFit, int, float]:
|
||||
"""Centre a measured 255->0 splay travel on its angular midpoint."""
|
||||
raw = np.asarray(fit.angle_rad, dtype=float)
|
||||
if raw.shape != (256,) or not np.all(np.isfinite(raw)):
|
||||
raise ValueError("splay curve must contain 256 finite values")
|
||||
if np.any(np.diff(raw) > 1.0e-7):
|
||||
raise ValueError("raw splay curve must be non-increasing")
|
||||
travel_midpoint = 0.5 * float(raw[0] + raw[255])
|
||||
if travel_midpoint <= 0.0:
|
||||
raise ValueError("splay travel must be positive")
|
||||
centred = raw - travel_midpoint
|
||||
# Make the public endpoint symmetry exact after decimal rounding.
|
||||
endpoint = round(0.5 * float(raw[0] - raw[255]), 8)
|
||||
centred[0] = endpoint
|
||||
centred[255] = -endpoint
|
||||
centred = np.asarray([round(float(value), 8) for value in centred])
|
||||
zero_command = int(np.argmin(np.abs(centred)))
|
||||
return (
|
||||
replace(
|
||||
fit,
|
||||
angle_rad=tuple(float(value) for value in centred),
|
||||
zero_offset_rad=round(float(travel_midpoint), 8),
|
||||
),
|
||||
zero_command,
|
||||
round(float(travel_midpoint), 8),
|
||||
)
|
||||
|
||||
|
||||
def measure_joint_vector(fit: JointCurveFit, vector_xyz_m: Sequence[float]) -> float:
|
||||
"""Measure one vector with a fitted model, including splay centring."""
|
||||
return float(
|
||||
_angle_for_circle(vector_xyz_m, fit.circle) - fit.zero_offset_rad
|
||||
)
|
||||
|
||||
|
||||
def measure_joint_observation(
|
||||
fit: JointCurveFit,
|
||||
*,
|
||||
vector_xyz_m: Sequence[float],
|
||||
image_vector_xy_px: Sequence[float],
|
||||
) -> float:
|
||||
"""Measure an observation in the same space used to fit its curve."""
|
||||
if fit.circle.get("space") == "image_2d":
|
||||
return float(
|
||||
_signed_image_circle_angle(image_vector_xy_px, fit.circle)
|
||||
- fit.zero_offset_rad
|
||||
)
|
||||
return measure_joint_vector(fit, vector_xyz_m)
|
||||
|
||||
|
||||
def fit_projected_zero(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
minimum_radius_px: float = 20.0,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
maximum_radial_rms_px: float = 2.0,
|
||||
maximum_radial_p95_px: float = 3.5,
|
||||
maximum_round_difference_rad: float = math.radians(1.0),
|
||||
) -> float:
|
||||
"""Return the image/table-projected zero from three sweep rounds."""
|
||||
samples = [dict(record) for record in records]
|
||||
points = np.asarray(
|
||||
[record.get("image_relative_xy_px") for record in samples], dtype=float
|
||||
)
|
||||
if points.ndim != 2 or points.shape[1] != 2 or len(points) < 12:
|
||||
raise ValueError("projected zero requires 2-D trajectory samples")
|
||||
if not np.all(np.isfinite(points)):
|
||||
raise ValueError("projected zero samples must be finite")
|
||||
centre, radius = _fit_circle(points)
|
||||
radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
|
||||
radial_rms = float(np.sqrt(np.mean(np.square(radial_error))))
|
||||
radial_p95 = float(np.percentile(radial_error, 95.0))
|
||||
arc = _trajectory_arc_rad(points, centre)
|
||||
if radius < minimum_radius_px:
|
||||
raise ValueError("projected_zero_radius_too_small")
|
||||
if arc < minimum_arc_rad:
|
||||
raise ValueError("projected_zero_arc_too_small")
|
||||
if radial_rms > maximum_radial_rms_px:
|
||||
raise ValueError("projected_zero_radial_rms_too_large")
|
||||
if radial_p95 > maximum_radial_p95_px:
|
||||
raise ValueError("projected_zero_radial_p95_too_large")
|
||||
|
||||
cycles = sorted({int(record["cycle"]) for record in samples})
|
||||
if len(cycles) < 3:
|
||||
raise ValueError("projected zero requires three sweep rounds")
|
||||
angles: list[float] = []
|
||||
for cycle in cycles:
|
||||
zero_points = np.asarray(
|
||||
[
|
||||
record["image_relative_xy_px"]
|
||||
for record in samples
|
||||
if int(record["cycle"]) == cycle
|
||||
and int(record["command_u8"]) == 255
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if zero_points.size == 0:
|
||||
raise ValueError("projected zero is missing command 255")
|
||||
point = np.median(zero_points, axis=0)
|
||||
inward = centre - point
|
||||
if float(np.linalg.norm(inward)) < 1.0e-9:
|
||||
raise ValueError("projected zero point is degenerate")
|
||||
angles.append(
|
||||
wrap_angle_rad(math.atan2(-float(inward[1]), float(inward[0])))
|
||||
)
|
||||
if maximum_pairwise_angle_difference_rad(angles) > maximum_round_difference_rad:
|
||||
raise ValueError("projected_zero_round_difference_too_large")
|
||||
return round(float(circular_median_rad(angles)), 8)
|
||||
|
||||
|
||||
def build_compact_payload(
|
||||
*,
|
||||
serial_number: str,
|
||||
measured_fits: Mapping[str, JointCurveFit],
|
||||
urdf_zero_offsets_rad: Mapping[str, float],
|
||||
validation_errors_rad: Sequence[float],
|
||||
passed: bool,
|
||||
baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
side: str = "left",
|
||||
) -> dict[str, Any]:
|
||||
profile = get_hand_calibration_profile(side)
|
||||
if set(measured_fits) != set(profile.measured_joints):
|
||||
raise ValueError("measured_fits must contain all directly measured joints")
|
||||
expected_active = {
|
||||
name for name, spec in profile.joint_specs.items() if spec.active
|
||||
}
|
||||
if set(urdf_zero_offsets_rad) != expected_active:
|
||||
raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set")
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 commands")
|
||||
|
||||
joints: dict[str, dict[str, Any]] = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
source_name = spec.source_joint or name
|
||||
fit = measured_fits[source_name]
|
||||
joint: dict[str, Any] = {
|
||||
"motor_index": int(spec.motor_index),
|
||||
"angle_rad": [round(float(value), 8) for value in fit.angle_rad],
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = int(baseline[spec.motor_index])
|
||||
joint["zero_angles"] = {
|
||||
"urdf_zero_offset_rad": round(
|
||||
float(urdf_zero_offsets_rad[name]), 8
|
||||
)
|
||||
}
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
joints[name] = joint
|
||||
|
||||
errors = np.abs(np.asarray(validation_errors_rad, dtype=float))
|
||||
mae = float(np.mean(errors)) if errors.size else float("nan")
|
||||
p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan")
|
||||
payload = {
|
||||
"schema_version": 4,
|
||||
"model": "G20",
|
||||
"side": profile.side,
|
||||
"serial_number": str(serial_number),
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [int(value) for value in baseline],
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": bool(passed),
|
||||
"validation_mae_rad": (
|
||||
None if not math.isfinite(mae) else round(mae, 8)
|
||||
),
|
||||
"validation_p95_rad": (
|
||||
None if not math.isfinite(p95) else round(p95, 8)
|
||||
),
|
||||
},
|
||||
}
|
||||
validate_compact_payload(payload)
|
||||
return payload
|
||||
|
||||
|
||||
def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
expected_top = {
|
||||
"schema_version", "model", "side", "serial_number", "angle_unit",
|
||||
"command_range", "baseline_command_u8", "joints", "quality",
|
||||
}
|
||||
if set(payload) != expected_top:
|
||||
raise ValueError("compact calibration has unexpected top-level fields")
|
||||
if payload["schema_version"] != 4:
|
||||
raise ValueError("schema_version must be 4")
|
||||
if payload["model"] != "G20" or payload["side"] not in {"left", "right"}:
|
||||
raise ValueError("payload must describe a left or right G20")
|
||||
profile = get_hand_calibration_profile(str(payload["side"]))
|
||||
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
|
||||
raise ValueError("payload angle or command units are invalid")
|
||||
baseline = payload["baseline_command_u8"]
|
||||
if not isinstance(baseline, list) or len(baseline) != 20:
|
||||
raise ValueError("baseline_command_u8 must contain 20 values")
|
||||
joints = payload["joints"]
|
||||
if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs):
|
||||
raise ValueError("payload must contain exactly 21 G20 joints")
|
||||
|
||||
for name, spec in profile.joint_specs.items():
|
||||
joint = joints[name]
|
||||
allowed = {"motor_index", "angle_rad"}
|
||||
allowed.add("zero_command_u8" if spec.active else "passive")
|
||||
if spec.active:
|
||||
allowed.add("zero_angles")
|
||||
if spec.source_joint is not None:
|
||||
allowed.add("source_joint")
|
||||
if set(joint) != allowed:
|
||||
raise ValueError(f"{name} has unexpected fields")
|
||||
if int(joint["motor_index"]) != spec.motor_index:
|
||||
raise ValueError(f"{name} has the wrong motor_index")
|
||||
curve = np.asarray(joint["angle_rad"], dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"{name}.angle_rad must contain 256 finite values")
|
||||
if np.any(np.diff(curve) > 1.0e-7):
|
||||
raise ValueError(f"{name}.angle_rad must be non-increasing")
|
||||
if spec.active:
|
||||
zero = joint["zero_command_u8"]
|
||||
if not isinstance(zero, int) or not 0 <= zero <= 255:
|
||||
raise ValueError(f"{name}.zero_command_u8 is invalid")
|
||||
zero_angles = joint.get("zero_angles")
|
||||
if not isinstance(zero_angles, Mapping) or set(zero_angles) != {
|
||||
"urdf_zero_offset_rad"
|
||||
}:
|
||||
raise ValueError(
|
||||
f"{name}.zero_angles must contain urdf_zero_offset_rad"
|
||||
)
|
||||
offset = float(zero_angles["urdf_zero_offset_rad"])
|
||||
if not math.isfinite(offset):
|
||||
raise ValueError(f"{name}.urdf_zero_offset_rad is invalid")
|
||||
if abs(float(curve[zero])) > 1.0e-6:
|
||||
raise ValueError(
|
||||
f"{name}.angle_rad must be zero at zero_command_u8"
|
||||
)
|
||||
elif joint.get("passive") is not True:
|
||||
raise ValueError(f"{name} must be marked passive")
|
||||
if spec.source_joint is not None:
|
||||
if joint.get("source_joint") != spec.source_joint:
|
||||
raise ValueError(f"{name} has the wrong source_joint")
|
||||
source = joints[spec.source_joint]
|
||||
if joint["angle_rad"] != source["angle_rad"]:
|
||||
raise ValueError(f"{name} must copy its source curve exactly")
|
||||
if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]:
|
||||
raise ValueError(f"{name} must copy its source zero command")
|
||||
# source_joint denotes a dynamic command-angle curve source. The
|
||||
# absolute URDF zero belongs to an independent motor/assembly and
|
||||
# must remain per-joint unless it was independently observed.
|
||||
|
||||
quality = payload["quality"]
|
||||
if set(quality) != {
|
||||
"passed", "validation_mae_rad", "validation_p95_rad"
|
||||
}:
|
||||
raise ValueError("quality must contain only compact summary fields")
|
||||
@@ -1,846 +0,0 @@
|
||||
"""Safely replay a complete three-camera session without moving the hand."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from collections import defaultdict
|
||||
from dataclasses import replace
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import tempfile
|
||||
from typing import Any, Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
import yaml
|
||||
|
||||
from .extrinsics import load_three_camera_extrinsics
|
||||
from .full_hand import (
|
||||
HandCalibrationProfile,
|
||||
JointCurveFit,
|
||||
build_calibration_motion_command,
|
||||
build_compact_payload,
|
||||
get_hand_calibration_profile,
|
||||
validate_compact_payload,
|
||||
)
|
||||
from .storage import atomic_write_json
|
||||
from .urdf_zero import (
|
||||
JointAxisMeasurement,
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
fit_joint_axis_measurement,
|
||||
fit_rotation_joint_curve,
|
||||
get_zero_calibration_profile,
|
||||
rotation_curve_holdout_errors,
|
||||
solve_urdf_zero_offsets,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
|
||||
|
||||
def _sha256(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _output_suffix(output_tag: str | None) -> str:
|
||||
"""Return a filename-safe suffix for a non-destructive replay variant."""
|
||||
if output_tag is None:
|
||||
return ""
|
||||
tag = str(output_tag)
|
||||
if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag):
|
||||
raise ValueError(
|
||||
"output tag must be 1-64 filename-safe characters, beginning "
|
||||
"with a letter or digit"
|
||||
)
|
||||
return f"_{tag}"
|
||||
|
||||
|
||||
def _load_parameters(path: Path) -> dict[str, Any]:
|
||||
payload = yaml.safe_load(path.read_text(encoding="utf-8"))
|
||||
return dict(payload["g20_calibration"]["ros__parameters"])
|
||||
|
||||
|
||||
def _latest_attempt_records(
|
||||
rows: Sequence[Mapping[str, Any]],
|
||||
) -> dict[str, list[dict[str, Any]]]:
|
||||
"""Reproduce the online retry buffer from append-only raw samples.
|
||||
|
||||
Online retry clears only the failed joint/cycle/direction from memory,
|
||||
while JSONL deliberately retains every attempt for audit. Offline replay
|
||||
must therefore select the greatest attempt independently for each logical
|
||||
trajectory rather than mixing rejected attempts into the final fit.
|
||||
"""
|
||||
samples = [dict(row) for row in rows if row.get("kind") == "sample"]
|
||||
latest_attempt: dict[tuple[str, int, str], int] = {}
|
||||
for row in samples:
|
||||
key = (
|
||||
str(row["joint"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
latest_attempt[key] = max(
|
||||
latest_attempt.get(key, 0), int(row.get("attempt", 1))
|
||||
)
|
||||
records: dict[str, list[dict[str, Any]]] = defaultdict(list)
|
||||
for row in samples:
|
||||
key = (
|
||||
str(row["joint"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
if int(row.get("attempt", 1)) == latest_attempt[key]:
|
||||
records[key[0]].append(row)
|
||||
return dict(records)
|
||||
|
||||
|
||||
def _load_raw_session(
|
||||
session_dir: Path,
|
||||
) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]:
|
||||
raw_path = session_dir / "raw_samples.jsonl"
|
||||
if not raw_path.is_file():
|
||||
raise ValueError(f"raw session does not exist: {raw_path}")
|
||||
rows = [
|
||||
json.loads(line)
|
||||
for line in raw_path.read_text(encoding="utf-8").splitlines()
|
||||
if line.strip()
|
||||
]
|
||||
starts = [row for row in rows if row.get("kind") == "session_start"]
|
||||
if len(starts) != 1:
|
||||
raise ValueError("raw session must contain exactly one session_start")
|
||||
return starts[0], _latest_attempt_records(rows), raw_path
|
||||
|
||||
|
||||
def _fit_curve(
|
||||
name: str,
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
profile: HandCalibrationProfile,
|
||||
baseline: Sequence[int],
|
||||
) -> JointCurveFit:
|
||||
motor = profile.joint_specs[name].motor_index
|
||||
return fit_rotation_joint_curve(
|
||||
records,
|
||||
zero_command_u8=int(baseline[motor]),
|
||||
)
|
||||
|
||||
|
||||
def _fit_axes(
|
||||
*,
|
||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
profile: HandCalibrationProfile,
|
||||
baseline: Sequence[int],
|
||||
extrinsics_file: Path,
|
||||
repetitions: int,
|
||||
) -> list[JointAxisMeasurement]:
|
||||
zero_profile = get_zero_calibration_profile(profile.side)
|
||||
extrinsics = load_three_camera_extrinsics(extrinsics_file)
|
||||
cache: dict[tuple[str, int], JointAxisMeasurement] = {}
|
||||
reference = profile.reference_finger
|
||||
upstream_by_joint = {
|
||||
"thumb_mcp": "thumb_cmc_pitch",
|
||||
"thumb_ip": "thumb_mcp",
|
||||
f"{reference}_pip": f"{reference}_mcp_pitch",
|
||||
f"{reference}_dip": f"{reference}_pip",
|
||||
}
|
||||
|
||||
def fit_one(name: str, cycle: int) -> JointAxisMeasurement:
|
||||
key = (name, cycle)
|
||||
if key in cache:
|
||||
return cache[key]
|
||||
upstream = upstream_by_joint.get(name)
|
||||
constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz
|
||||
spec = profile.joint_specs[name]
|
||||
view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray(
|
||||
[0.0, 0.0, 1.0], dtype=float
|
||||
)
|
||||
result = fit_joint_axis_measurement(
|
||||
name,
|
||||
records_by_joint[name],
|
||||
cycle=cycle,
|
||||
zero_command_u8=int(baseline[spec.motor_index]),
|
||||
axis_common_constraint=constraint,
|
||||
constrained_circle_joints=zero_profile.constrained_circle_joints,
|
||||
view_normal_common_xyz=view_normal,
|
||||
)
|
||||
condition = build_calibration_motion_command(
|
||||
spec,
|
||||
int(baseline[spec.motor_index]),
|
||||
baseline=baseline,
|
||||
profile=profile,
|
||||
)
|
||||
result = replace(
|
||||
result,
|
||||
condition_command_u8=tuple(float(value) for value in condition),
|
||||
view_normal_common_xyz=tuple(float(value) for value in view_normal),
|
||||
)
|
||||
cache[key] = result
|
||||
return result
|
||||
|
||||
return [
|
||||
fit_one(name, cycle)
|
||||
for name in zero_profile.axis_joints
|
||||
for cycle in range(repetitions)
|
||||
]
|
||||
|
||||
|
||||
def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
|
||||
maximum = 0.0
|
||||
for left in axes:
|
||||
for right in axes:
|
||||
maximum = max(
|
||||
maximum,
|
||||
math.acos(
|
||||
abs(
|
||||
float(
|
||||
np.clip(
|
||||
np.asarray(left, dtype=float)
|
||||
@ np.asarray(right, dtype=float),
|
||||
-1.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
)
|
||||
),
|
||||
)
|
||||
return maximum
|
||||
|
||||
|
||||
def _quality_failures(
|
||||
*,
|
||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
profile: HandCalibrationProfile,
|
||||
baseline: Sequence[int],
|
||||
fits: Mapping[str, JointCurveFit],
|
||||
axes: Sequence[JointAxisMeasurement],
|
||||
parameters: Mapping[str, Any],
|
||||
) -> list[str]:
|
||||
failures: list[str] = []
|
||||
repetitions = int(parameters["repetitions"])
|
||||
zero_profile = get_zero_calibration_profile(profile.side)
|
||||
axis_by_key = {(item.joint, item.cycle): item for item in axes}
|
||||
expected_directions = {"decreasing", "increasing"}
|
||||
for name in profile.measured_joints:
|
||||
records = list(records_by_joint.get(name, ()))
|
||||
if not records:
|
||||
failures.append(f"{name}: no samples")
|
||||
continue
|
||||
attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set)
|
||||
for record in records:
|
||||
attempts_by_direction[
|
||||
(int(record["cycle"]), str(record["direction"]))
|
||||
].add(int(record.get("attempt", 1)))
|
||||
for cycle in range(repetitions):
|
||||
for direction in expected_directions:
|
||||
selected = [
|
||||
record
|
||||
for record in records
|
||||
if int(record["cycle"]) == cycle
|
||||
and str(record["direction"]) == direction
|
||||
]
|
||||
attempts = attempts_by_direction.get((cycle, direction), set())
|
||||
if len(attempts) != 1:
|
||||
failures.append(
|
||||
f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}"
|
||||
)
|
||||
continue
|
||||
commands = sorted({int(record["command_u8"]) for record in selected})
|
||||
if len(selected) < int(parameters["minimum_sweep_frames"]):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames")
|
||||
if not commands or max(commands) - min(commands) < float(
|
||||
parameters["minimum_state_span_u8"]
|
||||
):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span")
|
||||
if len(commands) < int(parameters["minimum_sweep_bins"]):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins")
|
||||
if 0 not in commands or 255 not in commands:
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing")
|
||||
if commands and max(np.diff(commands), default=0) > int(
|
||||
parameters["maximum_bin_gap"]
|
||||
):
|
||||
failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap")
|
||||
|
||||
sync_p95 = float(
|
||||
np.percentile(
|
||||
[float(record.get("state_image_sync_error_ms", 0.0)) for record in records],
|
||||
95.0,
|
||||
)
|
||||
)
|
||||
if sync_p95 > float(parameters["maximum_state_image_skew_ms"]):
|
||||
failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms")
|
||||
spec = profile.joint_specs[name]
|
||||
fit = fits[name]
|
||||
orthogonal_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"active_maximum_rotation_orthogonal_rms_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_rotation_orthogonal_rms_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit:
|
||||
failures.append(f"{name}: rotation orthogonal RMS")
|
||||
if float(fit.quality["arc_rad"]) < math.radians(
|
||||
float(parameters["trajectory_minimum_arc_deg"])
|
||||
):
|
||||
failures.append(f"{name}: trajectory arc")
|
||||
monotonic_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"maximum_monotonic_correction_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_monotonic_correction_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
hysteresis_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"maximum_hysteresis_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_hysteresis_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
if fit.maximum_monotonic_correction_rad > monotonic_limit:
|
||||
failures.append(f"{name}: monotonic correction")
|
||||
if fit.maximum_hysteresis_rad > hysteresis_limit:
|
||||
failures.append(f"{name}: hysteresis")
|
||||
|
||||
cycle_travels: list[float] = []
|
||||
cycle_axes: list[Sequence[float]] = []
|
||||
cycle_axis_sources: list[str] = []
|
||||
for cycle in range(repetitions):
|
||||
cycle_fit = _fit_curve(
|
||||
name,
|
||||
[record for record in records if int(record["cycle"]) == cycle],
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
)
|
||||
cycle_travels.append(
|
||||
abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255]))
|
||||
)
|
||||
axis = axis_by_key[(name, cycle)]
|
||||
cycle_axes.append(axis.axis_common_xyz)
|
||||
cycle_axis_sources.append(axis.axis_direction_source)
|
||||
if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
|
||||
failures.append(f"{name} cycle {cycle + 1}: radial RMS")
|
||||
if axis.pose_axis_line_rms_m > float(
|
||||
parameters["axis_maximum_pose_line_rms_m"]
|
||||
):
|
||||
failures.append(
|
||||
f"{name} cycle {cycle + 1}: pose axis-line RMS"
|
||||
)
|
||||
if name not in zero_profile.constrained_circle_joints:
|
||||
plane_limit = float(
|
||||
parameters[
|
||||
"axis_maximum_plane_rms_m"
|
||||
if spec.active
|
||||
else "passive_axis_maximum_plane_rms_m"
|
||||
]
|
||||
)
|
||||
if axis.plane_rms_m > plane_limit:
|
||||
failures.append(f"{name} cycle {cycle + 1}: plane RMS")
|
||||
if (
|
||||
name not in zero_profile.constrained_circle_joints
|
||||
and axis.rotation_circle_axis_difference_rad > math.radians(
|
||||
float(parameters["axis_maximum_rotation_circle_difference_deg"])
|
||||
)
|
||||
):
|
||||
failures.append(f"{name} cycle {cycle + 1}: axis disagreement")
|
||||
travel_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"trajectory_maximum_cycle_travel_difference_deg"
|
||||
if spec.active
|
||||
else "passive_maximum_cycle_travel_difference_deg"
|
||||
]
|
||||
)
|
||||
)
|
||||
if max(cycle_travels) - min(cycle_travels) > travel_limit:
|
||||
failures.append(f"{name}: cycle travel difference")
|
||||
if (
|
||||
not all(
|
||||
source == "upstream_constraint"
|
||||
for source in cycle_axis_sources
|
||||
)
|
||||
and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
|
||||
float(parameters["zero_maximum_axis_cycle_difference_deg"])
|
||||
)
|
||||
):
|
||||
failures.append(f"{name}: cycle axis difference")
|
||||
return failures
|
||||
|
||||
|
||||
def _joint_xml(path: Path) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(path).getroot().findall("joint")
|
||||
}
|
||||
|
||||
|
||||
def _triplet(value: str) -> np.ndarray:
|
||||
return np.asarray([float(item) for item in value.split()], dtype=float)
|
||||
|
||||
|
||||
def _validate_corrected_urdf(
|
||||
*,
|
||||
source: Path,
|
||||
corrected: Path,
|
||||
offsets: Mapping[str, float],
|
||||
axes: Sequence[JointAxisMeasurement],
|
||||
curves: Mapping[str, JointCurveFit],
|
||||
motor_by_joint: Mapping[str, int],
|
||||
inherited_zero_joints: Mapping[str, str],
|
||||
) -> dict[str, float]:
|
||||
source_joints = _joint_xml(source)
|
||||
corrected_joints = _joint_xml(corrected)
|
||||
if set(source_joints) != set(corrected_joints):
|
||||
raise ValueError("corrected URDF changed the joint set")
|
||||
maximum_origin_rotation_error = 0.0
|
||||
maximum_origin_translation_error = 0.0
|
||||
for name, original_joint in source_joints.items():
|
||||
corrected_joint = corrected_joints[name]
|
||||
original_origin = original_joint.find("origin")
|
||||
corrected_origin = corrected_joint.find("origin")
|
||||
if original_origin is None or corrected_origin is None:
|
||||
continue
|
||||
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
|
||||
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
|
||||
maximum_origin_translation_error = max(
|
||||
maximum_origin_translation_error,
|
||||
float(np.linalg.norm(corrected_xyz - original_xyz)),
|
||||
)
|
||||
original_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
corrected_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
expected = original_rotation
|
||||
if name in offsets:
|
||||
axis_node = original_joint.find("axis")
|
||||
axis = _triplet(
|
||||
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
|
||||
)
|
||||
axis /= np.linalg.norm(axis)
|
||||
expected = original_rotation * Rotation.from_rotvec(
|
||||
axis * float(offsets[name])
|
||||
)
|
||||
error = float((expected.inv() * corrected_rotation).magnitude())
|
||||
maximum_origin_rotation_error = max(maximum_origin_rotation_error, error)
|
||||
original_limit = original_joint.find("limit")
|
||||
corrected_limit = corrected_joint.find("limit")
|
||||
if original_limit is not None and corrected_limit is not None:
|
||||
if (
|
||||
original_limit.get("lower") != corrected_limit.get("lower")
|
||||
or original_limit.get("upper") != corrected_limit.get("upper")
|
||||
):
|
||||
raise ValueError(f"corrected URDF unexpectedly changed {name} limits")
|
||||
if maximum_origin_translation_error > 1.0e-12:
|
||||
raise ValueError("corrected URDF changed a joint origin translation")
|
||||
if maximum_origin_rotation_error > 1.0e-10:
|
||||
raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)")
|
||||
original_model = UrdfKinematicModel(source)
|
||||
corrected_model = UrdfKinematicModel(corrected)
|
||||
maximum_axis_error = 0.0
|
||||
maximum_point_error = 0.0
|
||||
for measurement in axes:
|
||||
state = (
|
||||
measurement.condition_state_u8
|
||||
if measurement.condition_command_u8 is None
|
||||
else measurement.condition_command_u8
|
||||
)
|
||||
angles = _angles_from_state(
|
||||
state,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=inherited_zero_joints,
|
||||
)
|
||||
expected_axis, expected_point = original_model.axis_line(
|
||||
measurement.joint,
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
actual_axis, actual_point = corrected_model.axis_line(
|
||||
measurement.joint,
|
||||
zero_offsets={},
|
||||
joint_angles=angles,
|
||||
)
|
||||
maximum_axis_error = max(
|
||||
maximum_axis_error,
|
||||
math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))),
|
||||
)
|
||||
maximum_point_error = max(
|
||||
maximum_point_error,
|
||||
float(np.linalg.norm(expected_point - actual_point)),
|
||||
)
|
||||
if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10:
|
||||
raise ValueError("written URDF kinematics differ from the solved correction")
|
||||
return {
|
||||
"maximum_origin_rotation_error_rad": maximum_origin_rotation_error,
|
||||
"maximum_origin_translation_error_m": maximum_origin_translation_error,
|
||||
"maximum_axis_equivalence_error_rad": maximum_axis_error,
|
||||
"maximum_axis_point_equivalence_error_m": maximum_point_error,
|
||||
}
|
||||
|
||||
|
||||
def replay_session(
|
||||
session_dir: str | Path,
|
||||
*,
|
||||
serial_number: str | None = None,
|
||||
config_file: str | Path | None = None,
|
||||
write_outputs: bool = False,
|
||||
output_tag: str | None = None,
|
||||
) -> dict[str, Any]:
|
||||
session = Path(session_dir).expanduser().resolve()
|
||||
package_root = Path(__file__).resolve().parents[1]
|
||||
config = (
|
||||
package_root / "config" / "three_camera_calibration.yaml"
|
||||
if config_file is None
|
||||
else Path(config_file).expanduser().resolve()
|
||||
)
|
||||
parameters = _load_parameters(config)
|
||||
start, records_by_joint, raw_path = _load_raw_session(session)
|
||||
side = str(start["hand_type"]).lower()
|
||||
profile = get_hand_calibration_profile(side)
|
||||
zero_profile = get_zero_calibration_profile(side)
|
||||
baseline = tuple(int(value) for value in start["baseline_command_u8"])
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("session baseline must contain exactly 20 commands")
|
||||
if set(records_by_joint) != set(profile.measured_joints):
|
||||
raise ValueError("raw session does not contain exactly the measured joint set")
|
||||
source_urdf = Path(start["source_urdf_path"]).expanduser().resolve()
|
||||
extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve()
|
||||
if not source_urdf.is_file() or not extrinsics_file.is_file():
|
||||
raise ValueError("session source URDF or camera extrinsics is missing")
|
||||
if "zero_calibrated" in source_urdf.stem.lower():
|
||||
raise ValueError("offline replay requires the original CAD URDF")
|
||||
hand_serial = str(serial_number or session.parent.name)
|
||||
output_suffix = _output_suffix(output_tag)
|
||||
repetitions = int(parameters["repetitions"])
|
||||
|
||||
measured_fits = {
|
||||
name: _fit_curve(
|
||||
name,
|
||||
records_by_joint[name],
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
)
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
training_fits = {
|
||||
name: _fit_curve(
|
||||
name,
|
||||
[
|
||||
record
|
||||
for record in records_by_joint[name]
|
||||
if int(record["cycle"]) in {0, 1}
|
||||
],
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
)
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
axes = _fit_axes(
|
||||
records_by_joint=records_by_joint,
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
extrinsics_file=extrinsics_file,
|
||||
repetitions=repetitions,
|
||||
)
|
||||
failures = _quality_failures(
|
||||
records_by_joint=records_by_joint,
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
fits=measured_fits,
|
||||
axes=axes,
|
||||
parameters=parameters,
|
||||
)
|
||||
if failures:
|
||||
raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures))
|
||||
|
||||
holdout_by_joint = {
|
||||
name: rotation_curve_holdout_errors(
|
||||
training_fits[name],
|
||||
[
|
||||
record
|
||||
for record in records_by_joint[name]
|
||||
if int(record["cycle"]) == 2
|
||||
],
|
||||
zero_command_u8=int(
|
||||
baseline[profile.joint_specs[name].motor_index]
|
||||
),
|
||||
)
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
trajectory_errors = np.abs(
|
||||
np.asarray(
|
||||
[value for values in holdout_by_joint.values() for value in values],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
maximum_validation_mae = math.radians(
|
||||
float(parameters["maximum_validation_mae_deg"])
|
||||
)
|
||||
maximum_validation_p95 = math.radians(
|
||||
float(parameters["maximum_validation_p95_deg"])
|
||||
)
|
||||
trajectory_mae = float(np.mean(trajectory_errors))
|
||||
trajectory_p95 = float(np.percentile(trajectory_errors, 95.0))
|
||||
if (
|
||||
trajectory_mae > maximum_validation_mae
|
||||
or trajectory_p95 > maximum_validation_p95
|
||||
):
|
||||
raise ValueError("third-cycle trajectory holdout failed")
|
||||
|
||||
motor_by_joint = {
|
||||
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
|
||||
}
|
||||
joint_limits: dict[str, float] = {}
|
||||
solve_arguments = {
|
||||
"source_urdf": source_urdf,
|
||||
"measurements": axes,
|
||||
"motor_by_joint": motor_by_joint,
|
||||
"maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
|
||||
"finger_maximum_offset_rad": math.radians(
|
||||
float(parameters.get("zero_finger_maximum_offset_deg", 3.0))
|
||||
),
|
||||
"joint_maximum_offset_rad": joint_limits,
|
||||
"maximum_cycle_difference_rad": math.radians(
|
||||
float(parameters["zero_maximum_axis_cycle_difference_deg"])
|
||||
),
|
||||
"maximum_axis_cone_mismatch_rad": math.radians(
|
||||
float(parameters["zero_maximum_axis_cone_mismatch_deg"])
|
||||
),
|
||||
"maximum_pose_axis_line_rms_m": float(
|
||||
parameters["axis_maximum_pose_line_rms_m"]
|
||||
),
|
||||
"maximum_validation_mae_rad": maximum_validation_mae,
|
||||
"maximum_validation_p95_rad": maximum_validation_p95,
|
||||
"hand_type": side,
|
||||
}
|
||||
holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
|
||||
if not holdout_zero.passed:
|
||||
failure = {
|
||||
"reasons": dict(holdout_zero.failure_reasons),
|
||||
"fitted_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.direct_offsets_rad.items()
|
||||
},
|
||||
"cycle_offsets_deg": {
|
||||
name: [math.degrees(value) for value in values]
|
||||
for name, values in holdout_zero.cycle_offsets_rad.items()
|
||||
},
|
||||
}
|
||||
raise ValueError(
|
||||
"third-cycle zero/URDF holdout failed: "
|
||||
+ json.dumps(failure, ensure_ascii=False, sort_keys=True)
|
||||
)
|
||||
final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments)
|
||||
if not final_zero.passed:
|
||||
raise ValueError(
|
||||
"all-cycle zero refit failed: "
|
||||
+ json.dumps(
|
||||
{
|
||||
"reasons": dict(final_zero.failure_reasons),
|
||||
"fitted_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in final_zero.direct_offsets_rad.items()
|
||||
},
|
||||
},
|
||||
ensure_ascii=False,
|
||||
sort_keys=True,
|
||||
)
|
||||
)
|
||||
for target, source_name in zero_profile.inherited_static_zero_joints.items():
|
||||
if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]:
|
||||
raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}")
|
||||
for target in (
|
||||
set(zero_profile.inherited_zero_joints)
|
||||
- set(zero_profile.inherited_static_zero_joints)
|
||||
):
|
||||
if final_zero.all_active_offsets_rad[target] != 0.0:
|
||||
raise ValueError(f"unobserved static zero must retain source CAD: {target}")
|
||||
|
||||
validation_errors = [
|
||||
float(value) for values in holdout_by_joint.values() for value in values
|
||||
]
|
||||
validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad)
|
||||
payload = build_compact_payload(
|
||||
serial_number=hand_serial,
|
||||
measured_fits=measured_fits,
|
||||
urdf_zero_offsets_rad=final_zero.all_active_offsets_rad,
|
||||
validation_errors_rad=validation_errors,
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
side=side,
|
||||
)
|
||||
validate_compact_payload(payload)
|
||||
|
||||
stamp = session.name
|
||||
final_json = session / (
|
||||
f"g20_{side}_{hand_serial}_calibration{output_suffix}.json"
|
||||
)
|
||||
expected_urdf_name = (
|
||||
f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}"
|
||||
f"{output_suffix}.urdf"
|
||||
)
|
||||
final_urdf = source_urdf.parent / expected_urdf_name
|
||||
report_path = session / (
|
||||
f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json"
|
||||
)
|
||||
if write_outputs:
|
||||
existing = [path for path in (final_json, final_urdf, report_path) if path.exists()]
|
||||
if existing:
|
||||
raise ValueError(
|
||||
"refusing to overwrite replay outputs: "
|
||||
+ ", ".join(str(path) for path in existing)
|
||||
)
|
||||
|
||||
source_hash_before = _sha256(source_urdf)
|
||||
with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary:
|
||||
candidate = write_zero_corrected_urdf(
|
||||
source_urdf=source_urdf,
|
||||
output_directory=temporary,
|
||||
serial_number=hand_serial,
|
||||
offsets_rad=final_zero.all_active_offsets_rad,
|
||||
timestamp=stamp,
|
||||
)
|
||||
urdf_checks = _validate_corrected_urdf(
|
||||
source=source_urdf,
|
||||
corrected=candidate,
|
||||
offsets=final_zero.all_active_offsets_rad,
|
||||
axes=axes,
|
||||
curves=measured_fits,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero_profile.inherited_zero_joints,
|
||||
)
|
||||
residual_zero = solve_urdf_zero_offsets(
|
||||
curves=measured_fits,
|
||||
fixed_direct_zero_offsets_rad={
|
||||
name: 0.0
|
||||
for name in zero_profile.fixed_direct_zero_offsets_rad
|
||||
},
|
||||
static_output_zero_offsets_rad={
|
||||
name: 0.0
|
||||
for name in zero_profile.static_output_zero_offsets_rad
|
||||
},
|
||||
**{**solve_arguments, "source_urdf": candidate},
|
||||
)
|
||||
maximum_residual_offset = max(
|
||||
abs(float(value)) for value in residual_zero.direct_offsets_rad.values()
|
||||
)
|
||||
if not residual_zero.passed or maximum_residual_offset > math.radians(0.3):
|
||||
raise ValueError("written URDF retains a significant zero correction")
|
||||
candidate_hash = _sha256(candidate)
|
||||
if write_outputs:
|
||||
os.replace(candidate, final_urdf)
|
||||
|
||||
if _sha256(source_urdf) != source_hash_before:
|
||||
raise ValueError("source URDF changed during offline replay")
|
||||
|
||||
report: dict[str, Any] = {
|
||||
"passed": True,
|
||||
"session_dir": str(session),
|
||||
"side": side,
|
||||
"serial_number": hand_serial,
|
||||
"output_tag": output_tag,
|
||||
"raw_samples_sha256": _sha256(raw_path),
|
||||
"source_urdf": str(source_urdf),
|
||||
"source_urdf_sha256": source_hash_before,
|
||||
"joint_limits_deg": {
|
||||
"finger_default": float(
|
||||
parameters.get("zero_finger_maximum_offset_deg", 3.0)
|
||||
),
|
||||
"thumb_default": float(parameters["zero_maximum_offset_deg"]),
|
||||
},
|
||||
"static_zero_policy": "direct_measurements_only",
|
||||
"fixed_zero_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in zero_profile.fixed_direct_zero_offsets_rad.items()
|
||||
},
|
||||
"static_output_zero_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in zero_profile.static_output_zero_offsets_rad.items()
|
||||
},
|
||||
"direct_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in final_zero.direct_offsets_rad.items()
|
||||
},
|
||||
"cycle_offsets_deg": {
|
||||
name: [math.degrees(value) for value in values]
|
||||
for name, values in holdout_zero.cycle_offsets_rad.items()
|
||||
},
|
||||
"offset_uncertainty_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.offset_uncertainty_rad.items()
|
||||
},
|
||||
"trajectory_holdout_mae_deg": math.degrees(trajectory_mae),
|
||||
"trajectory_holdout_p95_deg": math.degrees(trajectory_p95),
|
||||
"zero_holdout_error_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.validation_error_by_joint_rad.items()
|
||||
},
|
||||
"zero_original_error_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.validation_original_error_by_joint_rad.items()
|
||||
},
|
||||
"zero_improvement_95pct_lower_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in (
|
||||
holdout_zero.validation_improvement_confidence_lower_rad.items()
|
||||
)
|
||||
},
|
||||
"corrected_urdf_checks": {
|
||||
**urdf_checks,
|
||||
"maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
|
||||
},
|
||||
"corrected_urdf_sha256": candidate_hash,
|
||||
"final_json": str(final_json) if write_outputs else None,
|
||||
"corrected_urdf": str(final_urdf) if write_outputs else None,
|
||||
}
|
||||
if write_outputs:
|
||||
atomic_write_json(final_json, payload)
|
||||
report["final_json_sha256"] = _sha256(final_json)
|
||||
if _sha256(final_urdf) != candidate_hash:
|
||||
raise ValueError("formal corrected URDF differs from validated candidate")
|
||||
atomic_write_json(report_path, report)
|
||||
report["validation_report"] = str(report_path)
|
||||
return report
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Replay and independently validate a complete G20 calibration session."
|
||||
)
|
||||
parser.add_argument("session_dir")
|
||||
parser.add_argument("--serial-number", default=None)
|
||||
parser.add_argument("--config-file", default=None)
|
||||
parser.add_argument("--write", action="store_true")
|
||||
parser.add_argument(
|
||||
"--output-tag",
|
||||
default=None,
|
||||
help="safe suffix for a replay variant; existing outputs are never overwritten",
|
||||
)
|
||||
arguments = parser.parse_args()
|
||||
result = replay_session(
|
||||
arguments.session_dir,
|
||||
serial_number=arguments.serial_number,
|
||||
config_file=arguments.config_file,
|
||||
write_outputs=arguments.write,
|
||||
output_tag=arguments.output_tag,
|
||||
)
|
||||
print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
-481
@@ -1,481 +0,0 @@
|
||||
"""Chinese, operator-facing diagnostics for three-camera calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"RETURN_BASELINE": "正在恢复目标姿态",
|
||||
"PREPARE_SWEEP": "正在到达扫描起点",
|
||||
"SWEEP": "正在采集轨迹",
|
||||
"FITTING": "正在拟合轨迹和零位",
|
||||
"VALIDATION_MOVE": "正在移动到随机复测位置",
|
||||
"VALIDATION_CAPTURE": "正在采集随机复测数据",
|
||||
"PAUSED": "标定已暂停",
|
||||
"ABORTED": "标定已终止",
|
||||
"COMPLETE": "标定已完成",
|
||||
}
|
||||
|
||||
VIEW_NAMES_ZH = {
|
||||
"front": "正面",
|
||||
"side": "侧面",
|
||||
"top": "上面",
|
||||
}
|
||||
|
||||
JOINT_NAMES_ZH = {
|
||||
"thumb_cmc_pitch": "拇指CMC俯仰",
|
||||
"thumb_cmc_roll": "拇指CMC滚转",
|
||||
"thumb_mcp": "拇指MCP",
|
||||
"thumb_ip": "拇指IP(被动)",
|
||||
"index_mcp_roll": "食指MCP侧摆",
|
||||
"index_mcp_pitch": "食指MCP屈伸",
|
||||
"index_pip": "食指PIP",
|
||||
"index_dip": "食指DIP(被动)",
|
||||
"middle_mcp_roll": "中指MCP侧摆",
|
||||
"middle_mcp_pitch": "中指MCP屈伸",
|
||||
"middle_pip": "中指PIP",
|
||||
"middle_dip": "中指DIP(被动)",
|
||||
"ring_mcp_roll": "无名指MCP侧摆",
|
||||
"ring_mcp_pitch": "无名指MCP屈伸",
|
||||
"ring_pip": "无名指PIP",
|
||||
"ring_dip": "无名指DIP(被动)",
|
||||
"pinky_mcp_roll": "小指MCP侧摆",
|
||||
"pinky_mcp_pitch": "小指MCP屈伸",
|
||||
"pinky_pip": "小指PIP",
|
||||
"pinky_dip": "小指DIP(被动)",
|
||||
"thumb_cmc_yaw": "拇指CMC侧摆",
|
||||
}
|
||||
|
||||
|
||||
def _format_u8(value: Any) -> str:
|
||||
if value is None:
|
||||
return "尚无反馈"
|
||||
return f"{float(value):.1f}"
|
||||
|
||||
|
||||
def _task_text(active: Mapping[str, Any]) -> str:
|
||||
if not active:
|
||||
return "尚无活动任务"
|
||||
view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
|
||||
if active.get("kind") == "fit_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位,{joint_text}拟合检查失败,"
|
||||
f"电机{active.get('motor_index')},"
|
||||
f"第{active.get('attempt', 1)}次尝试"
|
||||
)
|
||||
if active.get("kind") == "zero_model_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位,{joint_text}零位/URDF验证失败,"
|
||||
f"电机{active.get('motor_index')},不会自动重扫"
|
||||
)
|
||||
if active.get("kind") == "motion_stall":
|
||||
return (
|
||||
f"电机{active.get('motor_index', '?')}运动停滞,目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}"
|
||||
)
|
||||
if active.get("kind") == "validation":
|
||||
return (
|
||||
f"{view}机位,随机复测,电机{active.get('motor_index')},"
|
||||
f"目标命令{active.get('command_u8')}"
|
||||
)
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
start = active.get("start_u8")
|
||||
target = active.get("target_u8")
|
||||
cycle = active.get("cycle", "?")
|
||||
repetitions = active.get("repetitions", "?")
|
||||
task = (
|
||||
f"{view}机位,{joint_text},电机{active.get('motor_index')},"
|
||||
f"第{cycle}/{repetitions}轮,{start}→{target}"
|
||||
)
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
if fit_attempt > 1:
|
||||
task += (
|
||||
f"(整关节自动重采第{fit_attempt}/"
|
||||
f"{active.get('fit_attempt_limit', '?')}次)"
|
||||
)
|
||||
return task
|
||||
|
||||
|
||||
def three_camera_reason_zh(
|
||||
state: str,
|
||||
reason: str,
|
||||
active: Mapping[str, Any],
|
||||
) -> tuple[str, str]:
|
||||
"""Translate a reason code and provide one concrete operator action."""
|
||||
reason = str(reason)
|
||||
sample = active.get("sample", {}) if active else {}
|
||||
missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
|
||||
sample_range = (
|
||||
f"{_format_u8(sample.get('minimum_u8'))}~"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
tolerance = sample.get("endpoint_tolerance_u8", "?")
|
||||
|
||||
if reason.startswith("motor_state_stalled:"):
|
||||
fields = reason.split(":")
|
||||
context = fields[1] if len(fields) > 1 else "unknown"
|
||||
error_match = re.search(r"error_u8=([0-9.]+)", reason)
|
||||
error = error_match.group(1) if error_match else "未知"
|
||||
motor = active.get("motor_index")
|
||||
if motor is not None:
|
||||
return (
|
||||
f"电机{motor}反馈连续8秒没有向目标推进;目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8,"
|
||||
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
|
||||
f"(阶段={context})。程序已保持当前位置。",
|
||||
"若实际反馈是稳定的固件端点,应只配置该电机该端点的专用容差后"
|
||||
"重启;若仍在变化或有摩擦,则先排查机械问题,不要反复resume强推。",
|
||||
)
|
||||
return (
|
||||
f"电机反馈连续8秒没有向目标推进;停止位置距目标{error}个u8"
|
||||
f"(阶段={context})。程序已保持当前位置,防止机械碰撞或摩擦加重。",
|
||||
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
|
||||
"正常端点,应配置该电机专用端点容差后重启标定;不要反复调用resume强推。",
|
||||
)
|
||||
|
||||
if "URDF zero offset reached the configured" in reason:
|
||||
bound_match = re.search(
|
||||
r"configured\s+([0-9.]+)\s+degree bound", reason
|
||||
)
|
||||
bound = bound_match.group(1) if bound_match else "配置的"
|
||||
hit_text = ""
|
||||
if "bound:" in reason:
|
||||
hit_text = reason.split("bound:", 1)[1].split(
|
||||
"; all_offsets:", 1
|
||||
)[0]
|
||||
for name, label in JOINT_NAMES_ZH.items():
|
||||
hit_text = hit_text.replace(name, label)
|
||||
hit_suffix = f";触边关节:{hit_text}" if hit_text else ""
|
||||
return (
|
||||
f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
|
||||
"零位结果,而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
|
||||
"不要调用resume,也不要增大零位边界。先确认Tag有效黑框边长、三相机"
|
||||
"内外参和原始CAD URDF;Tag尺寸修正后必须调用start重新采集,旧尺度"
|
||||
"产生的轨迹不能直接生成修正URDF。",
|
||||
)
|
||||
|
||||
if reason == "sweep_missing_endpoint_bin":
|
||||
missing_text = "、".join(str(value) for value in missing) or "0或255"
|
||||
return (
|
||||
f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据,但缺少"
|
||||
f"电机端点{missing_text}附近的有效分箱;采样到的实际电机范围为"
|
||||
f"{sample_range},端点容差为±{tolerance}。这通常表示电机虽然运动到"
|
||||
"端点,但该时刻没有同时取得有效Tag图像和电机状态。",
|
||||
"确认当前机位所需Tag在整个行程(尤其缺失端点)均可见,然后调用"
|
||||
"/g20_calibration/resume;程序会重新扫描当前方向,不要调用start。",
|
||||
)
|
||||
if reason == "sweep_bins_too_few":
|
||||
return (
|
||||
f"有效电机分箱只有{sample.get('bin_count', 0)}个,要求至少"
|
||||
f"{sample.get('minimum_bin_count', '?')}个;当前采样范围{sample_range}。",
|
||||
"检查Tag连续识别和电机状态频率,修正后调用resume重新扫描当前方向。",
|
||||
)
|
||||
if reason == "sweep_bin_gap_too_large":
|
||||
return (
|
||||
f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')},"
|
||||
f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
"检查运动中Tag是否间歇丢失;修正遮挡、反光或对焦后调用resume。",
|
||||
)
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
return (
|
||||
"运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
"按时间戳配对”的图像帧。",
|
||||
"查看下面活动机位的缺失Tag,确认状态话题仍在更新;修正后调用resume,"
|
||||
"程序会重扫当前方向。",
|
||||
)
|
||||
if reason == "sweep_start_position_timeout":
|
||||
return (
|
||||
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
|
||||
f"{active.get('start_u8')},当前实际值{_format_u8(active.get('actual_u8'))}。",
|
||||
"检查CAN、机械手使能和是否存在机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "sweep_timeout":
|
||||
return (
|
||||
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
|
||||
"检查电机实际值、Tag连续识别和标定速度,修正后调用resume。",
|
||||
)
|
||||
if reason == "return_baseline_timeout":
|
||||
return (
|
||||
"一个或多个标定电机未在规定时间返回基准命令。",
|
||||
"检查机械手状态、CAN和机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_move_timeout":
|
||||
return (
|
||||
"随机复测时电机未在规定时间到达目标命令。",
|
||||
"检查机械手状态和机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_capture_timeout":
|
||||
return (
|
||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||
"检查当前机位Tag可见性后调用resume。",
|
||||
)
|
||||
if reason == "joint_fit_check_failed":
|
||||
metric_names = {
|
||||
"plane_rms_mm": "平面拟合RMS",
|
||||
"radial_rms_mm": "圆半径拟合RMS",
|
||||
"radius_mm": "拟合半径",
|
||||
"image_radial_rms_px": "二维圆半径拟合RMS",
|
||||
"image_radial_p95_px": "二维圆半径误差P95",
|
||||
"image_radius_px": "二维拟合半径",
|
||||
"arc_deg": "实测圆弧",
|
||||
"monotonic_correction_deg": "最大单调修正",
|
||||
"hysteresis_deg": "最大正反程差",
|
||||
"cycle_travel_range_deg": "三轮行程差",
|
||||
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
|
||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||
"axis_radial_rms_mm": "三维圆半径RMS",
|
||||
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
||||
"axis_cycle_difference_deg": "三轮转轴方向极差",
|
||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
|
||||
"tag_valid_rate_percent": "所需Tag同时有效率",
|
||||
}
|
||||
metric_units = {
|
||||
"plane_rms_mm": "mm",
|
||||
"radial_rms_mm": "mm",
|
||||
"radius_mm": "mm",
|
||||
"image_radial_rms_px": "px",
|
||||
"image_radial_p95_px": "px",
|
||||
"image_radius_px": "px",
|
||||
"arc_deg": "°",
|
||||
"monotonic_correction_deg": "°",
|
||||
"hysteresis_deg": "°",
|
||||
"cycle_travel_range_deg": "°",
|
||||
"rotation_orthogonal_rms_deg": "°",
|
||||
"axis_plane_rms_mm": "mm",
|
||||
"axis_radial_rms_mm": "mm",
|
||||
"rotation_circle_axis_difference_deg": "°",
|
||||
"axis_cycle_difference_deg": "°",
|
||||
"third_cycle_axis_holdout_deg": "°",
|
||||
"third_cycle_axis_line_rms_mm": "mm",
|
||||
"third_cycle_trajectory_p95_deg": "°",
|
||||
"state_image_sync_p95_ms": "ms",
|
||||
"tag_valid_rate_percent": "%",
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
metric = str(failure.get("metric", ""))
|
||||
if metric in metric_names:
|
||||
comparison = str(failure.get("comparison", ""))
|
||||
requirement = "不超过" if comparison == "maximum" else "至少"
|
||||
unit = metric_units[metric]
|
||||
detail = (
|
||||
f"{joint}的{metric_names[metric]}为"
|
||||
f"{float(failure.get('actual', 0.0)):.2f}{unit},"
|
||||
f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
|
||||
)
|
||||
cycle_travel = failure.get("cycle_travel_deg", [])
|
||||
if cycle_travel:
|
||||
detail += "(三轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_travel
|
||||
) + ")"
|
||||
details.append(detail)
|
||||
else:
|
||||
cycle = failure.get("cycle")
|
||||
cycle_text = "" if cycle is None else f"第{cycle}轮"
|
||||
details.append(
|
||||
f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败:"
|
||||
f"{failure.get('reason', '未知原因')}"
|
||||
)
|
||||
detail_text = ";".join(details) or "当前关节的轨迹拟合未通过"
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
"修正Tag位置、遮挡或机械行程后调用"
|
||||
"/g20_calibration/resume;程序只清除当前失败关节的数据"
|
||||
f"并重扫{active.get('directions_to_rescan', 6)}个方向,不要调用start。",
|
||||
)
|
||||
if reason == "zero_model_validation_failed":
|
||||
reason_names = {
|
||||
"zero_offset_reached_configured_bound": "零位解触及安全边界",
|
||||
"zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
|
||||
"zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
|
||||
"zero_offset_cycle_difference_too_large": "三轮零位离散过大",
|
||||
"zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
|
||||
"zero_axis_cone_mismatch_too_large": (
|
||||
"父子轴夹角与原始URDF不一致,零位旋转无法解释"
|
||||
),
|
||||
"zero_phase_axis_line_residual_too_large": (
|
||||
"整段SE(3)运动无法稳定确定平行轴线相位"
|
||||
),
|
||||
"zero_offset_did_not_improve_with_95pct_confidence": (
|
||||
"第三轮留出验证未以95%置信度改善"
|
||||
),
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
if failure.get("metric") == "zero_guard":
|
||||
reason_text = reason_names.get(
|
||||
str(failure.get("reason")), str(failure.get("reason"))
|
||||
)
|
||||
if "actual_deg" in failure and "limit_deg" in failure:
|
||||
reason_text += (
|
||||
f"(估计{float(failure['actual_deg']):+.2f}°,"
|
||||
f"允许±{float(failure['limit_deg']):.2f}°)"
|
||||
)
|
||||
details.append(f"{joint}:{reason_text}")
|
||||
return (
|
||||
"轨迹采集已完成,但零位/URDF几何验证失败"
|
||||
+ ("(" + ";".join(details) + ")" if details else "")
|
||||
+ "。程序没有生成正式JSON或修正URDF。",
|
||||
"该类稳定模型失败不能靠重复运动修复,程序不会自动重扫;"
|
||||
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查;全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
return (
|
||||
"三机位预检已经通过,等待操作员确认开始。",
|
||||
"清空机械手运动范围后调用/g20_calibration/start。",
|
||||
)
|
||||
if reason == "operator_pause":
|
||||
return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
|
||||
if reason == "operator_abort":
|
||||
return "操作员终止了本次标定,程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
|
||||
if reason == "collecting_timestamp_synchronised_tag_centres":
|
||||
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作,保持相机、标签和底座不动。"
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作,保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成,但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
|
||||
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成,正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
|
||||
return f"未分类原因码:{reason}", "保留该原因码和启动终端日志用于进一步定位。"
|
||||
|
||||
|
||||
def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
"""Render the complete operator status; the JSON topic remains unchanged."""
|
||||
state = str(payload.get("state", ""))
|
||||
active = payload.get("active", {})
|
||||
reason_zh, action_zh = three_camera_reason_zh(
|
||||
state, str(payload.get("reason", "")), active
|
||||
)
|
||||
progress = float(payload.get("progress", 0.0))
|
||||
completed = payload.get("completed_sweeps", 0)
|
||||
total = payload.get("total_sweeps", 0)
|
||||
scan_progress = float(
|
||||
payload.get(
|
||||
"scan_progress",
|
||||
0.0 if not total else float(completed) / float(total),
|
||||
)
|
||||
)
|
||||
lines = [
|
||||
f"状态:{STATE_NAMES_ZH.get(state, state)}({state})",
|
||||
f"原因:{reason_zh}",
|
||||
f"建议:{action_zh}",
|
||||
f"总体进度:{progress:.1%}(计划扫描{completed}/{total}个方向,"
|
||||
f"扫描进度{scan_progress:.1%})",
|
||||
f"当前任务:{_task_text(active)}",
|
||||
]
|
||||
if state == "RETURN_BASELINE":
|
||||
baseline_command = payload.get("baseline_command_u8", [])
|
||||
return_command = payload.get("return_command_u8", baseline_command)
|
||||
label = "恢复姿态" if return_command != baseline_command else "基准姿态"
|
||||
lines.append(f"正在确认{label}:{return_command}")
|
||||
if active and active.get("kind") not in {
|
||||
"fit_failure",
|
||||
"zero_model_failure",
|
||||
"motion_stall",
|
||||
}:
|
||||
retry_count = int(active.get("automatic_retry_count", 0))
|
||||
if retry_count:
|
||||
lines.append(
|
||||
"自动重试:当前方向已自动重扫"
|
||||
f"{retry_count}/{active.get('automatic_retry_limit', '?')}次,"
|
||||
f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%},"
|
||||
f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
|
||||
)
|
||||
sample = active.get("sample", {})
|
||||
motion_progress = active.get("motion_progress")
|
||||
motion_text = (
|
||||
"未知" if motion_progress is None else f"{float(motion_progress):.1%}"
|
||||
)
|
||||
lines.append(
|
||||
"运动采样:"
|
||||
f"目标{active.get('target_u8', active.get('command_u8', '?'))},"
|
||||
f"实际{_format_u8(active.get('actual_u8'))},"
|
||||
f"本方向{motion_text},有效帧{active.get('valid_frames', 0)},"
|
||||
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}~"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
auxiliary = active.get("auxiliary_motors", [])
|
||||
if auxiliary:
|
||||
lines.append(
|
||||
"避挡姿态:"
|
||||
+ ",".join(
|
||||
f"电机{item.get('motor_index')}目标"
|
||||
f"{item.get('command_u8')}、实际"
|
||||
f"{_format_u8(item.get('actual_u8'))}"
|
||||
for item in auxiliary
|
||||
)
|
||||
)
|
||||
speed = active.get("speed", {})
|
||||
if speed:
|
||||
lines.append(
|
||||
"阶段速度:五指目标"
|
||||
f"{speed.get('commanded_finger_speed')},SDK报告"
|
||||
f"{speed.get('reported_finger_speed')}"
|
||||
)
|
||||
if active.get("sweep_timeout_seconds") is not None:
|
||||
lines.append(
|
||||
"运动保护:扫描超时"
|
||||
f"{float(active['sweep_timeout_seconds']):.1f}s,"
|
||||
"连续"
|
||||
f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
|
||||
"进展不足"
|
||||
f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
|
||||
"则立即暂停"
|
||||
)
|
||||
lines.append("机位:")
|
||||
for name, view in payload.get("views", {}).items():
|
||||
missing = view.get("missing_tag_ids", [])
|
||||
missing_text = "无" if not missing else ",".join(map(str, missing))
|
||||
lines.append(
|
||||
f"- {VIEW_NAMES_ZH.get(str(name), str(name))}:"
|
||||
f"{'就绪' if view.get('ready') else '等待'},"
|
||||
f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'},"
|
||||
f"{float(view.get('detection_hz', 0.0)):.1f}Hz,"
|
||||
f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%},"
|
||||
f"当前缺失Tag={missing_text}"
|
||||
)
|
||||
extrinsics_error = payload.get("camera_extrinsics_error")
|
||||
if extrinsics_error:
|
||||
lines.append(f"外参文件:{extrinsics_error}")
|
||||
lines.append(f"JSON结果:{payload.get('result_path') or '尚未生成'}")
|
||||
lines.append(
|
||||
f"修正URDF:{payload.get('corrected_urdf_path') or '尚未生成'}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
-3845
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/g20_thumb_apriltag_calibration
|
||||
[install]
|
||||
install_scripts=$base/lib/g20_thumb_apriltag_calibration
|
||||
@@ -1,69 +0,0 @@
|
||||
from glob import glob
|
||||
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
|
||||
package_name = "g20_thumb_apriltag_calibration"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=find_packages(),
|
||||
data_files=[
|
||||
(
|
||||
"share/ament_index/resource_index/packages",
|
||||
["resource/" + package_name],
|
||||
),
|
||||
("share/" + package_name, ["package.xml", "README.md"]),
|
||||
(
|
||||
"share/" + package_name + "/config",
|
||||
glob("config/*.yaml") + glob("config/*.xml"),
|
||||
),
|
||||
("share/" + package_name + "/launch", glob("launch/*.launch.py")),
|
||||
],
|
||||
install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
|
||||
tests_require=["pytest"],
|
||||
zip_safe=True,
|
||||
maintainer="lxp",
|
||||
maintainer_email="support@linker-robotics.com",
|
||||
description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand",
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
(
|
||||
"hikrobot_camera_node = "
|
||||
"g20_thumb_apriltag_calibration.hikrobot_camera:main"
|
||||
),
|
||||
"calibration_node = g20_thumb_apriltag_calibration.node:main",
|
||||
(
|
||||
"cmc_pitch_zero_node = "
|
||||
"g20_thumb_apriltag_calibration.zero_node:main"
|
||||
),
|
||||
(
|
||||
"cmc_roll_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.zero_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.three_camera_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_extrinsics_node = "
|
||||
"g20_thumb_apriltag_calibration.extrinsics_node:main"
|
||||
),
|
||||
(
|
||||
"offline_replay = "
|
||||
"g20_thumb_apriltag_calibration.offline_replay:main"
|
||||
),
|
||||
(
|
||||
"camera_alignment_view = "
|
||||
"g20_thumb_apriltag_calibration.alignment_view:main"
|
||||
),
|
||||
(
|
||||
"calibrated_joint_state_bridge = "
|
||||
"g20_thumb_apriltag_calibration."
|
||||
"calibrated_joint_state_bridge:main"
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -1,87 +0,0 @@
|
||||
import copy
|
||||
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
|
||||
G20_COMMAND_NAMES,
|
||||
G20_URDF_JOINT_NAMES,
|
||||
CalibratedCommandMapper,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
def _payload(side: str = "right") -> dict:
|
||||
profile = get_hand_calibration_profile(side)
|
||||
curve = [round((255 - command) * 0.001, 8) for command in range(256)]
|
||||
joints = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
joint = {
|
||||
"motor_index": spec.motor_index,
|
||||
"angle_rad": list(curve),
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = 255
|
||||
joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
joints[name] = joint
|
||||
return {
|
||||
"schema_version": 4,
|
||||
"model": "G20",
|
||||
"side": side,
|
||||
"serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [255] * 20,
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": True,
|
||||
"validation_mae_rad": 0.01,
|
||||
"validation_p95_rad": 0.02,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
result = dict(
|
||||
zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
|
||||
)
|
||||
|
||||
assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
|
||||
assert result["thumb_cmc_roll"] == pytest.approx(0.250)
|
||||
assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
|
||||
assert result["thumb_mcp"] == pytest.approx(0.240)
|
||||
assert result["thumb_ip"] == pytest.approx(0.240)
|
||||
assert result["pinky_pip"] == pytest.approx(0.236)
|
||||
assert result["pinky_dip"] == pytest.approx(0.236)
|
||||
|
||||
|
||||
def test_mapper_uses_names_instead_of_message_order() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
names = list(reversed(G20_COMMAND_NAMES))
|
||||
positions = list(reversed(command))
|
||||
|
||||
assert mapper.map_positions(positions, names) == mapper.map_positions(command)
|
||||
|
||||
|
||||
def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
|
||||
with pytest.raises(ValueError, match="does not match"):
|
||||
CalibratedCommandMapper(_payload("left"), expected_side="right")
|
||||
|
||||
payload = copy.deepcopy(_payload())
|
||||
payload["quality"]["passed"] = False
|
||||
with pytest.raises(ValueError, match="quality.passed"):
|
||||
CalibratedCommandMapper(payload, expected_side="right")
|
||||
|
||||
|
||||
def test_mapper_rejects_incomplete_named_command() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
with pytest.raises(ValueError, match="missing named channels"):
|
||||
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
|
||||
@@ -1,500 +0,0 @@
|
||||
import math
|
||||
from dataclasses import replace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
ACTIVE_JOINTS,
|
||||
IMAGE_TRAJECTORY_JOINTS,
|
||||
JOINT_SPECS,
|
||||
LEFT_HAND_PROFILE,
|
||||
MEASURED_JOINTS,
|
||||
PASSIVE_JOINTS,
|
||||
RIGHT_HAND_PROFILE,
|
||||
SPLAY_JOINTS,
|
||||
SWEEP_SPECS,
|
||||
VIEW_TAGS,
|
||||
build_calibration_motion_command,
|
||||
build_calibration_speed_profile,
|
||||
build_compact_payload,
|
||||
build_full_hand_command,
|
||||
center_splay_curve,
|
||||
fit_joint_center_curve,
|
||||
fit_joint_image_curve,
|
||||
fit_measured_joint_curve,
|
||||
fit_projected_zero,
|
||||
get_hand_calibration_profile,
|
||||
measure_joint_observation,
|
||||
validate_compact_payload,
|
||||
)
|
||||
|
||||
|
||||
def _records() -> list[dict[str, object]]:
|
||||
commands = list(range(0, 256, 16))
|
||||
if commands[-1] != 255:
|
||||
commands.append(255)
|
||||
records: list[dict[str, object]] = []
|
||||
centre = np.asarray([0.006, -0.004, 0.012])
|
||||
radius = 0.025
|
||||
image_centre = np.asarray([30.0, -12.0])
|
||||
image_radius = 100.0
|
||||
for cycle in range(3):
|
||||
for direction, sequence in (
|
||||
("decreasing", reversed(commands)),
|
||||
("increasing", commands),
|
||||
):
|
||||
for command in sequence:
|
||||
angle = 0.70 * (255.0 - command) / 255.0
|
||||
point = centre + np.asarray(
|
||||
[radius * math.cos(angle), radius * math.sin(angle), 0.0]
|
||||
)
|
||||
# At command 255 the inward vector points along image +x, so
|
||||
# table_projected_zero_rad is exactly zero.
|
||||
image_point = image_centre + np.asarray(
|
||||
[
|
||||
-image_radius * math.cos(angle),
|
||||
image_radius * math.sin(angle),
|
||||
]
|
||||
)
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_translation_xyz_m": point.tolist(),
|
||||
"image_relative_xy_px": image_point.tolist(),
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def test_joint_layout_covers_16_active_and_5_passive_joints() -> None:
|
||||
assert len(JOINT_SPECS) == 21
|
||||
assert len(ACTIVE_JOINTS) == 16
|
||||
assert len(PASSIVE_JOINTS) == 5
|
||||
|
||||
|
||||
def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None:
|
||||
profile = get_hand_calibration_profile("right")
|
||||
assert profile is RIGHT_HAND_PROFILE
|
||||
assert profile.reference_finger == "pinky"
|
||||
assert [spec.motor_index for spec in profile.sweep_specs] == [
|
||||
0, 5, 15, 9, 4, 19, 10
|
||||
]
|
||||
assert profile.view_tags["front"]["pinky_roll"] == 10
|
||||
assert profile.view_tags["side"] == {
|
||||
"side_base": 4,
|
||||
"pinky_mcp": 5,
|
||||
"pinky_pip": 6,
|
||||
"pinky_dip": 7,
|
||||
}
|
||||
assert profile.preflight_view_roles["side"] == (
|
||||
"side_base",
|
||||
"pinky_mcp",
|
||||
"pinky_pip",
|
||||
"pinky_dip",
|
||||
)
|
||||
thumb_pitch = profile.joint_specs["thumb_cmc_pitch"]
|
||||
assert thumb_pitch.view == "front"
|
||||
assert thumb_pitch.parent_role == "front_base"
|
||||
assert thumb_pitch.child_role == "thumb_cmc"
|
||||
assert profile.joint_specs["index_mcp_roll"].source_joint == (
|
||||
"pinky_mcp_roll"
|
||||
)
|
||||
assert profile.joint_specs["middle_mcp_pitch"].source_joint == (
|
||||
"pinky_mcp_pitch"
|
||||
)
|
||||
assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip"
|
||||
assert profile.joint_specs["index_dip"].source_joint == "pinky_dip"
|
||||
|
||||
roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9)
|
||||
command = build_calibration_motion_command(
|
||||
roll, 127, profile=profile
|
||||
)
|
||||
assert command[9] == 127
|
||||
assert command[6:9] == [255, 255, 255]
|
||||
speeds = build_calibration_speed_profile(
|
||||
roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
profile=profile,
|
||||
)
|
||||
assert speeds == [15, 15, 15, 15, 5]
|
||||
assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set(
|
||||
range(11)
|
||||
)
|
||||
assert VIEW_TAGS["front"]["index_roll"] == 10
|
||||
assert VIEW_TAGS["side"] == {
|
||||
"side_base": 4,
|
||||
"index_mcp": 5,
|
||||
"index_pip": 6,
|
||||
"index_dip": 7,
|
||||
}
|
||||
assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9}
|
||||
assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10]
|
||||
|
||||
|
||||
def test_joint_trajectory_spaces_match_observation_geometry() -> None:
|
||||
assert IMAGE_TRAJECTORY_JOINTS == {
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
"index_mcp_roll",
|
||||
"index_mcp_pitch",
|
||||
"index_pip",
|
||||
}
|
||||
records = _records()
|
||||
for name in IMAGE_TRAJECTORY_JOINTS:
|
||||
fit = fit_measured_joint_curve(name, records)
|
||||
assert fit.circle["space"] == "image_2d"
|
||||
for name in ("index_dip", "thumb_cmc_yaw"):
|
||||
assert "space" not in fit_measured_joint_curve(name, records).circle
|
||||
|
||||
|
||||
def test_full_hand_command_changes_exactly_one_controlled_motor() -> None:
|
||||
result = build_full_hand_command(6, 27)
|
||||
assert result[6] == 27
|
||||
assert result[:6] == [255] * 6
|
||||
with pytest.raises(ValueError, match="controlled"):
|
||||
build_full_hand_command(11, 27)
|
||||
|
||||
|
||||
def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
|
||||
result = build_calibration_motion_command(index_roll, 27, baseline)
|
||||
|
||||
assert result[6:10] == [27, 0, 0, 0]
|
||||
assert result[:6] == baseline[:6]
|
||||
assert result[10:] == baseline[10:]
|
||||
|
||||
|
||||
def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None:
|
||||
profile = RIGHT_HAND_PROFILE
|
||||
pinky_roll = next(
|
||||
spec for spec in profile.sweep_specs if spec.motor_index == 9
|
||||
)
|
||||
|
||||
result = build_calibration_motion_command(
|
||||
pinky_roll, 27, profile=profile
|
||||
)
|
||||
|
||||
assert result[6:10] == [255, 255, 255, 27]
|
||||
assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255}
|
||||
|
||||
|
||||
def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
|
||||
result = build_calibration_motion_command(thumb_pitch, 17, baseline)
|
||||
|
||||
assert result[0] == 17
|
||||
assert result[6:10] == [127, 127, 127, 127]
|
||||
|
||||
|
||||
def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None:
|
||||
profile = RIGHT_HAND_PROFILE
|
||||
thumb_pitch = next(
|
||||
spec for spec in profile.sweep_specs if spec.motor_index == 0
|
||||
)
|
||||
|
||||
result = build_calibration_motion_command(
|
||||
thumb_pitch, 17, profile=profile
|
||||
)
|
||||
|
||||
assert result[0] == 17
|
||||
assert result[10] == 255
|
||||
assert result[5] == 255
|
||||
assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255}
|
||||
|
||||
|
||||
def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None:
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
|
||||
result = build_calibration_motion_command(thumb_pitch, 17)
|
||||
|
||||
assert result[0] == 17
|
||||
assert result[5] == 255
|
||||
assert result[10] == 255
|
||||
assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {}
|
||||
|
||||
|
||||
def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10)
|
||||
|
||||
result = build_calibration_motion_command(thumb_yaw, 27, baseline)
|
||||
|
||||
assert result[5] == 145
|
||||
assert result[10] == 27
|
||||
assert result[:5] == baseline[:5]
|
||||
assert result[6:10] == baseline[6:10]
|
||||
assert result[11:] == baseline[11:]
|
||||
|
||||
|
||||
def test_only_index_roll_uses_the_slow_index_finger_speed() -> None:
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1)
|
||||
|
||||
assert build_calibration_speed_profile(
|
||||
index_roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 5, 15, 15, 15]
|
||||
assert build_calibration_speed_profile(
|
||||
index_pitch,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 10, 15, 15, 15]
|
||||
|
||||
index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16)
|
||||
assert build_calibration_speed_profile(
|
||||
index_pip,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 10, 15, 15, 15]
|
||||
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
assert build_calibration_speed_profile(
|
||||
thumb_pitch,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 15, 15, 15, 15]
|
||||
|
||||
|
||||
def test_calibration_speed_profile_rejects_out_of_range_values() -> None:
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
with pytest.raises(ValueError, match="speeds"):
|
||||
build_calibration_speed_profile(
|
||||
index_roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=256,
|
||||
index_flex_speed=10,
|
||||
)
|
||||
|
||||
|
||||
def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None:
|
||||
records = _records()
|
||||
fit = fit_joint_center_curve(records)
|
||||
assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8)
|
||||
assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4)
|
||||
assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8)
|
||||
assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6)
|
||||
|
||||
|
||||
def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None:
|
||||
records = _records()
|
||||
for record in records:
|
||||
command = int(record["command_u8"])
|
||||
record["relative_translation_xyz_m"] = [
|
||||
0.001 * command,
|
||||
0.0,
|
||||
0.0,
|
||||
]
|
||||
|
||||
fit = fit_measured_joint_curve("thumb_mcp", records)
|
||||
|
||||
assert fit.circle["space"] == "image_2d"
|
||||
assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02)
|
||||
assert fit.angle_rad[255] == pytest.approx(0.0)
|
||||
assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01
|
||||
assert math.degrees(fit.maximum_hysteresis_rad) < 0.01
|
||||
|
||||
command_zero = next(
|
||||
record for record in records if int(record["command_u8"]) == 0
|
||||
)
|
||||
observed = measure_joint_observation(
|
||||
fit,
|
||||
vector_xyz_m=command_zero["relative_translation_xyz_m"],
|
||||
image_vector_xy_px=command_zero["image_relative_xy_px"],
|
||||
)
|
||||
assert observed == pytest.approx(0.70, abs=0.02)
|
||||
|
||||
|
||||
def test_image_curve_rejects_insufficient_projected_arc() -> None:
|
||||
records = _records()
|
||||
for record in records:
|
||||
command = float(record["command_u8"])
|
||||
angle = math.radians(2.0) * (255.0 - command) / 255.0
|
||||
record["image_relative_xy_px"] = [
|
||||
100.0 * math.cos(angle),
|
||||
100.0 * math.sin(angle),
|
||||
]
|
||||
|
||||
with pytest.raises(
|
||||
ValueError, match="joint_image_trajectory_quality_failed:arc"
|
||||
):
|
||||
fit_joint_image_curve(records)
|
||||
|
||||
|
||||
def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
|
||||
fit = fit_joint_center_curve(_records())
|
||||
centred, zero_command, midpoint = center_splay_curve(fit)
|
||||
assert midpoint == pytest.approx(0.35, abs=1.0e-4)
|
||||
assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4)
|
||||
assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4)
|
||||
assert zero_command in {127, 128}
|
||||
assert abs(centred.angle_rad[zero_command]) <= 0.002
|
||||
|
||||
|
||||
def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, _ = center_splay_curve(base)
|
||||
splay = replace(
|
||||
splay,
|
||||
angle_rad=tuple(
|
||||
value - splay.angle_rad[zero_command]
|
||||
for value in splay.angle_rad
|
||||
),
|
||||
)
|
||||
measured = {
|
||||
name: splay if name == "index_mcp_roll" else base
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
offsets = {name: 0.01 for name in ACTIVE_JOINTS}
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [zero_command] * 4
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_LEFT_001",
|
||||
measured_fits=measured,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[0.01, -0.02],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
)
|
||||
validate_compact_payload(payload)
|
||||
assert set(payload) == {
|
||||
"schema_version",
|
||||
"model",
|
||||
"side",
|
||||
"serial_number",
|
||||
"angle_unit",
|
||||
"command_range",
|
||||
"baseline_command_u8",
|
||||
"joints",
|
||||
"quality",
|
||||
}
|
||||
assert len(payload["joints"]) == 21
|
||||
assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values())
|
||||
assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16
|
||||
assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5
|
||||
assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll"
|
||||
assert (
|
||||
payload["joints"]["middle_mcp_roll"]["angle_rad"]
|
||||
== payload["joints"]["index_mcp_roll"]["angle_rad"]
|
||||
)
|
||||
assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
|
||||
"urdf_zero_offset_rad": pytest.approx(0.01)
|
||||
}
|
||||
for name in SPLAY_JOINTS:
|
||||
assert payload["joints"][name]["zero_command_u8"] == zero_command
|
||||
|
||||
|
||||
def test_compact_payload_allows_skipped_random_validation() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, _ = center_splay_curve(base)
|
||||
splay = replace(
|
||||
splay,
|
||||
angle_rad=tuple(
|
||||
value - splay.angle_rad[zero_command]
|
||||
for value in splay.angle_rad
|
||||
),
|
||||
)
|
||||
measured = {
|
||||
name: splay if name == "index_mcp_roll" else base
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
offsets = {name: 0.0 for name in ACTIVE_JOINTS}
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [zero_command] * 4
|
||||
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_LEFT_001",
|
||||
measured_fits=measured,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
)
|
||||
|
||||
validate_compact_payload(payload)
|
||||
assert payload["quality"] == {
|
||||
"passed": True,
|
||||
"validation_mae_rad": None,
|
||||
"validation_p95_rad": None,
|
||||
}
|
||||
|
||||
|
||||
def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
|
||||
profile = RIGHT_HAND_PROFILE
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, _ = center_splay_curve(base)
|
||||
splay = replace(
|
||||
splay,
|
||||
angle_rad=tuple(
|
||||
value - splay.angle_rad[zero_command]
|
||||
for value in splay.angle_rad
|
||||
),
|
||||
)
|
||||
measured = {
|
||||
name: splay if name == "pinky_mcp_roll" else base
|
||||
for name in profile.measured_joints
|
||||
}
|
||||
offsets = {
|
||||
name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0)
|
||||
for name in profile.active_joints
|
||||
}
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [zero_command] * 4
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_RIGHT_001",
|
||||
measured_fits=measured,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[0.01],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
side="right",
|
||||
)
|
||||
|
||||
validate_compact_payload(payload)
|
||||
assert payload["schema_version"] == 4
|
||||
assert payload["side"] == "right"
|
||||
assert set(payload) == {
|
||||
"schema_version",
|
||||
"model",
|
||||
"side",
|
||||
"serial_number",
|
||||
"angle_unit",
|
||||
"command_range",
|
||||
"baseline_command_u8",
|
||||
"joints",
|
||||
"quality",
|
||||
}
|
||||
for finger in ("index", "middle", "ring"):
|
||||
assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == (
|
||||
"pinky_mcp_roll"
|
||||
)
|
||||
assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == (
|
||||
"pinky_mcp_pitch"
|
||||
)
|
||||
assert payload["joints"][f"{finger}_pip"]["source_joint"] == (
|
||||
"pinky_pip"
|
||||
)
|
||||
for suffix in ("mcp_roll", "mcp_pitch", "pip"):
|
||||
joint = payload["joints"][f"{finger}_{suffix}"]
|
||||
source = payload["joints"][joint["source_joint"]]
|
||||
assert joint["angle_rad"] == source["angle_rad"]
|
||||
assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
|
||||
assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}
|
||||
@@ -1,44 +0,0 @@
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.offline_replay import (
|
||||
_latest_attempt_records,
|
||||
_output_suffix,
|
||||
)
|
||||
|
||||
|
||||
def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict:
|
||||
return {
|
||||
"kind": "sample",
|
||||
"joint": joint,
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"attempt": attempt,
|
||||
}
|
||||
|
||||
|
||||
def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
|
||||
rows = [
|
||||
{"kind": "session_start"},
|
||||
_sample("pinky_pip", 0, "decreasing", 1),
|
||||
_sample("pinky_pip", 0, "decreasing", 3),
|
||||
_sample("pinky_pip", 0, "increasing", 1),
|
||||
_sample("pinky_pip", 1, "decreasing", 2),
|
||||
_sample("thumb_cmc_yaw", 0, "decreasing", 1),
|
||||
]
|
||||
|
||||
selected = _latest_attempt_records(rows)
|
||||
|
||||
assert [
|
||||
record["attempt"] for record in selected["pinky_pip"]
|
||||
] == [3, 1, 2]
|
||||
assert [
|
||||
record["attempt"] for record in selected["thumb_cmc_yaw"]
|
||||
] == [1]
|
||||
|
||||
|
||||
def test_output_suffix_is_safe_and_explicit() -> None:
|
||||
assert _output_suffix(None) == ""
|
||||
assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
|
||||
for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
|
||||
with pytest.raises(ValueError, match="output tag"):
|
||||
_output_suffix(invalid)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,977 +0,0 @@
|
||||
import math
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from g20_thumb_apriltag_calibration.extrinsics import (
|
||||
camera_info_fingerprint,
|
||||
dump_three_camera_extrinsics,
|
||||
load_three_camera_extrinsics,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
AXIS_JOINTS,
|
||||
DIRECT_ZERO_JOINTS,
|
||||
INHERITED_ZERO_JOINTS,
|
||||
JointAxisMeasurement,
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
_zero_sensitive_axis_error_rad,
|
||||
fit_joint_axis_measurement,
|
||||
fit_rotation_joint_curve,
|
||||
solve_urdf_zero_offsets,
|
||||
get_zero_calibration_profile,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
ACTIVE_JOINTS,
|
||||
JOINT_SPECS,
|
||||
MEASURED_JOINTS,
|
||||
PASSIVE_JOINTS,
|
||||
JointCurveFit,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
REPOSITORY = Path(__file__).resolve().parents[3]
|
||||
SOURCE_URDF = REPOSITORY / (
|
||||
"src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
|
||||
"linker_hand/g20_left/linkerhand_g20_left.urdf"
|
||||
)
|
||||
RIGHT_SOURCE_URDF = REPOSITORY / (
|
||||
"src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
|
||||
"linker_hand/g20_right/linkerhand_g20_right.urdf"
|
||||
)
|
||||
|
||||
|
||||
def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
|
||||
parent = np.asarray([0.0, 0.0, 1.0])
|
||||
predicted = np.asarray([1.0, 0.0, 0.0])
|
||||
cone_mismatch = np.asarray(
|
||||
[math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))]
|
||||
)
|
||||
zero_mismatch = np.asarray(
|
||||
[math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0]
|
||||
)
|
||||
|
||||
assert _zero_sensitive_axis_error_rad(
|
||||
predicted, cone_mismatch, parent
|
||||
) == pytest.approx(0.0, abs=1.0e-12)
|
||||
assert math.degrees(
|
||||
_zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent)
|
||||
) == pytest.approx(3.0, abs=1.0e-9)
|
||||
|
||||
|
||||
def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None:
|
||||
parent = np.asarray([0.0, 0.0, 1.0])
|
||||
cone = math.radians(32.0)
|
||||
phase = math.radians(7.0)
|
||||
predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)])
|
||||
observed = Rotation.from_rotvec(parent * phase).apply(predicted)
|
||||
|
||||
error = _zero_sensitive_axis_error_rad(predicted, observed, parent)
|
||||
|
||||
assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9)
|
||||
|
||||
|
||||
def _payload(transform: np.ndarray) -> dict[str, list[float]]:
|
||||
return {
|
||||
"translation_xyz_m": transform[:3, 3].tolist(),
|
||||
"quaternion_xyzw": Rotation.from_matrix(
|
||||
transform[:3, :3]
|
||||
).as_quat().tolist(),
|
||||
}
|
||||
|
||||
|
||||
def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]:
|
||||
axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float)
|
||||
axis_parent /= np.linalg.norm(axis_parent)
|
||||
centre_parent = np.asarray([0.012, -0.008, 0.021])
|
||||
radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2]))
|
||||
radial = 0.035 * radial / np.linalg.norm(radial)
|
||||
child_tag_mount = Rotation.from_euler(
|
||||
"xyz", [1.1, -0.7, 0.45]
|
||||
)
|
||||
common_from_parent = np.eye(4)
|
||||
common_from_parent[:3, :3] = Rotation.from_euler(
|
||||
"xyz", [-0.8, 0.55, 1.3]
|
||||
).as_matrix()
|
||||
common_from_parent[:3, 3] = [0.41, -0.12, 0.73]
|
||||
expected_axis = common_from_parent[:3, :3] @ axis_parent
|
||||
expected_point = (
|
||||
common_from_parent[:3, :3] @ centre_parent
|
||||
+ common_from_parent[:3, 3]
|
||||
)
|
||||
|
||||
commands = list(range(0, 256, 16)) + [255]
|
||||
records = []
|
||||
for cycle in range(3):
|
||||
for direction in ("decreasing", "increasing"):
|
||||
for command in commands:
|
||||
angle = math.radians(62.0) * (255.0 - command) / 255.0
|
||||
motion = Rotation.from_rotvec(axis_parent * angle)
|
||||
relative_rotation = motion * child_tag_mount
|
||||
relative_translation = centre_parent + motion.apply(radial)
|
||||
child_common = common_from_parent.copy()
|
||||
child_common[:3, :3] = (
|
||||
common_from_parent[:3, :3]
|
||||
@ relative_rotation.as_matrix()
|
||||
)
|
||||
child_common[:3, 3] = (
|
||||
common_from_parent[:3, :3] @ relative_translation
|
||||
+ common_from_parent[:3, 3]
|
||||
)
|
||||
state = [255.0] * 20
|
||||
state[5] = float(command)
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_translation_xyz_m": relative_translation.tolist(),
|
||||
"relative_quaternion_xyzw": relative_rotation.as_quat().tolist(),
|
||||
"parent_pose_common": _payload(common_from_parent),
|
||||
"child_pose_common": _payload(child_common),
|
||||
"state_u8": state,
|
||||
}
|
||||
)
|
||||
return records, expected_axis, expected_point
|
||||
|
||||
|
||||
def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
curve = fit_rotation_joint_curve(records, zero_command_u8=255)
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"thumb_cmc_roll", records, cycle=0, zero_command_u8=255
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05))
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 1.0e-6
|
||||
assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9)
|
||||
assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
|
||||
def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
|
||||
joint: str,
|
||||
) -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
|
||||
axis_parent = parent_rotation.inv().apply(expected_axis)
|
||||
tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7]))
|
||||
tangent /= np.linalg.norm(tangent)
|
||||
# Reproduce monocular planar-PnP depth bias: the centre trajectory remains
|
||||
# precise in its dominant directions but receives a command-correlated
|
||||
# component that makes a free 3-D plane normal substantially wrong.
|
||||
biased_records = []
|
||||
for record in records:
|
||||
biased = dict(record)
|
||||
point = np.asarray(record["relative_translation_xyz_m"], dtype=float)
|
||||
depth_bias = 0.30 * float(point @ tangent)
|
||||
biased["relative_translation_xyz_m"] = (
|
||||
point + depth_bias * axis_parent
|
||||
).tolist()
|
||||
biased_records.append(biased)
|
||||
|
||||
measurement = fit_joint_axis_measurement(
|
||||
joint, biased_records, cycle=0, zero_command_u8=255
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert abs(float(observed_axis @ expected_axis)) > math.cos(
|
||||
math.radians(0.05)
|
||||
)
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 0.003
|
||||
assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0)
|
||||
assert measurement.plane_rms_m < 0.003
|
||||
assert measurement.radial_rms_m < 0.003
|
||||
|
||||
|
||||
def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
|
||||
axis_parent = common_from_parent.inv().apply(expected_axis)
|
||||
# Exact end-on depth is a gauge along the physical axis and therefore
|
||||
# cannot alter the observable axis line. An oblique camera has a small
|
||||
# irreducible coupling between monocular depth and radial position; that
|
||||
# case must be bounded by the residual/holdout gates, not asserted to be
|
||||
# exactly recoverable from one view.
|
||||
view_normal_parent = axis_parent
|
||||
view_normal_common = common_from_parent.apply(view_normal_parent)
|
||||
biased = []
|
||||
for record in records:
|
||||
changed = dict(record)
|
||||
fraction = (255.0 - float(record["command_u8"])) / 255.0
|
||||
depth_bias = 0.03 * (fraction - 0.5)
|
||||
changed["relative_translation_xyz_m"] = (
|
||||
np.asarray(record["relative_translation_xyz_m"], dtype=float)
|
||||
+ depth_bias * view_normal_parent
|
||||
).tolist()
|
||||
biased.append(changed)
|
||||
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"thumb_cmc_pitch",
|
||||
biased,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
view_normal_common_xyz=view_normal_common,
|
||||
)
|
||||
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert measurement.axis_point_source == "pose_trajectory_image_plane"
|
||||
assert measurement.pose_axis_line_rms_m < 1.0e-6
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 1.0e-6
|
||||
|
||||
|
||||
def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
|
||||
records, expected_axis, _ = _arbitrary_tag_records()
|
||||
assert not any(record["command_u8"] == 127 for record in records)
|
||||
|
||||
curve = fit_rotation_joint_curve(records, zero_command_u8=127)
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"index_mcp_roll", records, cycle=0, zero_command_u8=127
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
assert abs(float(observed_axis @ expected_axis)) > math.cos(
|
||||
math.radians(0.05)
|
||||
)
|
||||
assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9)
|
||||
|
||||
|
||||
def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None:
|
||||
records, expected_axis, expected_point = _arbitrary_tag_records()
|
||||
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
|
||||
physical_axis_parent = common_from_parent.inv().apply(expected_axis)
|
||||
wrong_axis_parent = np.cross(
|
||||
physical_axis_parent, np.asarray([0.2, 0.8, -0.1])
|
||||
)
|
||||
wrong_axis_parent /= np.linalg.norm(wrong_axis_parent)
|
||||
mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"])
|
||||
contradictory = []
|
||||
for record in records:
|
||||
changed = dict(record)
|
||||
angle = math.radians(62.0) * (
|
||||
255.0 - float(record["command_u8"])
|
||||
) / 255.0
|
||||
changed["relative_quaternion_xyzw"] = (
|
||||
Rotation.from_rotvec(wrong_axis_parent * angle) * mount
|
||||
).as_quat().tolist()
|
||||
contradictory.append(changed)
|
||||
|
||||
measurement = fit_joint_axis_measurement(
|
||||
"index_dip",
|
||||
contradictory,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
axis_common_constraint=expected_axis,
|
||||
)
|
||||
|
||||
observed_axis = np.asarray(measurement.axis_common_xyz)
|
||||
observed_point = np.asarray(measurement.point_common_xyz_m)
|
||||
assert abs(float(observed_axis @ expected_axis)) > math.cos(
|
||||
math.radians(0.05)
|
||||
)
|
||||
assert np.linalg.norm(
|
||||
np.cross(observed_point - expected_point, expected_axis)
|
||||
) < 1.0e-6
|
||||
|
||||
|
||||
def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None:
|
||||
cameras = {
|
||||
view: {
|
||||
"serial_number": f"SERIAL_{view}",
|
||||
"width": 1624,
|
||||
"height": 1240,
|
||||
"intrinsics_sha256": camera_info_fingerprint(
|
||||
width=1624,
|
||||
height=1240,
|
||||
camera_matrix=np.asarray(
|
||||
[[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]]
|
||||
),
|
||||
),
|
||||
}
|
||||
for view in ("front", "side", "top")
|
||||
}
|
||||
transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)}
|
||||
transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix()
|
||||
transforms["side"][:3, 3] = [0.2, 0.0, 0.1]
|
||||
transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix()
|
||||
transforms["top"][:3, 3] = [-0.1, 0.3, 0.2]
|
||||
destination = tmp_path / "extrinsics.yaml"
|
||||
|
||||
dump_three_camera_extrinsics(
|
||||
destination,
|
||||
cameras=cameras,
|
||||
front_from_view=transforms,
|
||||
quality={
|
||||
"passed": True,
|
||||
"reprojection_rms_px": 0.3,
|
||||
"maximum_rotation_repeatability_deg": 0.2,
|
||||
"maximum_translation_repeatability_m": 0.001,
|
||||
"front_side_captures": 15,
|
||||
"front_top_captures": 15,
|
||||
},
|
||||
)
|
||||
loaded = load_three_camera_extrinsics(destination)
|
||||
|
||||
assert loaded.cameras["front"].serial_number == "SERIAL_front"
|
||||
assert np.allclose(loaded.transform("side"), transforms["side"])
|
||||
assert np.allclose(loaded.transform("top"), transforms["top"])
|
||||
assert loaded.camera_matches(
|
||||
"front",
|
||||
serial_number="SERIAL_front",
|
||||
width=1624,
|
||||
height=1240,
|
||||
intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
|
||||
)
|
||||
assert not loaded.camera_matches(
|
||||
"front",
|
||||
serial_number="WRONG_SERIAL",
|
||||
width=1624,
|
||||
height=1240,
|
||||
intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
|
||||
)
|
||||
|
||||
|
||||
def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]:
|
||||
joint = next(
|
||||
element
|
||||
for element in ET.parse(path).getroot().findall("joint")
|
||||
if element.get("name") == name
|
||||
)
|
||||
origin = joint.find("origin")
|
||||
axis = joint.find("axis")
|
||||
xyz = np.asarray([float(value) for value in origin.get("xyz").split()])
|
||||
rpy = np.asarray([float(value) for value in origin.get("rpy").split()])
|
||||
axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()])
|
||||
return np.block(
|
||||
[
|
||||
[Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]],
|
||||
[np.asarray([[0.0, 0.0, 0.0, 1.0]])],
|
||||
]
|
||||
), axis_xyz / np.linalg.norm(axis_xyz)
|
||||
|
||||
|
||||
def _joint_limit(path: Path, name: str) -> tuple[float, float]:
|
||||
joint = next(
|
||||
element
|
||||
for element in ET.parse(path).getroot().findall("joint")
|
||||
if element.get("name") == name
|
||||
)
|
||||
limit = joint.find("limit")
|
||||
return float(limit.get("lower")), float(limit.get("upper"))
|
||||
|
||||
|
||||
def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None:
|
||||
offset = math.radians(7.3)
|
||||
destination = write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": offset},
|
||||
timestamp="20260806_120000",
|
||||
)
|
||||
original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw")
|
||||
corrected, _ = _joint_origin(destination, "thumb_cmc_yaw")
|
||||
expected = original.copy()
|
||||
expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
|
||||
axis * offset
|
||||
).as_matrix()
|
||||
|
||||
assert destination != SOURCE_URDF
|
||||
assert np.allclose(corrected, expected, atol=1.0e-12)
|
||||
with pytest.raises(ValueError, match="refusing to overwrite"):
|
||||
write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": offset},
|
||||
timestamp="20260806_120000",
|
||||
)
|
||||
with pytest.raises(ValueError, match="original CAD URDF"):
|
||||
write_zero_corrected_urdf(
|
||||
source_urdf=destination,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": offset},
|
||||
timestamp="20260806_120001",
|
||||
)
|
||||
with pytest.raises(ValueError, match="finite and within"):
|
||||
write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": math.nan},
|
||||
timestamp="20260806_120002",
|
||||
)
|
||||
|
||||
|
||||
def test_urdf_writer_changes_only_the_16_active_zero_origins(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
before = SOURCE_URDF.read_bytes()
|
||||
offsets = {
|
||||
name: math.radians(0.25 * (index + 1))
|
||||
for index, name in enumerate(ACTIVE_JOINTS)
|
||||
}
|
||||
destination = write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad=offsets,
|
||||
timestamp="20260807_180000",
|
||||
)
|
||||
|
||||
assert len(offsets) == 16
|
||||
assert SOURCE_URDF.read_bytes() == before
|
||||
for name in ACTIVE_JOINTS:
|
||||
original, axis = _joint_origin(SOURCE_URDF, name)
|
||||
corrected, corrected_axis = _joint_origin(destination, name)
|
||||
expected = original.copy()
|
||||
expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
|
||||
axis * offsets[name]
|
||||
).as_matrix()
|
||||
assert np.allclose(corrected, expected, atol=1.0e-12)
|
||||
assert np.allclose(corrected_axis, axis, atol=1.0e-12)
|
||||
for name in PASSIVE_JOINTS:
|
||||
original, axis = _joint_origin(SOURCE_URDF, name)
|
||||
corrected, corrected_axis = _joint_origin(destination, name)
|
||||
assert np.allclose(corrected, original, atol=1.0e-12)
|
||||
assert np.allclose(corrected_axis, axis, atol=1.0e-12)
|
||||
# A zero calibration must not silently expand mechanical/CAD safety
|
||||
# limits. Dynamic measured ranges remain in the calibration JSON.
|
||||
for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS):
|
||||
assert _joint_limit(destination, name) == pytest.approx(
|
||||
_joint_limit(SOURCE_URDF, name)
|
||||
)
|
||||
|
||||
|
||||
def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit:
|
||||
values = np.asarray(
|
||||
[travel * (255.0 - command) / 255.0 for command in range(256)]
|
||||
)
|
||||
values -= values[zero_command]
|
||||
data = tuple(float(value) for value in values)
|
||||
return JointCurveFit(
|
||||
angle_rad=data,
|
||||
decreasing_rad=data,
|
||||
increasing_rad=data,
|
||||
circle={},
|
||||
maximum_monotonic_correction_rad=0.0,
|
||||
maximum_hysteresis_rad=0.0,
|
||||
quality={},
|
||||
)
|
||||
|
||||
|
||||
def _solve_synthetic_offsets(
|
||||
side: str,
|
||||
offset_degrees: list[float],
|
||||
*,
|
||||
inject_oblique_optical_depth_bias: bool = False,
|
||||
inject_secondary_root_axis_bias_degrees: float = 0.0,
|
||||
inject_secondary_root_point_bias_m: float = 0.0,
|
||||
inject_observer_cone_bias_degrees: float = 0.0,
|
||||
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
|
||||
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
||||
):
|
||||
hand = get_hand_calibration_profile(side)
|
||||
zero = get_zero_calibration_profile(side)
|
||||
source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
|
||||
baseline = [255.0] * 20
|
||||
baseline[6:10] = [127.0] * 4
|
||||
curves = {
|
||||
name: _synthetic_curve(
|
||||
int(baseline[hand.joint_specs[name].motor_index]),
|
||||
math.radians(50.0),
|
||||
)
|
||||
for name in hand.measured_joints
|
||||
}
|
||||
if inject_secondary_root_axis_bias_degrees:
|
||||
# Make the thumb root the higher-travel, directly observed direction,
|
||||
# matching the real right-hand data where the short pinky splay arc is
|
||||
# the less reliable root-axis orientation estimate.
|
||||
curves["thumb_cmc_roll"] = _synthetic_curve(
|
||||
int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]),
|
||||
math.radians(70.0),
|
||||
)
|
||||
motor_by_joint = {
|
||||
name: spec.motor_index for name, spec in hand.joint_specs.items()
|
||||
}
|
||||
offsets = {
|
||||
name: math.radians(value)
|
||||
for name, value in zip(zero.direct_zero_joints, offset_degrees)
|
||||
}
|
||||
model = UrdfKinematicModel(source)
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
measurements: list[JointAxisMeasurement] = []
|
||||
for cycle in range(3):
|
||||
for joint in zero.axis_joints:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
state[5] = 145.0
|
||||
angles = _angles_from_state(
|
||||
state,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero.inherited_zero_joints,
|
||||
)
|
||||
axis, point = model.axis_line(
|
||||
joint, zero_offsets=offsets, joint_angles=angles
|
||||
)
|
||||
point_common = base_rotation.apply(point) + base_translation
|
||||
if (
|
||||
inject_secondary_root_point_bias_m
|
||||
and joint == f"{zero.reference_finger}_mcp_roll"
|
||||
):
|
||||
# A repeatable monocular depth error on the second parallel
|
||||
# root line must affect translation only, never palm rotation
|
||||
# or the inferred thumb-roll zero.
|
||||
point_common = point_common + base_rotation.apply(
|
||||
np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m])
|
||||
)
|
||||
view_normal_common = None
|
||||
if (
|
||||
inject_oblique_optical_depth_bias
|
||||
and joint in zero.phase_parent_joint
|
||||
):
|
||||
parent_axis = model.axis_line(
|
||||
zero.phase_parent_joint[joint],
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)[0]
|
||||
helper = (
|
||||
np.asarray([1.0, 0.0, 0.0])
|
||||
if abs(float(parent_axis[0])) < 0.8
|
||||
else np.asarray([0.0, 1.0, 0.0])
|
||||
)
|
||||
tilt_axis = np.cross(parent_axis, helper)
|
||||
tilt_axis /= np.linalg.norm(tilt_axis)
|
||||
view_normal = Rotation.from_rotvec(
|
||||
math.radians(15.0) * tilt_axis
|
||||
).apply(parent_axis)
|
||||
view_normal_common = tuple(base_rotation.apply(view_normal))
|
||||
# Simulate an independent planar-PnP depth error on the child
|
||||
# Tag. It is large enough to drive the old 3-D phase solve to
|
||||
# a configured offset bound.
|
||||
point_common = point_common + 0.03 * np.asarray(
|
||||
view_normal_common
|
||||
)
|
||||
axis_common = base_rotation.apply(axis)
|
||||
if (
|
||||
inject_observer_cone_bias_degrees
|
||||
and joint == "thumb_cmc_pitch"
|
||||
):
|
||||
parent_axis = model.axis_line(
|
||||
zero.axis_parent_joint[joint],
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)[0]
|
||||
cone_normal = np.cross(axis, parent_axis)
|
||||
cone_normal /= np.linalg.norm(cone_normal)
|
||||
axis_common = base_rotation.apply(
|
||||
Rotation.from_rotvec(
|
||||
math.radians(inject_observer_cone_bias_degrees)
|
||||
* cone_normal
|
||||
).apply(axis)
|
||||
)
|
||||
if (
|
||||
inject_secondary_root_axis_bias_degrees
|
||||
and joint == f"{zero.reference_finger}_mcp_roll"
|
||||
):
|
||||
helper = np.asarray([0.0, 0.0, 1.0])
|
||||
if abs(float(axis_common @ helper)) > 0.8:
|
||||
helper = np.asarray([0.0, 1.0, 0.0])
|
||||
bias_axis = np.cross(axis_common, helper)
|
||||
bias_axis /= np.linalg.norm(bias_axis)
|
||||
axis_common = Rotation.from_rotvec(
|
||||
math.radians(inject_secondary_root_axis_bias_degrees)
|
||||
* bias_axis
|
||||
).apply(axis_common)
|
||||
measurements.append(
|
||||
JointAxisMeasurement(
|
||||
joint=joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(axis_common),
|
||||
point_common_xyz_m=tuple(point_common),
|
||||
condition_state_u8=tuple(state),
|
||||
plane_rms_m=0.0002,
|
||||
radial_rms_m=0.0002,
|
||||
rotation_circle_axis_difference_rad=math.radians(0.1),
|
||||
view_normal_common_xyz=view_normal_common,
|
||||
pose_axis_line_rms_m=(
|
||||
pose_axis_line_rms_by_joint_m or {}
|
||||
).get(joint, 0.0),
|
||||
)
|
||||
)
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=source,
|
||||
measurements=measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
hand_type=side,
|
||||
joint_maximum_offset_rad={
|
||||
name: math.radians(value)
|
||||
for name, value in (joint_maximum_offset_degrees or {}).items()
|
||||
},
|
||||
)
|
||||
return zero, result
|
||||
|
||||
|
||||
def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
|
||||
zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
|
||||
assert result.passed is True
|
||||
static_policy = {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}
|
||||
for name, value in result.direct_offsets_rad.items():
|
||||
assert value == pytest.approx(static_policy.get(name, 0.0))
|
||||
|
||||
|
||||
def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
|
||||
right = get_zero_calibration_profile("right")
|
||||
left = get_zero_calibration_profile("left")
|
||||
|
||||
assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad
|
||||
|
||||
|
||||
def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0]
|
||||
)
|
||||
reference_roll = f"{zero.reference_finger}_mcp_roll"
|
||||
assert result.passed is True
|
||||
assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
|
||||
|
||||
def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_secondary_root_axis_bias_degrees=15.0,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
expected = dict(
|
||||
zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
|
||||
)
|
||||
expected.update(
|
||||
{
|
||||
name: math.degrees(value)
|
||||
for name, value in {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}.items()
|
||||
}
|
||||
)
|
||||
for name, value in expected.items():
|
||||
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
|
||||
value, abs=0.05
|
||||
)
|
||||
|
||||
|
||||
def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_secondary_root_point_bias_m=0.02,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_roll"]
|
||||
) == pytest.approx(2.0, abs=0.05)
|
||||
assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0
|
||||
|
||||
|
||||
def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None:
|
||||
offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0]
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
offsets,
|
||||
joint_maximum_offset_degrees={"thumb_mcp": 45.0},
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
|
||||
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
|
||||
assert "thumb_ip" not in result.validation_error_by_joint_rad
|
||||
for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
|
||||
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
|
||||
for name in ("pinky_mcp_pitch", "pinky_pip"):
|
||||
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0
|
||||
assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None:
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_oblique_optical_depth_bias=True,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
expected = dict(
|
||||
zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
|
||||
)
|
||||
expected.update(
|
||||
{
|
||||
name: math.degrees(value)
|
||||
for name, value in {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}.items()
|
||||
}
|
||||
)
|
||||
for name, value in expected.items():
|
||||
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
|
||||
value, abs=0.05
|
||||
)
|
||||
|
||||
|
||||
def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None:
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
inject_observer_cone_bias_degrees=8.0,
|
||||
)
|
||||
|
||||
assert result.passed is False
|
||||
assert result.failure_reasons["thumb_cmc_yaw"] == (
|
||||
"zero_axis_cone_mismatch_too_large"
|
||||
)
|
||||
|
||||
|
||||
def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002},
|
||||
)
|
||||
|
||||
assert result.passed is False
|
||||
assert result.failure_reasons["thumb_cmc_pitch"] == (
|
||||
"zero_phase_axis_line_residual_too_large"
|
||||
)
|
||||
|
||||
|
||||
def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
|
||||
zero = get_zero_calibration_profile("left")
|
||||
baseline = [255.0] * 20
|
||||
baseline[6:10] = [127.0] * 4
|
||||
curves = {
|
||||
name: _synthetic_curve(
|
||||
int(baseline[JOINT_SPECS[name].motor_index]),
|
||||
math.radians(50.0),
|
||||
)
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
motor_by_joint = {
|
||||
name: spec.motor_index for name, spec in JOINT_SPECS.items()
|
||||
}
|
||||
true_offsets = {
|
||||
name: math.radians(value)
|
||||
for name, value in zip(
|
||||
DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]
|
||||
)
|
||||
}
|
||||
model = UrdfKinematicModel(SOURCE_URDF)
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
measurements = []
|
||||
yaw_axis_without_clearance = None
|
||||
yaw_axis_with_clearance = None
|
||||
for cycle in range(3):
|
||||
for joint in AXIS_JOINTS:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
state[5] = 145.0
|
||||
angles = _angles_from_state(
|
||||
state, curves=curves, motor_by_joint=motor_by_joint
|
||||
)
|
||||
axis, point = model.axis_line(
|
||||
joint,
|
||||
zero_offsets=true_offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
yaw_axis_with_clearance = axis.copy()
|
||||
baseline_angles = _angles_from_state(
|
||||
baseline, curves=curves, motor_by_joint=motor_by_joint
|
||||
)
|
||||
yaw_axis_without_clearance = model.axis_line(
|
||||
joint,
|
||||
zero_offsets=true_offsets,
|
||||
joint_angles=baseline_angles,
|
||||
)[0]
|
||||
measurements.append(
|
||||
JointAxisMeasurement(
|
||||
joint=joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(base_rotation.apply(axis)),
|
||||
point_common_xyz_m=tuple(
|
||||
base_rotation.apply(point) + base_translation
|
||||
),
|
||||
condition_state_u8=tuple(state),
|
||||
plane_rms_m=0.0002,
|
||||
radial_rms_m=0.0002,
|
||||
rotation_circle_axis_difference_rad=math.radians(0.1),
|
||||
)
|
||||
)
|
||||
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=SOURCE_URDF,
|
||||
measurements=measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
)
|
||||
|
||||
assert math.degrees(
|
||||
math.acos(
|
||||
np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0)
|
||||
)
|
||||
) > 1.0
|
||||
assert result.passed is True
|
||||
static_policy = {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}
|
||||
for name, expected in true_offsets.items():
|
||||
if name in static_policy:
|
||||
expected = static_policy[name]
|
||||
assert result.direct_offsets_rad[name] == pytest.approx(
|
||||
expected, abs=math.radians(0.05)
|
||||
)
|
||||
assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS)
|
||||
assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx(
|
||||
0.0, abs=math.radians(0.05)
|
||||
)
|
||||
for target in INHERITED_ZERO_JOINTS:
|
||||
assert result.all_active_offsets_rad[target] == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys()
|
||||
|
||||
|
||||
def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None:
|
||||
hand = get_hand_calibration_profile("right")
|
||||
zero = get_zero_calibration_profile("right")
|
||||
baseline = [255.0] * 20
|
||||
baseline[6:10] = [127.0] * 4
|
||||
curves = {
|
||||
name: _synthetic_curve(
|
||||
int(baseline[hand.joint_specs[name].motor_index]),
|
||||
math.radians(50.0),
|
||||
)
|
||||
for name in hand.measured_joints
|
||||
}
|
||||
motor_by_joint = {
|
||||
name: spec.motor_index for name, spec in hand.joint_specs.items()
|
||||
}
|
||||
true_offsets = {
|
||||
name: math.radians(value)
|
||||
for name, value in zip(
|
||||
zero.direct_zero_joints,
|
||||
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
|
||||
)
|
||||
}
|
||||
model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
measurements: list[JointAxisMeasurement] = []
|
||||
for cycle in range(3):
|
||||
states: dict[str, list[float]] = {}
|
||||
lines: dict[str, tuple[np.ndarray, np.ndarray]] = {}
|
||||
for joint in zero.axis_joints:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
state[5] = 145.0
|
||||
states[joint] = state
|
||||
angles = _angles_from_state(
|
||||
state,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero.inherited_zero_joints,
|
||||
)
|
||||
lines[joint] = model.axis_line(
|
||||
joint,
|
||||
zero_offsets=true_offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
original_lines = {
|
||||
name: (axis.copy(), point.copy())
|
||||
for name, (axis, point) in lines.items()
|
||||
}
|
||||
for observer, parent in zero.phase_parent_joint.items():
|
||||
parent_axis, parent_point = lines[parent]
|
||||
original_parent_axis, original_parent_point = original_lines[parent]
|
||||
child_axis, original_child_point = original_lines[observer]
|
||||
radial = original_child_point - original_parent_point
|
||||
radial -= original_parent_axis * float(
|
||||
radial @ original_parent_axis
|
||||
)
|
||||
# Preserve angular phase while deliberately corrupting link radius
|
||||
# and along-axis depth. These components must not move a zero.
|
||||
lines[observer] = (
|
||||
child_axis,
|
||||
parent_point + 1.25 * radial + 0.02 * parent_axis,
|
||||
)
|
||||
for joint in zero.axis_joints:
|
||||
axis, point = lines[joint]
|
||||
measurements.append(
|
||||
JointAxisMeasurement(
|
||||
joint=joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(base_rotation.apply(axis)),
|
||||
point_common_xyz_m=tuple(
|
||||
base_rotation.apply(point) + base_translation
|
||||
),
|
||||
condition_state_u8=tuple(states[joint]),
|
||||
plane_rms_m=0.0002,
|
||||
radial_rms_m=0.0002,
|
||||
rotation_circle_axis_difference_rad=math.radians(0.1),
|
||||
)
|
||||
)
|
||||
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=RIGHT_SOURCE_URDF,
|
||||
measurements=measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
hand_type="right",
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
static_policy = {
|
||||
**zero.fixed_direct_zero_offsets_rad,
|
||||
**zero.static_output_zero_offsets_rad,
|
||||
}
|
||||
for name, expected in true_offsets.items():
|
||||
if name in static_policy:
|
||||
expected = static_policy[name]
|
||||
assert result.direct_offsets_rad[name] == pytest.approx(
|
||||
expected, abs=math.radians(0.05)
|
||||
)
|
||||
for target in zero.inherited_zero_joints:
|
||||
assert result.all_active_offsets_rad[target] == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
assert set(result.all_active_offsets_rad) == set(hand.active_joints)
|
||||
@@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||||
"握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
|
||||
"张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"OK": [0, 0, 255, 255, 255, 151, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 255, 255, 255],
|
||||
"拇指对中指": [0, 255, 0, 255, 255, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 255, 255],
|
||||
"拇指对无名指": [0, 255, 255, 0, 255, 88, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254],
|
||||
"拇指对小指": [0, 255, 255, 255, 0, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215],
|
||||
"OK": [0, 0, 255, 255, 255, 138, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 211, 255, 255, 255],
|
||||
"拇指对中指": [0, 255, 0, 255, 255, 107, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 202, 255, 255],
|
||||
"拇指对无名指": [0, 255, 255, 0, 255, 88, 171, 148, 105, 42, 59, 255, 255, 255, 255, 255, 255, 255, 206, 254],
|
||||
"拇指对小指": [0, 255, 255, 255, 0, 32, 170, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 203],
|
||||
"准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||||
"壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
|
||||
"贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],
|
||||
@@ -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)
|
||||
+443
-36
@@ -1,4 +1,395 @@
|
||||
# G20 左右手 AprilTag 标定
|
||||
# LinkerHand 专业标定包
|
||||
|
||||
## O6 右手局部标定(o6_right_8/v1)
|
||||
|
||||
O6 使用与 L6 相同的三机位八 Tag 观测拓扑,但保留 O6 自己的六通道协议与
|
||||
URDF 关节名:正面 ID0/1/2 标定 `rh_thumb_cmc_pitch` 与
|
||||
`rh_thumb_ip`,侧面 ID3/4/5 标定 `rh_pinky_mcp_pitch` 与
|
||||
`rh_pinky_dip`,上面 ID6/7 标定 `rh_thumb_cmc_yaw`。六路 baseline 均为
|
||||
`255`;每次只扫描通道 0、1 或 5,其他通道保持 255。经硬件确认,食指、中指、
|
||||
无名指与小指同机构,因此小指 MCP/DIP 的实测结果会以迁移来源标记后用于其余
|
||||
三指。O6 实测 IP/DIP 均存在稳定的非线性,因此使用通过独立 holdout 的双向
|
||||
运行曲线和二次耦合模型。标准 URDF 无法表示二次 mimic,因此修正 URDF 与 L6
|
||||
一样保留端点对齐的线性 `<mimic>`,使普通 URDF/RViz 中五个被动关节能正常联动,
|
||||
同时把被动关节 limit 更新为实测范围;中间行程的精确双向非线性轨迹由下述标定桥
|
||||
发布。
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrate_hand --config \
|
||||
src/linkerhand_calibration/config/o6_right_product.yaml --validate-only
|
||||
|
||||
ros2 run linkerhand_calibration calibrate_hand --config \
|
||||
src/linkerhand_calibration/config/o6_right_product.yaml
|
||||
```
|
||||
|
||||
由于 O6 的 FRONT/TOP 光轴接近正交,同一平面棋盘的同步视角天然更倾斜。O6 外参
|
||||
允许最终批次 RMS 不超过 1.5 px,但仍保持 0.3° 旋转、1.5 mm 平移稳定性门限,
|
||||
并在标定发布前额外用 20 mm 跨机位实体轴线 RMS 粗差门限拦截移动相机等明显错误:
|
||||
|
||||
```bash
|
||||
ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \
|
||||
output_file:=$PWD/config/o6_three_camera_extrinsics.yaml \
|
||||
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \
|
||||
maximum_reprojection_rms_px:=1.5
|
||||
```
|
||||
|
||||
结果发布到 `calibration_output/<O6串号>/latest_partial_passed`,运行时 JSON、
|
||||
修正 URDF 与 correction-input JSON 均使用 `o6_right_` 前缀。产品 YAML 内的实物
|
||||
串号、相机身份、外参和四项输入哈希必须在启动硬件前通过校验。
|
||||
|
||||
标定完成后,以修正 URDF 启动 robot state publisher,并用同一会话中的 JSON 把
|
||||
O6 六路反馈转换为 11 个 URDF 关节:
|
||||
|
||||
```bash
|
||||
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
|
||||
hand_type:=right \
|
||||
calibration_file:=$PWD/calibration_output/O6_RIGHT_001/latest_partial_passed/o6_right_O6_RIGHT_001_partial_calibration.json
|
||||
```
|
||||
|
||||
## 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
|
||||
```
|
||||
|
||||
拟合通过后,程序先原子写入并重新校验
|
||||
`l6_right_<序列号>_urdf_correction_input.json`,验证串号、Profile 和源 URDF
|
||||
SHA-256 后,才把其中的原精度零偏、行程和 DIP 耦合参数交给现有 URDF 写回器。
|
||||
面向运行时的 schema-v6 `*_partial_calibration.json` 及修正 URDF 的字段、数值和
|
||||
格式保持兼容;单独的 correction-input JSON 是可审计的 URDF 生成依据,不是运行桥
|
||||
的输入文件。
|
||||
|
||||
运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、
|
||||
roll、index、middle、ring、pinky;旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路
|
||||
兼容别名读取,新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。
|
||||
预检和正式扫描均使用 L6 硬件速度 `1` 作为上限,并由 100 Hz 余弦缓入缓出
|
||||
轨迹把完整 `255↔0` 行程固定为 `6 s`;短行程按距离同比缩短。SDK 会过滤 L6
|
||||
在同一 CAN ID 回送的位置命令回显,标定只使用状态查询返回的真实反馈。标定
|
||||
启动时还会检查重复 SDK/GUI 发布者,避免两个进程同时访问同一只手。
|
||||
|
||||
标定完成后,用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节
|
||||
`JointState`:
|
||||
|
||||
```bash
|
||||
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
|
||||
hand_type:=right \
|
||||
calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json
|
||||
```
|
||||
|
||||
schema v6 默认订阅 `/l6/cb_right_hand_state`,发布
|
||||
`/sim/mujoco/l6/right/joint_state`。标准 URDF 的 `<mimic>` 本身只支持线性关系,
|
||||
所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似;需要实测轨迹时使用该桥
|
||||
或修正 URDF 内的 MuJoCo equality。
|
||||
|
||||
## G20右手正式一键标定
|
||||
|
||||
固定三相机和19张Tag安装完成后,用户只运行:
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrate_hand
|
||||
```
|
||||
|
||||
旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名;
|
||||
旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。
|
||||
|
||||
构建和正式调用统一为:
|
||||
|
||||
```bash
|
||||
colcon build --packages-select linkerhand_calibration
|
||||
ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml>
|
||||
```
|
||||
|
||||
## 代码边界
|
||||
|
||||
- `core/`:无 ROS、无具体型号,包含领域类型、PnP/旋转数学、拟合接口、统一样本
|
||||
契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。其中
|
||||
`core/urdf/patch.py` 是所有型号共用的声明式 URDF patch engine,统一负责属性级
|
||||
文本修改、MuJoCo equality、mesh 安全复制、禁止覆盖和原子发布。
|
||||
- `runtime/`:通用会话状态机与注册 Profile 分发;ROS 消息和硬件适配只能位于
|
||||
`runtime/nodes`、`runtime/adapters`。
|
||||
- `models/g20/`、`models/l6/`:只保留型号 Profile、拟合/零位策略以及把拟合结果
|
||||
转成 `UrdfPatchSet` 的薄适配层,不再各自实现 XML/mesh 文件写入器。新增 O6 时
|
||||
优先新增 Profile;只有测量链或传动模型不同的部分才增加小型拟合插件。
|
||||
后续型号或左右手作为新的独立 Profile 加入 `models/`,不在通用层增加分支。
|
||||
- `compat/`:v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留
|
||||
一版最小转发 shim,不包含算法副本。
|
||||
|
||||
产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、
|
||||
Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改;v2 配置使用
|
||||
`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明,通用层
|
||||
不要求 `front/side/top`,也不假设固定 20 个命令。
|
||||
|
||||
仓库中存在已审定硬件会话时,可执行只读金标准检查(不会覆盖任何产物):
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration validate_g20_goldens \
|
||||
calibration_output/G20_RIGHT_001
|
||||
```
|
||||
|
||||
它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话;完整
|
||||
整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。
|
||||
|
||||
完全独立地只标定大拇指4项任务时,使用:
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrate_hand \
|
||||
--scope thumb
|
||||
```
|
||||
|
||||
该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机
|
||||
方向观测以及拇指自己的机械端点;输出URDF从原始CAD生成,只修改4个拇指主动
|
||||
关节,12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`,其JSON是
|
||||
拇指标定/诊断产物,不冒充可直接运行的完整整手曲线JSON。
|
||||
|
||||
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定,才额外使用:
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrate_hand \
|
||||
--scope thumb \
|
||||
--base-session calibration_output/G20_RIGHT_001/latest_passed
|
||||
```
|
||||
|
||||
合并模式会冻结基础会话中的12个四指主动零位;发布前再次读取基础会话JSON核对,
|
||||
任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。
|
||||
|
||||
拇指专项结果重复性通过后,不必再做原来的16项整手扫描。以该拇指会话为基础,
|
||||
只重新采集12项四指任务并合成完整整手URDF:
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration calibrate_hand \
|
||||
--scope fingers \
|
||||
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
|
||||
```
|
||||
|
||||
`fingers`模式严格冻结基础会话中的4个拇指主动零位;最终完整整手URDF中的拇指
|
||||
零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的
|
||||
独立拇指内核,再冻结这4个结果求解12个四指零位,因此四指数据不能反向改写
|
||||
拇指结果。原来的默认`full`仍保留,用于需要16项全部重新采集的情况。
|
||||
|
||||
采样文件中的运动域是显式且不可混用的:
|
||||
`requested_command_u8` 表示下发命令,`feedback_u8` 表示电机反馈。
|
||||
在线拟合和离线重放通过同一个数据契约投影到曲线索引;新会话不会把含糊的
|
||||
`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
|
||||
`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`,完成复核后才允许机械手运动。
|
||||
|
||||
提供`--base-session`时,它必须解析到同一序列号目录下的完整PASS会话;启动前会校验源CAD
|
||||
URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF,不在旧校准
|
||||
URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务,其余任务的原始记录导入后仍按
|
||||
数据契约和产物哈希检查,但不会以历史四指拟合结果否决本次拇指专项标定。
|
||||
|
||||
开发阶段若上一次会话失败,同一命令会自动校验硬件/几何哈希,并恢复已经
|
||||
完整提交的关节任务;失败中的当前任务始终丢弃重做,位于它后面但已经完整通过的
|
||||
独立任务仍会复用,不再因“连续前缀”限制整段重采。导入的任务会立即用与
|
||||
最终验收相同的硬门限复检(不含视口实时有效率):只以预警带余量通过的旧数据
|
||||
当场剔除并从其在扫掠顺序中的原始位置重采,避免全部任务采完后才在最终验收
|
||||
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记;
|
||||
恢复时只读取该标记之后的替代采集,不能把同一尝试编号下重扫前后的稳态点合并。
|
||||
因此已经在线硬门限验收的任务保持已完成,暂停中的任务从任务开头重采,不会因
|
||||
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
|
||||
独立保留;单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
|
||||
轴线位置若也能明确定位为单轮异常,同样只补扫该轮;补扫会保留任务预检和前次
|
||||
采集确定的PnP分支参考,不会因重新初始化切换到另一组平面Tag镜像解。侧面
|
||||
校验视角的任务级有效率只记录为诊断;G20右手预检若逐帧识别率低于标称值,
|
||||
但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞,也按完整
|
||||
轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查,轴线、曲线和模型质量
|
||||
门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
|
||||
采集事务:相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
|
||||
产品审定速度,不再根据单次识别密度自动提速,确保不同会话测量的是同一动态
|
||||
过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查:前三轮
|
||||
极差默认不得超过0.5°,95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
|
||||
紧密簇、仅另一轮越界,自动补采该轮两个方向;无法明确定位时只重采完整yaw任务,
|
||||
不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入
|
||||
`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装,但该历史差值
|
||||
不直接否决当前会话,也不会用旧结果约束新零位。需要强制从第一个关节
|
||||
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
|
||||
一个完整同步任务断点;只有两边数据都完整时才复用。
|
||||
|
||||
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
|
||||
16个会话数据求解主动关节URDF零位修正、
|
||||
21条视觉实测命令曲线发布;四指PIP/DIP的动态曲线均实测,四指DIP静态零位保留CAD。
|
||||
终端只显示中文进度和问题;失败时复制“请复制以下内容给开发者”块即可。
|
||||
|
||||
四指末端的16 mm Tag允许使用刚性延长杆避挡;软件不假设末端Tag平面与中节Tag
|
||||
平面平行。延长杆和Tag在一次标定期间必须完全刚性,不能晃动、扭转或重新调整。
|
||||
侧面掌部基准Tag(ID 4)与各活动指节Tag也不要求安装面平行:首次联合PnP使用
|
||||
静态多帧刚性、重投影误差和跨轮任务参考选择分支,不再用固定15°安装角门限阻断扫描。
|
||||
单Tag独立位姿仍保留75°倾角保护;对包含锁定掌部基准和完整父子链的任务,倾角保护只
|
||||
限制独立选择,不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧
|
||||
绝对/相对位姿连续性约束这些斜视候选,最终轴线残差、四轮重复性和隔离留出门限不放宽。
|
||||
终端中的Tag计数表示“可见”;等待扫描起点时会另列PnP初始化进度和累计拒绝原因。
|
||||
若联合候选仍然失败,`raw_samples.jsonl` 会按8个反馈计数的区间保存
|
||||
`group_pnp_candidate_event`,其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参
|
||||
哈希,可直接定位运动到哪个机械位置后开始失效,而不需要再次盲扫整条流程。
|
||||
|
||||
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
|
||||
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
|
||||
全部通过后更新。
|
||||
|
||||
## G20右手19-Tag底层调试入口
|
||||
|
||||
以下内容仅保留给旧会话回放和开发调试;正式一键命令只发布上面的精简
|
||||
schema v4 JSON,不再生成schema v5运行文件。
|
||||
|
||||
新布局用独立参数启用,原有左右手11-Tag流程仍默认使用
|
||||
`tag_layout:=legacy_11`,两套配置和结果schema互不覆盖:
|
||||
|
||||
```bash
|
||||
ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
|
||||
hand_type:=right \
|
||||
tag_layout:=g20_right_19 \
|
||||
serial_number:=G20_RIGHT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>
|
||||
```
|
||||
|
||||
当前产品布局共19张`tag36h11`,所有Tag的黑色码区边长均为16 mm;四指末节ID为
|
||||
`7,14,16,18`。正面为`0,1,2,3,10,11,12,13`,侧面为
|
||||
`4,5,6,7,14,15,16,17,18`,上面为`8,9`。详细角色和逐ID尺寸以
|
||||
`config/three_camera_tags_g20_right_19.yaml`为唯一软件配置源。程序执行4项拇指任务,
|
||||
以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
|
||||
侧面PIP/DIP联合任务,共16个物理运动任务。同步roll只驱动电机一次,但两台相机仍分别拟合并通过
|
||||
各自的观测质量门限。每个PIP任务只驱动一次对应电机,同时用“手掌→中节Tag”实测PIP、
|
||||
用“中节Tag→末节Tag”实测被动DIP;四个DIP不再由URDF mimic系数生成,并参加完整视觉
|
||||
拟合和质量门限。每项正式四轮之前自动低速往返预检
|
||||
0/127/255可见性;
|
||||
|
||||
基准形态恢复完成后,程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
|
||||
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0,因此四指正面+侧面同步roll中
|
||||
允许ID 0暂时不可见,并使用本会话基准锁定值;运动连杆Tag仍必须实时可见,门限不
|
||||
放宽。终端用`锁`表示该固定参考有效,例如`正面[0锁,12✓]`,`✗`才表示需要处理的
|
||||
实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手,否则缓存参考
|
||||
失效,必须重新启动标定;两次独立会话之间轻微调整整只手的位置不会改变机械端点
|
||||
零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计;
|
||||
任务结束后角色要求会切回预检全套标签,静止期帧不参与该门限,避免把
|
||||
采集质量良好的任务误判为可见性失败。
|
||||
|
||||
恢复末端Tag后,程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
|
||||
相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差;实体手在反馈0端
|
||||
能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch
|
||||
和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出,不再把跨相机的
|
||||
平面PnP绝对相位直接当作编码器零位,也不写死为0。生成URDF时同步修正这些关节的
|
||||
坐标上限以及相关被动关节的mimic坐标偏置,
|
||||
保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
|
||||
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、
|
||||
四指MCP roll、MCP pitch及PIP,共16个。`thumb_mcp`与四指屈伸关节一样使用
|
||||
实测全屈曲行程和CAD机械端点联合求解,不直接采用MCP/IP耦合运动的单目PnP相位。
|
||||
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入,并按逆序分阶段退出;
|
||||
同类辅助电机(全部邻指滚转、全部PIP、全部MCP pitch)合并为同一个并行航点同时
|
||||
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
||||
等已评审不变量保持不变,过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
|
||||
`parallel_pose_transitions`(默认true)置false可回退旧的逐电机顺序。
|
||||
同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行,只在任务切换时退出,
|
||||
不再每轮重复展开/弯曲辅助手指。跨手指组切换时,下一组避障姿态仍然需要、且
|
||||
当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机
|
||||
退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的
|
||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
||||
不超过8,只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值,
|
||||
不会因本次预检帧率或识别密度而改变;旧11-Tag布局仍保留自适应速度兼容逻辑。
|
||||
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理
|
||||
零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
||||
四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。
|
||||
预检、正式四轮和拟合重扫始终使用速度5。
|
||||
正式roll的每个方向会在经过127时先到位保持0.5秒,再独立保存至少10帧静止Tag/反馈;
|
||||
方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再
|
||||
替代静态保持姿态。
|
||||
前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
||||
每个任务只在低速递减预检起点执行一次8帧PnP静态初始化;预检往返和四轮正式
|
||||
扫描连续复用同一帧间分支与任务参考,不再让每一轮独立选择平面Tag解。同一任务第1轮
|
||||
已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的
|
||||
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中
|
||||
排除与MCP同步运动明显矛盾(残差超过7.5°)的ID3镜像候选。该先验不生成或缩放
|
||||
`thumb_ip`曲线;通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。
|
||||
|
||||
当前19-Tag产品流程发布精简schema v4:21条运行时曲线全部来自当前会话的视觉实测。
|
||||
URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`,共16个,
|
||||
其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程
|
||||
与机械端点联合求解;
|
||||
写出非零`thumb_mcp`零偏时同步平移其关节坐标上限,并更新被动`thumb_ip`的
|
||||
`mimic offset`,因此不会改变CAD定义的最大屈曲实体姿态;
|
||||
`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容,
|
||||
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合;方向、
|
||||
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
|
||||
相对侧相机视线倾斜约13°~20°,平面标签的单目IPPE姿态二义性会给侧视姿态引入
|
||||
数度的系统性"绕视线"偏差(亚像素重投影无法发现,会话20260820_105535实测
|
||||
前后轴向稳定相差11.4°),因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收,
|
||||
正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中
|
||||
严格平行:小指作为先采集的参考轴,其余三指复用该公共方向并各自独立拟合轴线位置,
|
||||
避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响,其纯旋转拟合得到的是
|
||||
随手指结构变化的伪轴线,不能与侧视PIP连杆的物理轴线使用统一距离门限;
|
||||
两者线距仅记录在诊断中。侧视校验通道
|
||||
(`*_mcp_roll_side`)的
|
||||
姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断,不触发重复采集;这两个量来自
|
||||
近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变,真正发布的正面主轴
|
||||
仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断,组合轴线改用四轮
|
||||
位置RMS验收;径向、平面、圆一致性及可见性门限全部保留。
|
||||
侧面端视roll的圆轨迹方向已经受
|
||||
姿态轴约束,因此自由三维圆平面与姿态轴的夹角只保留诊断,不再被重复作为硬门限;
|
||||
径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的
|
||||
二维投影圆角度,侧面姿态曲线仅保留为诊断;任一正式视角自身四轮不重复或第四轮
|
||||
留出失败仍会拒绝发布。任一静态目标、第四轮留出、
|
||||
遮挡、PnP或跨机位检查失败时,只保留原始轨迹和`passed:false`诊断,不发布正式URDF。
|
||||
8个组合姿态仅保留为开发诊断,正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
|
||||
位置验收的手基座变换,因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
|
||||
`a609d521`验证过的完整四轮相对旋转曲线;全部实测关节均由隔离第四轮逐关节验收。
|
||||
现场需要区分某根手指的roll机构回差与单机位误差时,可设置
|
||||
`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
|
||||
正面+侧面同步roll,共10个预检/正式方向;任一机位数据不足会重扫同一物理任务,
|
||||
双机位数据齐全后即使存在轴质量失败也不再自动重采,而是把失败项随双机位结果
|
||||
一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久
|
||||
锁定URDF发布,不能用`resume`转换成正式标定。
|
||||
schema v5明确声明曲线输入域为真实反馈u8;运行桥默认订阅
|
||||
`/g20/cb_right_hand_state`,并按反馈增减方向选择正程/反程曲线,停止时锁存最后运动
|
||||
方向。尚未观察到运动方向时使用`255→127`标准分支,不使用两个机械分支的平均值。
|
||||
schema v4继续兼容旧
|
||||
命令域。两者都只发布动态角度,不重复叠加已写入URDF的静态偏移。
|
||||
|
||||
## 三机位三维关节轴零位标定(schema v4)
|
||||
|
||||
@@ -47,7 +438,7 @@ ID 9 必须在拇指横摆的完整行程中持续可见。
|
||||
|
||||
```bash
|
||||
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
ros2 launch linkerhand_calibration \
|
||||
three_camera_extrinsics.launch.py \
|
||||
output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
|
||||
@@ -63,11 +454,11 @@ PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后,棋盘稳定1秒
|
||||
外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中,每改变一次位置和倾角添加
|
||||
一个候选;随后以相同方法采集正面/上面。程序使用固定内参的
|
||||
`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离:FRONT和
|
||||
配对相机的单帧RMS分别不得超过1.5 px,同时组合RMS不得超过1.2 px;界面中
|
||||
配对相机的单帧RMS分别不得超过1.5 px,候选组合RMS不得超过1.5 px;界面中
|
||||
单相机1.2 px以内显示绿色、1.2~1.5 px显示黄色且仍可采集、超过1.5 px显示红色。
|
||||
新姿态会与全部已采姿态比较,避免在少数姿态间反复采集。拟合先剔除粗大异常组,
|
||||
再在不低于15个内点的前提下有界裁剪联合误差最高的候选,最终1.2 px门限不会被
|
||||
放宽。两组均得到
|
||||
再在不低于15个内点的前提下有界裁剪联合误差最高的候选,只有最终批次组合RMS
|
||||
不超过1.2 px才允许保存。两组均得到
|
||||
至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。
|
||||
|
||||
```bash
|
||||
@@ -88,12 +479,11 @@ ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
|
||||
先使用禁止运动模式检查三个机位、外参、内参和标签:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
ros2 launch linkerhand_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=left \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||||
commands_enabled:=false
|
||||
```
|
||||
|
||||
@@ -111,19 +501,20 @@ ros2 run image_view image_view --ros-args \
|
||||
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后,重启正式流程:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
ros2 launch linkerhand_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=left \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||||
can_interface:=can0
|
||||
```
|
||||
|
||||
右手使用同一入口;默认自动选择右手SDK话题和原始URDF:
|
||||
原始URDF及其mesh随`linkerhand_calibration`安装,默认根据`hand_type`自动选择。
|
||||
如需调试其他CAD版本,仍可通过`source_urdf_path:=<绝对路径>`显式覆盖。
|
||||
右手使用同一入口,并自动选择右手SDK话题和原始URDF:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
ros2 launch linkerhand_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=right \
|
||||
serial_number:=G20_RIGHT_001 \
|
||||
@@ -177,7 +568,7 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
|
||||
|
||||
前两轮拟合,第三轮强制留出验证;轨迹与零位角度MAE必须≤1°、P95≤2°,三轮轴/零位
|
||||
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF,并通过按三轮分组的
|
||||
95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
|
||||
训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
|
||||
|
||||
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定:主动关节上限2.5°,被动
|
||||
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差,
|
||||
@@ -194,13 +585,14 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
|
||||
约4°的整指倾斜;重复扫描与同源留出不能排除这种系统偏差,因此不得写入URDF。
|
||||
|
||||
四指MCP屈伸和PIP采用同一静态策略:参考指轨迹仍参与动态曲线、轴质量和机构诊断,
|
||||
但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
|
||||
修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证;代码和配置中不保存任何
|
||||
按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
|
||||
但拟合出的共同掌坐标相位不写入四指 `origin.rpy`;只发布各指相对四指中值的实测
|
||||
装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械
|
||||
端点,视觉轴链继续用于轴线、PnP和留出诊断;代码和配置中不保存任何按左右手或
|
||||
序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。
|
||||
|
||||
7个直接零位依赖链为:yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
|
||||
拇指pitch;IP轴线相位仅作诊断,不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
|
||||
约束roll,PIP/DIP轴线相位只用于参考指机构诊断,不再覆盖四指CAD静态零位。
|
||||
视觉依赖链仍为:yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指
|
||||
pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断,不再决定四个具有机械端点
|
||||
的拇指主动关节绝对零位;四指PIP/DIP轴线相位也继续用于机构诊断。
|
||||
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
|
||||
保持145,求解器使用实测 `angle_rad[145]` 还原该条件,不会把145误当成baseline。
|
||||
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计,因此状态和原始JSONL
|
||||
@@ -209,18 +601,22 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
|
||||
|
||||
生成修正URDF时只修改通过验收的主动关节 `origin.rpy`,不会修改任何关节的
|
||||
`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
|
||||
JSON;实测曲线即使略微越过CAD限位,也不能自动扩大URDF限位。
|
||||
JSON;任一曲线点越过CAD限位都会阻止正式发布,程序不会自动扩大URDF限位。
|
||||
|
||||
坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
|
||||
重置当前机位PnP、返回基准后重扫当前方向,最多3次;速度依次降为80%/60%/50%,
|
||||
端点保持延长到0.75/1.0/1.25秒,扫描超时按降速比例同步延长。若反馈在远离目标时
|
||||
连续8秒没有至少1个u8的进展,则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
|
||||
暂停,不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向,全局不一致才重扫
|
||||
完整关节,每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警,最终
|
||||
拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败,程序
|
||||
完整关节,每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色
|
||||
预警,但只要仍超过硬门限,就在当前关节立即使用剩余重试预算
|
||||
(`provisional_fit_warning_rescan`);第三次仍超限则当场暂停,不允许预警数据继续到
|
||||
后续关节。最终拟合仍按原硬门限验收,因此不会在全部任务采完后才回头重采靠前
|
||||
关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败,程序
|
||||
只暂停一次且不再自动重扫,防止重复运动;此时也拒绝`resume`形成死循环。其他可恢复
|
||||
失败在预算耗尽后才暂停,`resume`从最小失败单元继续,已通过数据保留。所有失败尝试
|
||||
仍保存在 `raw_samples.jsonl`。
|
||||
仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的
|
||||
PnP双簇行程,程序将它判为系统性分支失败并当场停止,不再浪费第3次全关节重扫。
|
||||
|
||||
每个新机位/Tag组合开始运动前,不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
|
||||
程序在静止端点联合8帧候选,按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
|
||||
@@ -273,27 +669,37 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_calibration.json
|
||||
src/.../g20_left/
|
||||
g20_left_G20_LEFT_001_calibration_urdf_correction_input.json
|
||||
linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
|
||||
calibration_output/G20_RIGHT_001/<时间戳>/
|
||||
g20_right_G20_RIGHT_001_calibration.json
|
||||
src/.../g20_right/
|
||||
g20_right_G20_RIGHT_001_calibration_urdf_correction_input.json
|
||||
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
```
|
||||
|
||||
`*_urdf_correction_input.json` 会在修正 URDF 之前落盘并重新读取,且绑定源 URDF
|
||||
哈希、型号、侧别、layout 和序列号。公开 schema-v4 运行 JSON 仍保持原格式,原有
|
||||
G20 运行桥、金标准哈希和 URDF 写回数值不因该交接层改变。
|
||||
|
||||
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
|
||||
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
||||
指定原始CAD文件生成,采用 `T_original × Rot(axis, offset)`,绝不叠加旧校准文件或
|
||||
覆盖原文件;只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
|
||||
改变,未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
|
||||
改变;当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解;四指DIP和
|
||||
`thumb_ip`为被动关节,发布实测动态曲线并保留CAD静态零位,其mimic坐标偏置只随
|
||||
上游主动关节坐标系变换作等价调整。
|
||||
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
|
||||
同一会话,保证会话内URDF可独立加载,并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
|
||||
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
||||
|
||||
完整 `raw_samples.jsonl` 已存在时,可以按当前算法离线重放,不连接相机、不发送电机
|
||||
命令。`--output-tag` 为新产物增加安全后缀,已有JSON、URDF和验证报告不会被覆盖:
|
||||
|
||||
```bash
|
||||
python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
||||
python3 -m linkerhand_calibration.offline_replay \
|
||||
calibration_output/G20_RIGHT_001/20260811_120146 \
|
||||
--output-tag AXIS_FRAME_V3 \
|
||||
--write
|
||||
@@ -310,12 +716,13 @@ python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
||||
`JointState`(包括5个被动关节):
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
|
||||
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
|
||||
hand_type:=right \
|
||||
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
|
||||
```
|
||||
|
||||
默认订阅 `/cb_right_hand_control_cmd`,发布
|
||||
schema v5默认订阅 `/g20/cb_right_hand_state`;schema v4默认订阅
|
||||
`/g20/cb_right_hand_control_cmd`。两者均发布
|
||||
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥,避免两个
|
||||
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令,
|
||||
因此不会静默退回旧标定。
|
||||
@@ -381,7 +788,7 @@ sudo apt-get install -y \
|
||||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
colcon build --symlink-install \
|
||||
--packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
|
||||
--packages-select linker_hand_ros2_sdk linkerhand_calibration
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
@@ -408,7 +815,7 @@ source install/setup.bash
|
||||
先单独启动相机(不会连接机械手,也不会发送关节命令):
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
|
||||
ros2 run linkerhand_calibration hikrobot_camera_node --ros-args \
|
||||
--remap __ns:=/camera/camera/color \
|
||||
-p serial_number:=DB2163742 \
|
||||
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
@@ -446,7 +853,7 @@ ros2 run camera_calibration cameracalibrator \
|
||||
但标定节点不会发送位置运动命令,也不会允许解锁全行程扫描:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_serial_number:=DB2163742 \
|
||||
commands_enabled:=false
|
||||
@@ -457,7 +864,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
速度,并使用单终点连续运动:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_serial_number:=DB2163742 \
|
||||
can_interface:=can0 \
|
||||
@@ -570,7 +977,7 @@ calibration_output/<序列号>/<时间戳>/
|
||||
恢复时必须显式复用原目录,否则会创建新会话:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
|
||||
```
|
||||
@@ -636,7 +1043,7 @@ for name, joint in data["joints"].items():
|
||||
不影响 AprilTag 的 ROI 输入。
|
||||
|
||||
静态预检先在单 Tag 层拒绝高重投影误差,再检查三组相对中心的位置内点率和毫米级 RMS。
|
||||
当前 30~38 px 的 10 mm Tag 属于试标定尺寸,如果中心位置 RMS 持续不合格,应优先增加照明、缩短
|
||||
当前末节16 mm Tag如果中心位置 RMS 持续不合格,应优先增加照明、缩短
|
||||
相机距离或提高 Tag 有效像素,而不是放宽最终随机复测精度。
|
||||
|
||||
启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`,避免新增一个全分辨率图像
|
||||
@@ -650,7 +1057,7 @@ for name, joint in data["joints"].items():
|
||||
URDF:
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
ros2 launch linkerhand_calibration \
|
||||
front_cmc_pitch_zero.launch.py \
|
||||
serial_number:=G20_LEFT_001
|
||||
```
|
||||
@@ -727,7 +1134,7 @@ zero_angles.table_projected_zero_rad
|
||||
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量,不读取T3标签朝向。
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
ros2 launch linkerhand_calibration \
|
||||
front_cmc_roll_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001
|
||||
```
|
||||
+1
-1
@@ -44,7 +44,7 @@ g20_thumb_calibration:
|
||||
minimum_detection_hz: 15.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
# Trial threshold for the current 10 mm tags (observed at 32-38 px).
|
||||
# Trial threshold for small/far tags (historically observed at 32-38 px).
|
||||
# Final acceptance is still guarded by static RMS and random validation.
|
||||
minimum_edge_pixels: 30.0
|
||||
# Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.
|
||||
@@ -0,0 +1,37 @@
|
||||
schema_version: 1
|
||||
model: G20
|
||||
side: right
|
||||
tag_layout: g20_right_19
|
||||
serial_number: G20_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/g20_right/linkerhand_g20_right.urdf
|
||||
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||
|
||||
release:
|
||||
# Each task already contains three training cycles plus an isolated fourth
|
||||
# holdout, so a second complete hardware session duplicates hours of motion.
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -0,0 +1,59 @@
|
||||
/l6_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2]
|
||||
frames: [front_base, thumb_pitch, thumb_dip]
|
||||
sizes: [0.016, 0.016, 0.016]
|
||||
|
||||
/l6_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [3, 4, 5]
|
||||
frames: [side_base, pinky_pitch, pinky_dip]
|
||||
sizes: [0.016, 0.016, 0.016]
|
||||
|
||||
/l6_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [6, 7]
|
||||
frames: [top_base, thumb_roll]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,37 @@
|
||||
schema_version: 2
|
||||
profile_id: L6/right/l6_right_8/v1
|
||||
model: L6
|
||||
side: right
|
||||
tag_layout: l6_right_8
|
||||
namespace: /l6_calibration
|
||||
serial_number: L6_RIGHT_001
|
||||
can_interface: can0
|
||||
output_root: calibration_output
|
||||
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
|
||||
artifacts:
|
||||
source_urdf: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf
|
||||
source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
|
||||
calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f
|
||||
tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
|
||||
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
|
||||
|
||||
release:
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -0,0 +1,67 @@
|
||||
l6_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /l6/cb_right_hand_control_cmd
|
||||
state_topic: /l6/cb_right_hand_state
|
||||
setting_topic: /l6/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /l6_calibration/front/camera/camera_info
|
||||
front_detections_topic: /l6_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /l6_calibration/side/camera/camera_info
|
||||
side_detections_topic: /l6_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /l6_calibration/top/camera/camera_info
|
||||
top_detections_topic: /l6_calibration/top/apriltag/detections
|
||||
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255]
|
||||
# L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10,
|
||||
# which left fewer than 32 useful feedback bins. Speed 1 is still only a
|
||||
# firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s.
|
||||
# A 100 Hz cosine trajectory therefore sets the actual, model-level pace.
|
||||
preflight_speed_u8: 1
|
||||
formal_speed_u8: 1
|
||||
speed_settle_seconds: 0.2
|
||||
command_trajectory_full_range_seconds: 6.0
|
||||
torque_u8: 80
|
||||
repetitions: 4
|
||||
preflight_checkpoints_u8: [255, 127, 0]
|
||||
tag_size_m: 0.016
|
||||
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
|
||||
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
minimum_detection_rate: 0.95
|
||||
# Per-Tag quality remains >=95%. With three independently detected Tags,
|
||||
# the fully joined frame rate may be 0.95^3 ~= 85.7%.
|
||||
minimum_joint_frame_rate: 0.85
|
||||
minimum_feedback_hz: 25.0
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_reprojection_tie_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
maximum_validation_error_deg: 3.0
|
||||
mimic_minimum_multiplier: 0.5
|
||||
mimic_maximum_multiplier: 1.5
|
||||
mimic_maximum_cycle_range: 0.03
|
||||
mimic_maximum_residual_p95_deg: 2.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
endpoint_hold_seconds: 1.0
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
automatic_sweep_retry_limit: 2
|
||||
non_target_motion_tolerance_u8: 3.0
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
@@ -0,0 +1,59 @@
|
||||
/o6_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_ip]
|
||||
sizes: [0.016, 0.016, 0.016]
|
||||
|
||||
/o6_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]
|
||||
|
||||
/o6_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_yaw]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,37 @@
|
||||
schema_version: 2
|
||||
profile_id: O6/right/o6_right_8/v1
|
||||
model: O6
|
||||
side: right
|
||||
tag_layout: o6_right_8
|
||||
namespace: /o6_calibration
|
||||
serial_number: O6_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/o6_right/linkerhand_o6_right.urdf
|
||||
source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
|
||||
camera_extrinsics: config/o6_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
|
||||
calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
|
||||
calibration_config_sha256: ce20d998a4342dfaacb14568513aa9af5063df48566fabd42180acc8da47e4a6
|
||||
tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml
|
||||
tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
|
||||
|
||||
release:
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -0,0 +1,64 @@
|
||||
o6_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /o6/cb_right_hand_control_cmd
|
||||
state_topic: /o6/cb_right_hand_state
|
||||
setting_topic: /o6/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /o6_calibration/front/camera/camera_info
|
||||
front_detections_topic: /o6_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /o6_calibration/side/camera/camera_info
|
||||
side_detections_topic: /o6_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /o6_calibration/top/camera/camera_info
|
||||
top_detections_topic: /o6_calibration/top/apriltag/detections
|
||||
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255]
|
||||
# O6 has a different speed scale from L6. Motion is still bounded by the
|
||||
# six-second cosine command trajectory; these values are firmware limits.
|
||||
baseline_speed_u8: 80
|
||||
preflight_speed_u8: 60
|
||||
formal_speed_u8: 40
|
||||
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
|
||||
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: 2.2
|
||||
mimic_maximum_cycle_range: 0.03
|
||||
mimic_maximum_residual_p95_deg: 2.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
endpoint_hold_seconds: 1.0
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
automatic_sweep_retry_limit: 2
|
||||
non_target_motion_tolerance_u8: 3.0
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
@@ -0,0 +1,193 @@
|
||||
g20_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /g20/cb_left_hand_control_cmd
|
||||
state_topic: /g20/cb_left_hand_state
|
||||
info_topic: /g20/cb_left_hand_info
|
||||
setting_topic: /g20/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /g20_calibration/front/camera/camera_info
|
||||
front_detections_topic: /g20_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /g20_calibration/side/camera/camera_info
|
||||
side_detections_topic: /g20_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /g20_calibration/top/camera/camera_info
|
||||
top_detections_topic: /g20_calibration/top/apriltag/detections
|
||||
|
||||
# /start先下发并确认这个20通道基准姿态,稳定后才进入第一条扫描。
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
normal_calibration_speed: 15
|
||||
index_roll_calibration_speed: 5
|
||||
index_flex_calibration_speed: 10
|
||||
# 19-Tag产品预检仍使用上面保守速度;只有正反预检都留出至少双倍正式分箱余量,
|
||||
# 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束,
|
||||
# 始终保持速度5;任一方向采样余量不足也保持原速度。
|
||||
adaptive_formal_speed_enabled: true
|
||||
adaptive_formal_speed_max_scale: 1.5
|
||||
adaptive_formal_speed_minimum_bins: 64
|
||||
adaptive_formal_speed_maximum_bin_gap: 8
|
||||
speed_setting_settle_seconds: 0.25
|
||||
|
||||
# tag36h11尺寸是检测角点围成的黑色正方形边长,不包含外围白边。
|
||||
# 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与
|
||||
# apriltag_ros逐ID尺寸一致,禁止用纸张/白边尺寸代替码区尺寸。
|
||||
tag_size_m: 0.016
|
||||
# ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个
|
||||
# 末端Tag仍显式写16 mm,防止节点启动时得到未初始化参数。
|
||||
tag_size_override_ids: [7, 14, 16, 18]
|
||||
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016]
|
||||
repetitions: 3
|
||||
# 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出;旧11-Tag仍读取repetitions=3。
|
||||
g20_right_19_repetitions: 4
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_detection_hz: 15.0
|
||||
minimum_feedback_hz: 25.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
|
||||
# 标定任务不再用第一帧决定平面Tag的IPPE分支;静止端点联合8帧选择整组最稳定解。
|
||||
pnp_group_initialization_frames: 8
|
||||
# 侧面当前任务所需Tag在初始化端点的贴面法向应一致;用此先验消除静态IPPE镜像双解。
|
||||
pnp_group_normal_alignment_scale_deg: 5.0
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见;
|
||||
# 任一角点相对会话基准漂移超过2 px并连续5帧时,判定标定中基准被移动。
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时,用源URDF mimic
|
||||
# 关系辅助选择IPPE分支;若全部候选超限则退回纯视觉,绝不丢帧,也不生成、
|
||||
# 缩放或替代被动IP的自身Tag实测曲线。
|
||||
thumb_ip_pnp_coupling_multiplier: 1.03
|
||||
thumb_ip_pnp_coupling_scale_deg: 3.0
|
||||
thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
|
||||
top_pnp_invalid_reset_seconds: 1.0
|
||||
# 三维位姿必须与实测20通道状态严格按时间戳配对。
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
|
||||
axis_maximum_plane_rms_m: 0.003
|
||||
# 被动耦合轴只用轨迹确定轴线位置,允许更大的轴向深度噪声;径向和跨轮
|
||||
# 轴线一致性仍沿用严格检查。
|
||||
passive_axis_maximum_plane_rms_m: 0.004
|
||||
axis_maximum_radial_rms_m: 0.003
|
||||
# 整段相对SE(3)运动拟合轴线点;端视关节会投影掉单目PnP光轴深度。
|
||||
axis_maximum_pose_line_rms_m: 0.001
|
||||
# 仅用于运动平面在三维中可观测的斜视关节;近图像平面关节使用姿态轴
|
||||
# 约束三维圆,不让单目平面Tag的深度噪声自由决定转轴方向。
|
||||
axis_maximum_rotation_circle_difference_deg: 1.0
|
||||
# 单轴模型残差与跨轮重复误差分开判定:主动刚性关节要求更严;被动耦合
|
||||
# 关节允许可重复的非理想单轴分量,但仍须通过0.75°跨轮轴差及最终轮留出。
|
||||
active_maximum_rotation_orthogonal_rms_deg: 2.5
|
||||
passive_maximum_rotation_orthogonal_rms_deg: 7.5
|
||||
zero_maximum_axis_cycle_difference_deg: 0.75
|
||||
# 零位无法改变父子轴夹角;超过该值属于CAD/PnP几何错误,不能吸收到零位。
|
||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||
zero_maximum_observability_condition_number: 10000000000.0
|
||||
zero_maximum_offset_deg: 20.0
|
||||
# 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
|
||||
# 求解且不假定电气端点等于CAD上限;thumb_mcp及四指MCP pitch/PIP
|
||||
# 静态零位由实测全行程与CAD机械端点联合求解,不写死为0。
|
||||
zero_finger_maximum_offset_deg: 3.0
|
||||
# 只对实物已确认等同CAD端点的关节使用该限制;CMC电气端点不作此假设。
|
||||
mechanical_endpoint_maximum_offset_deg: 5.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区,但反馈
|
||||
# 必须已经稳定;大残差仍由机械卡滞保护处理。
|
||||
steady_checkpoint_command_feedback_tolerance_u8: 8.0
|
||||
steady_checkpoint_maximum_feedback_range_u8: 2.0
|
||||
# 电机10在命令0时实测会稳定反馈为4;该0端使用±4。
|
||||
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
|
||||
# 右手电机10在命令255时多次实测稳定反馈为250;仅右手该端点使用±5。
|
||||
right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
|
||||
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5;仅其0端使用±5。
|
||||
pinky_pip_zero_endpoint_tolerance_u8: 5.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
# roll零位127必须从两个方向到位并静止采集,禁止用运动中经过127的帧判回差。
|
||||
baseline_hold_seconds: 0.5
|
||||
minimum_baseline_hold_frames: 10
|
||||
# 19-Tag产品每项正式四轮前先做一次低速往返,端点Tag稳定至少2秒。
|
||||
task_precheck_hold_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
# 启动宽限1秒后,反馈连续2秒没有至少1个u8的进展,按机械卡滞立即暂停;
|
||||
# 这类故障不进入遮挡/超时的三次自动重扫。
|
||||
# 低速5也应持续产生反馈进展;5秒无进展即停,减少机构持续顶死时间。
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
motor_stall_startup_grace_seconds: 1.0
|
||||
motor_stall_minimum_progress_u8: 1.0
|
||||
invalid_timeout_seconds: 3.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
# 可恢复的采样失败自动重扫当前方向;超过次数才暂停等待人工处理。
|
||||
automatic_sweep_retry_limit: 2
|
||||
# 轨迹拟合失败优先只重扫失败轮次;零位/URDF模型失败不重复运动。
|
||||
automatic_fit_retry_limit: 2
|
||||
automatic_motion_retry_limit: 2
|
||||
# 留空为正式标定;设为pinky/ring/middle/index时只采该指正面+侧面roll,
|
||||
# 即使正面baseline回差失败也继续完成侧面对照,并永久锁定本会话URDF发布。
|
||||
cross_view_roll_diagnostic_finger: ""
|
||||
# 过程检查允许25%的黄色预警带,最终验收仍使用下面的严格门限。
|
||||
provisional_warning_ratio: 1.25
|
||||
retry_minimum_speed: 3
|
||||
retry_speed_scales: [0.8, 0.6]
|
||||
retry_endpoint_hold_seconds: [0.75, 1.0]
|
||||
|
||||
trajectory_maximum_plane_rms_m: 0.004
|
||||
trajectory_maximum_radial_rms_m: 0.004
|
||||
trajectory_minimum_radius_m: 0.003
|
||||
trajectory_minimum_arc_deg: 15.0
|
||||
# 以下二维参数只供旧轨迹工具兼容,三机位v4零位不使用二维投影。
|
||||
image_trajectory_maximum_radial_rms_px: 2.0
|
||||
image_trajectory_maximum_radial_p95_px: 3.5
|
||||
image_trajectory_minimum_radius_px: 20.0
|
||||
trajectory_maximum_cycle_travel_difference_deg: 3.0
|
||||
passive_maximum_cycle_travel_difference_deg: 10.0
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
# 旧布局仍用连续扫描正反程差门限;19-Tag产品的连续运动包含速度相关滞后,
|
||||
# 由方向曲线和最终留出验证建模,不再重复硬判。其绝对正反程门禁使用下面
|
||||
# 的九点稳态command_maximum_direction_gap_deg。
|
||||
maximum_hysteresis_deg: 2.0
|
||||
# 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
|
||||
# 不超过0.5°;四指roll例外:127以255→127为唯一物理零位,反向分支
|
||||
# 保留实测偏差,并改为检查分支间隙上限及跨轮稳定性。
|
||||
baseline_maximum_hysteresis_deg: 0.5
|
||||
directional_zero_maximum_branch_gap_deg: 2.0
|
||||
directional_zero_maximum_branch_gap_range_deg: 0.3
|
||||
cross_view_roll_maximum_branch_gap_difference_deg: 0.3
|
||||
# 正面roll是Tag中心的二维投影角,侧面roll是三维姿态角。允许一个有界的
|
||||
# 固定比例吸收Tag安装倾角/偏置带来的投影缩放,再严格比较两条曲线形状;
|
||||
# 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。
|
||||
cross_view_roll_maximum_shape_rms_deg: 1.25
|
||||
cross_view_roll_maximum_projection_scale_ratio: 1.5
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 2.0
|
||||
# 九点姿态先按实测反馈重新对齐,再用此门限检查固有机械回差;请求命令域
|
||||
# 的两条运行曲线仍原样保留固件方向死区,不能把command/feedback差算成回差。
|
||||
command_maximum_direction_gap_deg: 2.0
|
||||
|
||||
# 默认无额外随机动作;19-Tag产品最终一轮始终作为不可关闭的留出验证。
|
||||
validation_enabled: false
|
||||
# 第四轮留出求解后必须再走8个固定安全组合姿态;三机位规定Tag全部可见
|
||||
# 且实测20通道到位才允许发布。只保存Tag位姿,不保存原始图像。
|
||||
# Developer diagnostic only. The formal fourth sweep cycle already gives
|
||||
# every isolated PIP/DIP pair an independent holdout.
|
||||
combination_validation_enabled: false
|
||||
combination_validation_frames: 10
|
||||
combination_maximum_position_p95_m: 0.003
|
||||
combination_maximum_orientation_p95_deg: 2.0
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
# 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
|
||||
maximum_validation_error_deg: 3.0
|
||||
zero_maximum_confidence_half_width_deg: 1.5
|
||||
@@ -0,0 +1,62 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 10, 11, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 15, 17]
|
||||
frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [8, 9]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,63 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 10, 11, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
# Distal Tags use the same measured 16 mm black-code edge as all others.
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 7, 14, 15, 16, 17, 18]
|
||||
frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [8, 9]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.016, 0.016]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""One-release compatibility surface for the former Python package name.
|
||||
|
||||
New code must import :mod:`linkerhand_calibration`. Only the documented
|
||||
configuration loader is re-exported here; calibration algorithms continue to
|
||||
have a single implementation in the renamed package.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import warnings
|
||||
|
||||
warnings.warn(
|
||||
"g20_thumb_apriltag_calibration is deprecated; "
|
||||
"import linkerhand_calibration instead",
|
||||
DeprecationWarning,
|
||||
stacklevel=2,
|
||||
)
|
||||
|
||||
from linkerhand_calibration.product import ProductConfig, load_product_config
|
||||
|
||||
__all__ = ["ProductConfig", "load_product_config"]
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
"""Deprecated forwarding entry point for the runtime joint-state bridge."""
|
||||
|
||||
from linkerhand_calibration.calibrated_joint_state_bridge import main
|
||||
|
||||
__all__ = ["main"]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,9 @@
|
||||
"""Deprecated forwarding entry point for offline replay."""
|
||||
|
||||
from linkerhand_calibration.offline_replay import main
|
||||
|
||||
__all__ = ["main"]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,9 @@
|
||||
"""Deprecated forwarding entry point for the former Python package."""
|
||||
|
||||
from linkerhand_calibration.one_command import main
|
||||
|
||||
__all__ = ["main"]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+3
-3
@@ -1,4 +1,4 @@
|
||||
"""Publish calibrated G20 URDF joint angles from raw u8 commands."""
|
||||
"""Publish profile-calibrated URDF angles from raw command/feedback u8 values."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
@@ -14,10 +14,10 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument("input_topic", default_value=""),
|
||||
DeclareLaunchArgument("output_topic", default_value=""),
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="calibrated_joint_state_bridge",
|
||||
name=[
|
||||
"g20_calibrated_joint_state_bridge_",
|
||||
"calibrated_joint_state_bridge_",
|
||||
LaunchConfiguration("hand_type"),
|
||||
],
|
||||
output="screen",
|
||||
+3
-3
@@ -48,7 +48,7 @@ def _launch_stack(context):
|
||||
tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
zero_config = LaunchConfiguration("zero_config").perform(context)
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
@@ -158,7 +158,7 @@ def _launch_stack(context):
|
||||
)
|
||||
|
||||
zero_node = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="cmc_pitch_zero_node",
|
||||
name="g20_thumb_cmc_pitch_zero",
|
||||
output="screen",
|
||||
@@ -203,7 +203,7 @@ def _launch_stack(context):
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
return LaunchDescription(
|
||||
[
|
||||
+3
-3
@@ -50,7 +50,7 @@ def _launch_stack(context):
|
||||
"calibration_config"
|
||||
).perform(context)
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
@@ -160,7 +160,7 @@ def _launch_stack(context):
|
||||
)
|
||||
|
||||
calibration_node = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="cmc_roll_calibration_node",
|
||||
name="g20_thumb_cmc_roll_calibration",
|
||||
output="screen",
|
||||
@@ -205,7 +205,7 @@ def _launch_stack(context):
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
return LaunchDescription(
|
||||
[
|
||||
+3
-3
@@ -82,7 +82,7 @@ def _launch_stack(context):
|
||||
)
|
||||
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
@@ -234,7 +234,7 @@ def _launch_stack(context):
|
||||
)
|
||||
|
||||
calibration = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="calibration_node",
|
||||
name="g20_thumb_calibration",
|
||||
output="screen",
|
||||
@@ -315,7 +315,7 @@ def _launch_stack(context):
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
default_output = str(Path.cwd() / "calibration_output")
|
||||
return LaunchDescription(
|
||||
+146
-34
@@ -1,8 +1,9 @@
|
||||
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
|
||||
"""Launch three Hikrobot views and one registered hand calibration owner."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
import hashlib
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
@@ -25,33 +26,90 @@ from launch_ros.parameter_descriptions import ParameterValue
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
def _default_source_urdf(hand_type: str) -> Path:
|
||||
relative = Path(
|
||||
"assets/robots/hands/linker_hand"
|
||||
) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
|
||||
workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
|
||||
def _default_source_urdf(model: str, hand_type: str) -> Path:
|
||||
relative = (
|
||||
Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf"
|
||||
if model.upper() == "L6" and hand_type == "right"
|
||||
else Path("urdf")
|
||||
/ f"{model.lower()}_{hand_type}"
|
||||
/ f"linkerhand_{model.lower()}_{hand_type}.urdf"
|
||||
)
|
||||
package_source_or_share = Path(__file__).resolve().parents[1] / relative
|
||||
try:
|
||||
installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
|
||||
installed = (
|
||||
Path(get_package_share_directory("linkerhand_calibration"))
|
||||
/ relative
|
||||
)
|
||||
except Exception:
|
||||
installed = workspace
|
||||
return workspace if workspace.is_file() else installed
|
||||
installed = package_source_or_share
|
||||
return installed if installed.is_file() else package_source_or_share
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
from linkerhand_calibration.product import (
|
||||
get_product_calibration_contract,
|
||||
)
|
||||
|
||||
model = LaunchConfiguration("model").perform(context).strip().upper()
|
||||
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise RuntimeError("hand_type must be left or right")
|
||||
command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"/g20/cb_{hand_type}_hand_state"
|
||||
info_topic = f"/g20/cb_{hand_type}_hand_info"
|
||||
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
|
||||
try:
|
||||
contract = get_product_calibration_contract(
|
||||
model, hand_type, tag_layout
|
||||
)
|
||||
except ValueError as error:
|
||||
raise RuntimeError(str(error)) from error
|
||||
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
package_share = Path(
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
tag_config = (
|
||||
Path(requested_tag_config).expanduser().resolve()
|
||||
if requested_tag_config
|
||||
else package_share
|
||||
/ "config"
|
||||
/ (
|
||||
"three_camera_tags_g20_right_19.yaml"
|
||||
if tag_layout == "g20_right_19"
|
||||
else "o6_right_8_tags.yaml"
|
||||
if tag_layout == "o6_right_8"
|
||||
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"
|
||||
)
|
||||
)
|
||||
if not tag_config.is_file():
|
||||
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
||||
topic_prefix = f"/{model.lower()}"
|
||||
command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"{topic_prefix}/cb_{hand_type}_hand_state"
|
||||
info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info"
|
||||
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
|
||||
source_urdf = (
|
||||
Path(requested_source).expanduser().resolve()
|
||||
if requested_source
|
||||
else _default_source_urdf(hand_type).resolve()
|
||||
else _default_source_urdf(model, hand_type).resolve()
|
||||
)
|
||||
if not source_urdf.is_file():
|
||||
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
|
||||
expected_source_hash = LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
).perform(context).strip().lower()
|
||||
if contract.typed_profile.artifacts.publish_corrected_urdf:
|
||||
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||
raise RuntimeError(
|
||||
"this profile requires source_urdf_expected_sha256 confirmed "
|
||||
"by the CAD/hardware owner"
|
||||
)
|
||||
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
|
||||
if actual_source_hash != expected_source_hash:
|
||||
raise RuntimeError(
|
||||
"source_urdf_expected_sha256 does not match source_urdf_path"
|
||||
)
|
||||
|
||||
hand_serial = LaunchConfiguration("serial_number").perform(context)
|
||||
if (
|
||||
@@ -90,19 +148,20 @@ def _launch_stack(context):
|
||||
raw_topics = []
|
||||
info_topics = []
|
||||
detection_topics = []
|
||||
calibration_namespace = contract.typed_profile.namespace
|
||||
for view in VIEWS:
|
||||
namespace = f"/g20_calibration/{view}/camera"
|
||||
namespace = f"{calibration_namespace}/{view}/camera"
|
||||
raw_topic = f"{namespace}/image_raw"
|
||||
info_topic = f"{namespace}/camera_info"
|
||||
rect_topic = f"{namespace}/image_rect"
|
||||
detector_namespace = f"/g20_calibration/{view}/apriltag"
|
||||
detector_namespace = f"{calibration_namespace}/{view}/apriltag"
|
||||
detection_topic = f"{detector_namespace}/detections"
|
||||
raw_topics.append(raw_topic)
|
||||
info_topics.append(info_topic)
|
||||
detection_topics.append(detection_topic)
|
||||
cameras.append(
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
@@ -118,7 +177,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(
|
||||
@@ -163,7 +224,7 @@ def _launch_stack(context):
|
||||
name="apriltag",
|
||||
namespace=detector_namespace,
|
||||
parameters=[
|
||||
LaunchConfiguration("tag_config"),
|
||||
str(tag_config),
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
@@ -181,7 +242,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",
|
||||
@@ -198,10 +259,10 @@ def _launch_stack(context):
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": hand_type,
|
||||
"hand_joint": "G20",
|
||||
"hand_joint": model,
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"topic_prefix": topic_prefix,
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
@@ -213,10 +274,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,
|
||||
@@ -224,28 +285,52 @@ def _launch_stack(context):
|
||||
],
|
||||
)
|
||||
calibration = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="three_camera_calibration_node",
|
||||
name="g20_calibration",
|
||||
name=f"{model.lower()}_calibration",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
arguments=[
|
||||
"--profile-id",
|
||||
contract.typed_profile.key.profile_id,
|
||||
],
|
||||
parameters=[
|
||||
LaunchConfiguration("calibration_config"),
|
||||
{
|
||||
"serial_number": hand_serial,
|
||||
"model": model,
|
||||
"hand_type": hand_type,
|
||||
"tag_layout": tag_layout,
|
||||
"session_dir": str(session_dir),
|
||||
"resume_raw_samples_path": LaunchConfiguration(
|
||||
"resume_raw_samples_path"
|
||||
),
|
||||
"recalibration_scope": LaunchConfiguration(
|
||||
"recalibration_scope"
|
||||
),
|
||||
# The SDK performs roughly 25 synchronous CAN queries whenever
|
||||
# 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(
|
||||
"camera_extrinsics_file"
|
||||
),
|
||||
"source_urdf_path": str(source_urdf),
|
||||
"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"
|
||||
),
|
||||
@@ -267,6 +352,13 @@ def _launch_stack(context):
|
||||
LaunchConfiguration("index_flex_calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"adaptive_formal_speed_enabled": ParameterValue(
|
||||
LaunchConfiguration("adaptive_formal_speed_enabled"),
|
||||
value_type=bool,
|
||||
),
|
||||
"cross_view_roll_diagnostic_finger": LaunchConfiguration(
|
||||
"cross_view_roll_diagnostic_finger"
|
||||
),
|
||||
"validation_enabled": ParameterValue(
|
||||
LaunchConfiguration("validation_enabled"), value_type=bool
|
||||
),
|
||||
@@ -293,14 +385,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} three-camera session: {session_dir}; "
|
||||
f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; "
|
||||
f"source_urdf={source_urdf}"
|
||||
)
|
||||
),
|
||||
@@ -321,7 +413,7 @@ def _launch_stack(context):
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
info_root = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
@@ -341,7 +433,9 @@ def generate_launch_description() -> LaunchDescription:
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("model", default_value="G20"),
|
||||
DeclareLaunchArgument("hand_type", default_value="left"),
|
||||
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_serial", default_value="DB2163742"
|
||||
@@ -387,6 +481,12 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument(
|
||||
"index_flex_calibration_speed", default_value="10"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"adaptive_formal_speed_enabled", default_value="true"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"cross_view_roll_diagnostic_finger", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument("validation_enabled", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_extrinsics_file",
|
||||
@@ -397,6 +497,18 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument(
|
||||
"source_urdf_path", default_value=""
|
||||
),
|
||||
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=""
|
||||
),
|
||||
@@ -409,6 +521,8 @@ def generate_launch_description() -> LaunchDescription:
|
||||
default_value=str(Path.cwd() / "calibration_output"),
|
||||
),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
|
||||
DeclareLaunchArgument("recalibration_scope", default_value="full"),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(
|
||||
@@ -417,9 +531,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "three_camera_tags.yaml"
|
||||
),
|
||||
default_value="",
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
+13
-3
@@ -26,7 +26,7 @@ def _launch(context):
|
||||
namespace = f"/g20_extrinsics/{view}/camera"
|
||||
cameras.append(
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
@@ -85,7 +85,7 @@ def _launch(context):
|
||||
output="screen",
|
||||
)
|
||||
solver = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
package="linkerhand_calibration",
|
||||
executable="three_camera_extrinsics_node",
|
||||
name="g20_camera_extrinsics",
|
||||
output="screen",
|
||||
@@ -112,6 +112,12 @@ def _launch(context):
|
||||
LaunchConfiguration("maximum_reprojection_rms_px"),
|
||||
value_type=float,
|
||||
),
|
||||
"maximum_candidate_pair_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration(
|
||||
"maximum_candidate_pair_reprojection_rms_px"
|
||||
),
|
||||
value_type=float,
|
||||
),
|
||||
"maximum_single_camera_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration(
|
||||
"maximum_single_camera_reprojection_rms_px"
|
||||
@@ -138,7 +144,7 @@ def _launch(context):
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
)
|
||||
camera_info = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
@@ -182,6 +188,10 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument(
|
||||
"maximum_reprojection_rms_px", default_value="1.2"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_candidate_pair_reprojection_rms_px",
|
||||
default_value="1.5",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_single_camera_reprojection_rms_px",
|
||||
default_value="1.5",
|
||||
@@ -0,0 +1,5 @@
|
||||
"""Profile-driven LinkerHand calibration and validated URDF correction."""
|
||||
|
||||
from .core import CalibrationProfile, ProfileKey
|
||||
|
||||
__all__ = ["CalibrationProfile", "ProfileKey"]
|
||||
+8
-3
@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .core import PAIR_NAMES, robust_rotation_summary
|
||||
from .core import robust_rotation_summary
|
||||
from .pnp import SquareTagPose
|
||||
|
||||
|
||||
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
|
||||
"t3_t4": ("t3", "t4"),
|
||||
"t4_t5": ("t4", "t5"),
|
||||
}
|
||||
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -116,7 +117,7 @@ def interpolate_state_u8(
|
||||
*,
|
||||
maximum_skew_ns: int,
|
||||
) -> tuple[tuple[float, ...], int] | None:
|
||||
"""Interpolate the 20-D hand state at an image timestamp.
|
||||
"""Interpolate a profile-sized hand state at an image timestamp.
|
||||
|
||||
The SDK publishes state independently from the camera. Continuous
|
||||
calibration must therefore use the image timestamp instead of whichever
|
||||
@@ -150,7 +151,11 @@ def interpolate_state_u8(
|
||||
fraction = before_gap / denominator
|
||||
before_values = np.asarray(before.position_u8, dtype=float)
|
||||
after_values = np.asarray(after.position_u8, dtype=float)
|
||||
if before_values.shape != (20,) or after_values.shape != (20,):
|
||||
if (
|
||||
before_values.ndim != 1
|
||||
or before_values.size == 0
|
||||
or after_values.shape != before_values.shape
|
||||
):
|
||||
return None
|
||||
interpolated = before_values + fraction * (after_values - before_values)
|
||||
return (
|
||||
@@ -0,0 +1,336 @@
|
||||
"""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
|
||||
dynamic ``angle_rad`` values and never adds the static offsets a second time.
|
||||
|
||||
Schema-v5 trajectories are fitted against timestamp-synchronised hardware
|
||||
feedback, not controller set-points. They must therefore be queried with the
|
||||
SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for
|
||||
backwards compatibility only.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .full_hand import (
|
||||
get_hand_calibration_profile,
|
||||
infer_compact_payload_layout,
|
||||
validate_compact_payload,
|
||||
)
|
||||
from .models import get_default_registry, validate_schema_v6_runtime_payload
|
||||
from .core import ProfileKey
|
||||
|
||||
|
||||
G20_COMMAND_NAMES: tuple[str, ...] = (
|
||||
"thumb_cmc_pitch",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"reserved_11",
|
||||
"reserved_12",
|
||||
"reserved_13",
|
||||
"reserved_14",
|
||||
"thumb_mcp",
|
||||
"index_pip",
|
||||
"middle_pip",
|
||||
"ring_pip",
|
||||
"pinky_pip",
|
||||
)
|
||||
|
||||
# Match the stable ordering used by the existing MuJoCo bridge. JointState
|
||||
# consumers must use names, but retaining the ordering also keeps logs and
|
||||
# direct comparisons deterministic.
|
||||
G20_URDF_JOINT_NAMES: tuple[str, ...] = (
|
||||
"index_dip",
|
||||
"index_mcp_pitch",
|
||||
"index_mcp_roll",
|
||||
"index_pip",
|
||||
"middle_dip",
|
||||
"middle_mcp_pitch",
|
||||
"middle_mcp_roll",
|
||||
"middle_pip",
|
||||
"pinky_dip",
|
||||
"pinky_mcp_pitch",
|
||||
"pinky_mcp_roll",
|
||||
"pinky_pip",
|
||||
"ring_dip",
|
||||
"ring_mcp_pitch",
|
||||
"ring_mcp_roll",
|
||||
"ring_pip",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_ip",
|
||||
"thumb_mcp",
|
||||
)
|
||||
|
||||
|
||||
class CalibratedCommandMapper:
|
||||
"""Validated, profile-specific lookup from SDK u8 values to URDF radians."""
|
||||
|
||||
def __init__(
|
||||
self, payload: Mapping[str, Any], *, expected_side: str | None = None
|
||||
) -> None:
|
||||
schema_version = int(payload["schema_version"])
|
||||
if schema_version == 6:
|
||||
validate_schema_v6_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(
|
||||
f"calibration side {side!r} does not match requested side "
|
||||
f"{str(expected_side).lower()!r}"
|
||||
)
|
||||
quality = payload["quality"]
|
||||
if quality.get("passed") is not True:
|
||||
raise ValueError("calibration quality.passed must be true")
|
||||
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"])
|
||||
self.input_domain = str(
|
||||
payload.get(
|
||||
"curve_input_domain",
|
||||
"command_u8" if schema_version == 4 else "",
|
||||
)
|
||||
)
|
||||
if self.input_domain not in {"command_u8", "feedback_u8"}:
|
||||
raise ValueError("calibration curve_input_domain is invalid")
|
||||
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
|
||||
}
|
||||
registered = get_default_registry().get(
|
||||
ProfileKey.parse(self.profile_id)
|
||||
)
|
||||
self.feedback_name_aliases = dict(
|
||||
registered.profile.command.feedback_name_aliases
|
||||
)
|
||||
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 self.urdf_joint_names
|
||||
}
|
||||
self._decreasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"decreasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
self._increasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"increasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in self.urdf_joint_names
|
||||
}
|
||||
self._previous_by_motor: dict[int, float] = {}
|
||||
self._direction_by_motor: dict[int, str] = {}
|
||||
self.direction_deadband_u8 = 0.5
|
||||
|
||||
@staticmethod
|
||||
def _command_index(value: float) -> int:
|
||||
command = float(value)
|
||||
if not math.isfinite(command):
|
||||
raise ValueError("calibrated command positions must be finite")
|
||||
return max(0, min(255, int(math.floor(command + 0.5))))
|
||||
|
||||
def map_positions(
|
||||
self, positions: Sequence[float], names: Sequence[str] = ()
|
||||
) -> tuple[float, ...]:
|
||||
values = tuple(float(value) for value in positions)
|
||||
if names:
|
||||
if len(names) != len(values):
|
||||
raise ValueError(
|
||||
"JointState names and positions must have equal length"
|
||||
)
|
||||
if len(set(names)) != len(names):
|
||||
raise ValueError("JointState names must be unique")
|
||||
by_name = dict(zip((str(name) for name in names), values))
|
||||
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(
|
||||
f"{self.model} feedback is missing named channels: "
|
||||
+ ",".join(missing)
|
||||
)
|
||||
command = tuple(by_name[name] for name in self.command_names)
|
||||
else:
|
||||
if len(values) != len(self.command_names):
|
||||
raise ValueError(
|
||||
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)
|
||||
direction_by_motor: dict[int, str | None] = {}
|
||||
for motor, value in enumerate(command):
|
||||
previous = self._previous_by_motor.get(motor)
|
||||
direction = self._direction_by_motor.get(motor)
|
||||
if previous is not None:
|
||||
if value > previous + self.direction_deadband_u8:
|
||||
direction = "increasing"
|
||||
elif value < previous - self.direction_deadband_u8:
|
||||
direction = "decreasing"
|
||||
direction_by_motor[motor] = direction
|
||||
result: list[float] = []
|
||||
for name in self.urdf_joint_names:
|
||||
motor = self._motor_by_joint[name]
|
||||
direction = direction_by_motor[motor]
|
||||
curves = (
|
||||
self._increasing_curves
|
||||
if direction == "increasing"
|
||||
else self._decreasing_curves
|
||||
if direction == "decreasing"
|
||||
else self._curves
|
||||
)
|
||||
result.append(curves[name][indices[motor]])
|
||||
for motor, value in enumerate(command):
|
||||
self._previous_by_motor[motor] = value
|
||||
direction = direction_by_motor[motor]
|
||||
if direction is not None:
|
||||
self._direction_by_motor[motor] = direction
|
||||
return tuple(result)
|
||||
|
||||
|
||||
def load_calibrated_command_mapper(
|
||||
calibration_file: str | Path, *, expected_side: str | None = None
|
||||
) -> CalibratedCommandMapper:
|
||||
path = Path(calibration_file).expanduser().resolve()
|
||||
if not path.is_file():
|
||||
raise ValueError(f"calibration JSON does not exist: {path}")
|
||||
payload = json.loads(path.read_text(encoding="utf-8"))
|
||||
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
||||
|
||||
|
||||
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"/{str(model).lower()}/cb_{side}_hand_state"
|
||||
if input_domain == "command_u8":
|
||||
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__("calibrated_joint_state_bridge")
|
||||
self.declare_parameter("hand_type", "right")
|
||||
self.declare_parameter("calibration_file", "")
|
||||
self.declare_parameter("input_topic", "")
|
||||
self.declare_parameter("output_topic", "")
|
||||
|
||||
hand_type = str(self.get_parameter("hand_type").value).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
calibration_file = str(self.get_parameter("calibration_file").value)
|
||||
if not calibration_file:
|
||||
raise ValueError("calibration_file is required")
|
||||
self.mapper = load_calibrated_command_mapper(
|
||||
calibration_file, expected_side=hand_type
|
||||
)
|
||||
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, self.mapper.model
|
||||
)
|
||||
self.output_topic = (
|
||||
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(
|
||||
JointState, self.input_topic, self._command_callback, 10
|
||||
)
|
||||
self._last_error = ""
|
||||
self.get_logger().info(
|
||||
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}"
|
||||
)
|
||||
|
||||
def _command_callback(self, command: JointState) -> None:
|
||||
try:
|
||||
positions = self.mapper.map_positions(command.position, command.name)
|
||||
except ValueError as error:
|
||||
message = str(error)
|
||||
if message != self._last_error:
|
||||
self.get_logger().error(message)
|
||||
self._last_error = message
|
||||
return
|
||||
self._last_error = ""
|
||||
result = JointState()
|
||||
result.header = command.header
|
||||
result.name = list(self.mapper.urdf_joint_names)
|
||||
result.position = list(positions)
|
||||
self.publisher.publish(result)
|
||||
|
||||
|
||||
def main(args: Sequence[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node: CalibratedJointStateBridge | None = None
|
||||
try:
|
||||
node = CalibratedJointStateBridge()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,21 @@
|
||||
"""Compatibility adapters for one-release calibration migrations."""
|
||||
|
||||
from .config_v1 import (
|
||||
legacy_default_profile_key,
|
||||
product_profile_key,
|
||||
resolve_legacy_profile_alias,
|
||||
)
|
||||
from .defaults import (
|
||||
default_product_config_path,
|
||||
default_three_camera_config_path,
|
||||
)
|
||||
from .paths import resolve_renamed_package_path
|
||||
|
||||
__all__ = [
|
||||
"default_product_config_path",
|
||||
"default_three_camera_config_path",
|
||||
"legacy_default_profile_key",
|
||||
"product_profile_key",
|
||||
"resolve_legacy_profile_alias",
|
||||
"resolve_renamed_package_path",
|
||||
]
|
||||
@@ -0,0 +1,47 @@
|
||||
"""Identity migration for deployed product configuration schemas."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Mapping
|
||||
|
||||
from ..core import ProfileKey
|
||||
|
||||
|
||||
def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
|
||||
version = int(raw.get("schema_version", -1))
|
||||
if version == 2:
|
||||
key = ProfileKey.parse(str(raw.get("profile_id", "")))
|
||||
for field, actual in (
|
||||
("model", key.model),
|
||||
("side", key.side),
|
||||
("tag_layout", key.layout),
|
||||
):
|
||||
configured = str(raw.get(field, "")).strip()
|
||||
if configured and configured.lower() != actual.lower():
|
||||
raise ValueError(f"{field} differs from profile_id")
|
||||
return key
|
||||
if version != 1:
|
||||
raise ValueError("product config schema_version must be 1 or 2")
|
||||
model = str(raw.get("model", "")).strip().upper()
|
||||
side = str(raw.get("side", "")).strip().lower()
|
||||
layout = str(raw.get("tag_layout", "")).strip().lower()
|
||||
if not layout and (model, side) == ("G20", "right"):
|
||||
layout = "g20_right_19"
|
||||
return ProfileKey(model, side, layout, 1)
|
||||
|
||||
|
||||
def legacy_default_profile_key() -> ProfileKey:
|
||||
"""Preserve the former no-argument executable for one release."""
|
||||
return ProfileKey("G20", "right", "g20_right_19", 1)
|
||||
|
||||
|
||||
def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey:
|
||||
"""Map retired layout identifiers to their reviewed physical profile."""
|
||||
if (
|
||||
key.model == "G20"
|
||||
and key.side == "right"
|
||||
and key.layout == "g20_right_15"
|
||||
and key.revision == 1
|
||||
):
|
||||
return ProfileKey("G20", "right", "g20_right_19", 1)
|
||||
return key
|
||||
@@ -0,0 +1,16 @@
|
||||
"""One-release default selection for invocations without ``--config``."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def default_product_config_path() -> Path:
|
||||
share = Path(get_package_share_directory("linkerhand_calibration"))
|
||||
return share / "config/g20_right_product.yaml"
|
||||
|
||||
|
||||
def default_three_camera_config_path() -> Path:
|
||||
"""Resolve the installed calibration defaults through the ROS index."""
|
||||
share = Path(get_package_share_directory("linkerhand_calibration"))
|
||||
return share / "config/three_camera_calibration.yaml"
|
||||
@@ -0,0 +1,4 @@
|
||||
"""Legacy single-camera algorithms retained for one compatibility release."""
|
||||
from .session_v1 import uses_coupled_full_hand_zero_solver
|
||||
|
||||
__all__ = ["uses_coupled_full_hand_zero_solver"]
|
||||
@@ -0,0 +1,24 @@
|
||||
"""Version selection for replaying durable pre-v3 hardware sessions."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
def uses_coupled_full_hand_zero_solver(
|
||||
session_start: Mapping[str, Any],
|
||||
) -> bool:
|
||||
"""Return the solver contract recorded by the legacy session header.
|
||||
|
||||
Capabilities are not consulted by the live runtime. This adapter reads
|
||||
the durable v1 header only so offline replay can reproduce an artifact
|
||||
created before the independent thumb solver was introduced.
|
||||
"""
|
||||
capabilities = {
|
||||
str(value) for value in session_start.get("capabilities", ())
|
||||
}
|
||||
return (
|
||||
int(session_start.get("sample_schema_version", 1)) == 1
|
||||
and "palm_axis_side_channel_v2" in capabilities
|
||||
and "palm_axis_relative_motion_v3" not in capabilities
|
||||
)
|
||||
@@ -0,0 +1,52 @@
|
||||
"""Path compatibility for immutable v1 product configurations."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
_LEGACY_SOURCE_PREFIX = Path("src/g20_thumb_apriltag_calibration")
|
||||
_CURRENT_SOURCE_PREFIX = Path("src/linkerhand_calibration")
|
||||
|
||||
|
||||
def _resolve_package_uri(value: str, workspace: Path) -> Path | None:
|
||||
prefix = "package://"
|
||||
if not value.startswith(prefix):
|
||||
return None
|
||||
package_name, separator, relative = value[len(prefix) :].partition("/")
|
||||
if not separator or not package_name or not relative:
|
||||
raise ValueError(f"invalid ROS package resource path: {value}")
|
||||
workspace_candidate = (workspace / "src" / package_name / relative).resolve()
|
||||
if workspace_candidate.exists():
|
||||
return workspace_candidate
|
||||
try:
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
package_share = Path(get_package_share_directory(package_name))
|
||||
except Exception:
|
||||
return workspace_candidate
|
||||
return (package_share / relative).resolve()
|
||||
|
||||
|
||||
def resolve_renamed_package_path(value: str | Path, workspace: Path) -> Path:
|
||||
"""Resolve workspace paths, ROS package resources and the former prefix.
|
||||
|
||||
Deployed v1 product YAML files are kept byte-for-byte stable because the
|
||||
artifact paths participate in operational review. Existing paths always
|
||||
win; package URIs prefer a source-workspace copy, and the rename mapping
|
||||
is used only when the literal legacy path no longer exists.
|
||||
"""
|
||||
text = str(value).strip()
|
||||
package_resource = _resolve_package_uri(text, workspace)
|
||||
if package_resource is not None:
|
||||
return package_resource
|
||||
raw = Path(text).expanduser()
|
||||
candidate = raw if raw.is_absolute() else workspace / raw
|
||||
candidate = candidate.resolve()
|
||||
if candidate.exists() or raw.is_absolute():
|
||||
return candidate
|
||||
try:
|
||||
suffix = raw.relative_to(_LEGACY_SOURCE_PREFIX)
|
||||
except ValueError:
|
||||
return candidate
|
||||
return (workspace / _CURRENT_SOURCE_PREFIX / suffix).resolve()
|
||||
@@ -0,0 +1,76 @@
|
||||
"""Hardware- and model-independent calibration kernel."""
|
||||
|
||||
from .domain import (
|
||||
ArtifactPolicy,
|
||||
CalibrationProfile,
|
||||
CommandLayout,
|
||||
MeasurementPolicy,
|
||||
MeasurementSpec,
|
||||
MotionPolicy,
|
||||
ProfileKey,
|
||||
ProfileValidationError,
|
||||
QualityPolicy,
|
||||
SampleRecord,
|
||||
ScopePolicy,
|
||||
TagSpec,
|
||||
TaskSpec,
|
||||
ViewSpec,
|
||||
VisionRigSpec,
|
||||
ZeroSolvePolicy,
|
||||
validate_profile,
|
||||
)
|
||||
from .domain.task import (
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
DIRECTIONS,
|
||||
PHASE_ROOT,
|
||||
PHASE_TIP,
|
||||
)
|
||||
from .fitting import FitResult, isotonic_nonincreasing
|
||||
from .geometry import (
|
||||
delta_rotation_vector,
|
||||
fit_rotation_axis,
|
||||
image_plane_tag_quaternion_xyzw,
|
||||
normalize_quaternion_xyzw,
|
||||
relative_quaternion_xyzw,
|
||||
robust_rotation_summary,
|
||||
rotation_inlier_fraction,
|
||||
rotation_rms_rad,
|
||||
rotation_spread_rad,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"ArtifactPolicy",
|
||||
"CalibrationProfile",
|
||||
"CommandLayout",
|
||||
"DIRECTION_DECREASING",
|
||||
"DIRECTION_INCREASING",
|
||||
"DIRECTIONS",
|
||||
"FitResult",
|
||||
"MeasurementPolicy",
|
||||
"MeasurementSpec",
|
||||
"MotionPolicy",
|
||||
"PHASE_ROOT",
|
||||
"PHASE_TIP",
|
||||
"ProfileKey",
|
||||
"ProfileValidationError",
|
||||
"QualityPolicy",
|
||||
"SampleRecord",
|
||||
"ScopePolicy",
|
||||
"TagSpec",
|
||||
"TaskSpec",
|
||||
"ViewSpec",
|
||||
"VisionRigSpec",
|
||||
"ZeroSolvePolicy",
|
||||
"delta_rotation_vector",
|
||||
"fit_rotation_axis",
|
||||
"image_plane_tag_quaternion_xyzw",
|
||||
"isotonic_nonincreasing",
|
||||
"normalize_quaternion_xyzw",
|
||||
"relative_quaternion_xyzw",
|
||||
"robust_rotation_summary",
|
||||
"rotation_inlier_fraction",
|
||||
"rotation_rms_rad",
|
||||
"rotation_spread_rad",
|
||||
"validate_profile",
|
||||
]
|
||||
@@ -0,0 +1,5 @@
|
||||
"""Artifact schema and release validation contracts."""
|
||||
|
||||
from .release import ReleaseValidation, ReleaseValidator
|
||||
|
||||
__all__ = ["ReleaseValidation", "ReleaseValidator"]
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Release validation protocol used before atomic publication."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Mapping, Protocol
|
||||
|
||||
from ..domain import CalibrationProfile
|
||||
from ..urdf import UrdfCorrectionPlan
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ReleaseValidation:
|
||||
passed: bool
|
||||
errors: tuple[str, ...] = ()
|
||||
verified_hashes: Mapping[str, str] | None = None
|
||||
|
||||
|
||||
class ReleaseValidator(Protocol):
|
||||
def validate_release(
|
||||
self,
|
||||
profile: CalibrationProfile,
|
||||
plan: UrdfCorrectionPlan,
|
||||
calibration_json: Path,
|
||||
corrected_urdf: Path,
|
||||
) -> ReleaseValidation: ...
|
||||
@@ -0,0 +1,41 @@
|
||||
"""Calibration domain types."""
|
||||
|
||||
from .profile import (
|
||||
ArtifactPolicy,
|
||||
CalibrationProfile,
|
||||
CommandLayout,
|
||||
MeasurementPolicy,
|
||||
MeasurementSpec,
|
||||
MotionPolicy,
|
||||
ProfileKey,
|
||||
ProfileValidationError,
|
||||
QualityPolicy,
|
||||
ScopePolicy,
|
||||
TagSpec,
|
||||
TaskSpec,
|
||||
ViewSpec,
|
||||
VisionRigSpec,
|
||||
ZeroSolvePolicy,
|
||||
validate_profile,
|
||||
)
|
||||
from .sample import SampleRecord
|
||||
|
||||
__all__ = [
|
||||
"ArtifactPolicy",
|
||||
"CalibrationProfile",
|
||||
"CommandLayout",
|
||||
"MeasurementPolicy",
|
||||
"MeasurementSpec",
|
||||
"MotionPolicy",
|
||||
"ProfileKey",
|
||||
"ProfileValidationError",
|
||||
"QualityPolicy",
|
||||
"SampleRecord",
|
||||
"ScopePolicy",
|
||||
"TagSpec",
|
||||
"TaskSpec",
|
||||
"ViewSpec",
|
||||
"VisionRigSpec",
|
||||
"ZeroSolvePolicy",
|
||||
"validate_profile",
|
||||
]
|
||||
@@ -0,0 +1,390 @@
|
||||
"""Typed, hardware-independent calibration profile contracts."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import PurePath
|
||||
from typing import Mapping
|
||||
|
||||
|
||||
@dataclass(frozen=True, order=True)
|
||||
class ProfileKey:
|
||||
"""Stable identity for one independently reviewed hand profile."""
|
||||
|
||||
model: str
|
||||
side: str
|
||||
layout: str
|
||||
revision: int = 1
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
object.__setattr__(self, "model", str(self.model).strip().upper())
|
||||
object.__setattr__(self, "side", str(self.side).strip().lower())
|
||||
object.__setattr__(self, "layout", str(self.layout).strip().lower())
|
||||
object.__setattr__(self, "revision", int(self.revision))
|
||||
if not self.model or not self.side or not self.layout:
|
||||
raise ValueError("profile identity fields must be non-empty")
|
||||
if self.side not in {"left", "right"}:
|
||||
raise ValueError("profile side must be left or right")
|
||||
if self.revision < 1:
|
||||
raise ValueError("profile revision must be positive")
|
||||
|
||||
@property
|
||||
def profile_id(self) -> str:
|
||||
return f"{self.model}/{self.side}/{self.layout}/v{self.revision}"
|
||||
|
||||
@classmethod
|
||||
def parse(cls, value: str) -> "ProfileKey":
|
||||
parts = str(value).strip().split("/")
|
||||
if len(parts) != 4 or not parts[3].startswith("v"):
|
||||
raise ValueError(
|
||||
"profile_id must be MODEL/side/layout/vREVISION"
|
||||
)
|
||||
return cls(parts[0], parts[1], parts[2], int(parts[3][1:]))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CommandLayout:
|
||||
"""Command channels, joint bindings, and the reviewed baseline pose."""
|
||||
|
||||
names: tuple[str, ...]
|
||||
baseline_u8: tuple[int, ...]
|
||||
command_index_by_joint: Mapping[str, int]
|
||||
disabled_indices: frozenset[int] = frozenset()
|
||||
# Calibration names are allowed to stay model-neutral while the source
|
||||
# URDF keeps any vendor/side prefixes (for example ``rh_``).
|
||||
urdf_joint_by_joint: Mapping[str, str] = field(default_factory=dict)
|
||||
# Older SDKs occasionally published a wrong label for a physically stable
|
||||
# channel. Aliases are accepted only at the declared channel index.
|
||||
feedback_name_aliases: Mapping[str, str] = field(default_factory=dict)
|
||||
# SDK speed commands are not necessarily one value per position channel.
|
||||
# This mapping makes that protocol detail explicit in a profile.
|
||||
speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict)
|
||||
|
||||
@property
|
||||
def command_count(self) -> int:
|
||||
return len(self.names)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TagSpec:
|
||||
role: str
|
||||
tag_id: int
|
||||
fixed_reference: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ViewSpec:
|
||||
name: str
|
||||
tags: tuple[TagSpec, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class VisionRigSpec:
|
||||
"""Any number of named views and their Tag roles."""
|
||||
|
||||
views: tuple[ViewSpec, ...]
|
||||
common_frame: str
|
||||
extrinsic_reference_view: str
|
||||
extrinsics_quality_limits: Mapping[str, float] = field(
|
||||
default_factory=dict
|
||||
)
|
||||
minimum_capture_counts: Mapping[str, int] = field(default_factory=dict)
|
||||
|
||||
@property
|
||||
def view_names(self) -> tuple[str, ...]:
|
||||
return tuple(view.name for view in self.views)
|
||||
|
||||
@property
|
||||
def tag_ids(self) -> frozenset[int]:
|
||||
return frozenset(tag.tag_id for view in self.views for tag in view.tags)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TaskSpec:
|
||||
key: str
|
||||
view: str
|
||||
command_index: int
|
||||
joints: tuple[str, ...]
|
||||
auxiliary_commands: tuple[tuple[int, int], ...] = ()
|
||||
validation_only: bool = False
|
||||
start_u8: int = 255
|
||||
end_u8: int = 0
|
||||
preflight_speed_u8: int | None = None
|
||||
formal_speed_u8: int | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MotionPolicy:
|
||||
"""Reviewed motion tasks and optional safe waypoint sequences."""
|
||||
|
||||
tasks: tuple[TaskSpec, ...]
|
||||
preparation_waypoints_u8: tuple[tuple[int, ...], ...] = ()
|
||||
safe_return_waypoints_u8: tuple[tuple[int, ...], ...] = ()
|
||||
speed_parameters: Mapping[str, float] = field(default_factory=dict)
|
||||
precheck_sweeps: bool = False
|
||||
steady_command_checkpoints: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MeasurementSpec:
|
||||
joint: str
|
||||
kind: str
|
||||
view: str | None
|
||||
parent_role: str | None
|
||||
child_role: str | None
|
||||
validation_source: str | None = None
|
||||
# Some measured trajectories publish only a dynamic curve while their
|
||||
# static URDF zero/axis remains CAD- or mimic-owned. For those joints a
|
||||
# monocular 3-D axis-line residual is useful diagnostic evidence, but it
|
||||
# must not reject an otherwise clean image/SO(3) trajectory merely because
|
||||
# the hand was placed at a different valid position in the camera view.
|
||||
pose_axis_line_required: bool = True
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MeasurementPolicy:
|
||||
measurements: Mapping[str, MeasurementSpec]
|
||||
cross_view_sources: Mapping[str, str] = field(default_factory=dict)
|
||||
image_curve_joints: frozenset[str] = frozenset()
|
||||
directional_zero: bool = False
|
||||
cross_view_roll_curve: bool = False
|
||||
stable_cross_view_cone_bias: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ZeroSolvePolicy:
|
||||
active_joints: frozenset[str]
|
||||
passive_joints: frozenset[str]
|
||||
direct_zero_joints: tuple[str, ...]
|
||||
axis_joints: tuple[str, ...]
|
||||
mechanical_endpoint_joints: frozenset[str]
|
||||
post_solve_endpoint_joints: frozenset[str]
|
||||
mimic_source_by_joint: Mapping[str, str]
|
||||
cad_frozen_joints: frozenset[str]
|
||||
# ``upper_at_end`` means TaskSpec.end_u8 is the trusted source-URDF upper
|
||||
# physical endpoint. The measured travel then defines the electrical
|
||||
# zero and corrected [0, travel] coordinate range.
|
||||
endpoint_anchor_by_joint: Mapping[str, str] = field(default_factory=dict)
|
||||
fitted_mimic_joints: frozenset[str] = frozenset()
|
||||
# Passive coupling is not necessarily representable by the linear URDF
|
||||
# ``mimic`` element. Profiles must opt in explicitly before a nonlinear
|
||||
# runtime/MuJoCo relation may be published.
|
||||
coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class QualityPolicy:
|
||||
training_cycles: tuple[int, ...]
|
||||
holdout_cycle: int | None
|
||||
hard_threshold_keys: frozenset[str]
|
||||
retry_metric_scope: Mapping[str, str] = field(default_factory=dict)
|
||||
isolated_holdout: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ScopePolicy:
|
||||
calibrate_joints: Mapping[str, frozenset[str]]
|
||||
frozen_joints: Mapping[str, frozenset[str]]
|
||||
default_scope: str = "full"
|
||||
|
||||
def selected_joints(self, scope: str) -> frozenset[str]:
|
||||
try:
|
||||
return self.calibrate_joints[str(scope)]
|
||||
except KeyError as error:
|
||||
raise ValueError(f"unsupported calibration scope: {scope}") from error
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ArtifactPolicy:
|
||||
output_schema_version: int
|
||||
calibration_filename: str
|
||||
corrected_urdf_filename: str
|
||||
protected_input_fields: frozenset[str]
|
||||
publication_pointer: str = "latest_passed"
|
||||
session_compatibility_tokens: frozenset[str] = frozenset()
|
||||
publish_corrected_urdf: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CalibrationProfile:
|
||||
key: ProfileKey
|
||||
namespace: str
|
||||
command: CommandLayout
|
||||
vision: VisionRigSpec
|
||||
motion: MotionPolicy
|
||||
measurement: MeasurementPolicy
|
||||
zero: ZeroSolvePolicy
|
||||
quality: QualityPolicy
|
||||
scope: ScopePolicy
|
||||
artifacts: ArtifactPolicy
|
||||
# Per-URDF-joint provenance used by partial calibration artifacts.
|
||||
# Known values are: measured_static_dynamic, measured_dynamic_cad_static,
|
||||
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
|
||||
# and mimic_nominal.
|
||||
joint_coverage: Mapping[str, str] = field(default_factory=dict)
|
||||
|
||||
|
||||
class ProfileValidationError(ValueError):
|
||||
"""Raised before hardware startup when a profile is internally unsafe."""
|
||||
|
||||
|
||||
def validate_profile(profile: CalibrationProfile) -> None:
|
||||
"""Hard-check all cross-policy references before hardware is enabled."""
|
||||
errors: list[str] = []
|
||||
command = profile.command
|
||||
if not command.names or len(command.names) != len(command.baseline_u8):
|
||||
errors.append("command names and baseline must be non-empty and aligned")
|
||||
if len(set(command.names)) != len(command.names):
|
||||
errors.append("command names must be unique")
|
||||
if any(value < 0 or value > 255 for value in command.baseline_u8):
|
||||
errors.append("baseline command values must be in [0, 255]")
|
||||
indices = set(range(command.command_count))
|
||||
if not set(command.disabled_indices).issubset(indices):
|
||||
errors.append("disabled command index is out of range")
|
||||
if any(index not in indices for index in command.command_index_by_joint.values()):
|
||||
errors.append("joint command index is out of range")
|
||||
if command.urdf_joint_by_joint:
|
||||
if not set(command.command_index_by_joint).issubset(
|
||||
command.urdf_joint_by_joint
|
||||
):
|
||||
errors.append("every commanded joint must map to a URDF joint")
|
||||
urdf_names = tuple(command.urdf_joint_by_joint.values())
|
||||
if len(set(urdf_names)) != len(urdf_names):
|
||||
errors.append("URDF joint mappings must be unique")
|
||||
if any(
|
||||
index not in indices or slot < 0
|
||||
for index, slot in command.speed_slot_by_command_index.items()
|
||||
):
|
||||
errors.append("speed-slot mapping is invalid")
|
||||
if any(
|
||||
not str(alias).strip() or canonical not in command.names
|
||||
for alias, canonical in command.feedback_name_aliases.items()
|
||||
):
|
||||
errors.append("feedback name alias is not part of the command schema")
|
||||
|
||||
view_names = profile.vision.view_names
|
||||
if not view_names or len(set(view_names)) != len(view_names):
|
||||
errors.append("vision views must be non-empty and unique")
|
||||
if profile.vision.extrinsic_reference_view not in view_names:
|
||||
errors.append("extrinsic reference view is not declared")
|
||||
tag_ids = [tag.tag_id for view in profile.vision.views for tag in view.tags]
|
||||
tag_roles = [tag.role for view in profile.vision.views for tag in view.tags]
|
||||
if len(set(tag_ids)) != len(tag_ids):
|
||||
errors.append("Tag IDs must be unique across views")
|
||||
if len(set(tag_roles)) != len(tag_roles):
|
||||
errors.append("Tag roles must be unique across views")
|
||||
if not any(
|
||||
tag.fixed_reference for view in profile.vision.views for tag in view.tags
|
||||
):
|
||||
errors.append("at least one fixed reference Tag is required")
|
||||
|
||||
task_keys = [task.key for task in profile.motion.tasks]
|
||||
if not task_keys or len(set(task_keys)) != len(task_keys):
|
||||
errors.append("motion task keys must be non-empty and unique")
|
||||
measurement_names = set(profile.measurement.measurements)
|
||||
for task in profile.motion.tasks:
|
||||
if task.view not in view_names:
|
||||
errors.append(f"task {task.key} uses an unknown view")
|
||||
if task.command_index not in indices:
|
||||
errors.append(f"task {task.key} command index is out of range")
|
||||
if not task.joints or not set(task.joints).issubset(measurement_names):
|
||||
errors.append(f"task {task.key} references unknown measurements")
|
||||
if any(index not in indices for index, _ in task.auxiliary_commands):
|
||||
errors.append(f"task {task.key} auxiliary index is out of range")
|
||||
if not 0 <= task.start_u8 <= 255 or not 0 <= task.end_u8 <= 255:
|
||||
errors.append(f"task {task.key} sweep endpoint is out of range")
|
||||
if task.start_u8 == task.end_u8:
|
||||
errors.append(f"task {task.key} sweep endpoints must differ")
|
||||
for speed in (task.preflight_speed_u8, task.formal_speed_u8):
|
||||
if speed is not None and not 0 <= speed <= 255:
|
||||
errors.append(f"task {task.key} speed is out of range")
|
||||
for name, spec in profile.measurement.measurements.items():
|
||||
if name != spec.joint:
|
||||
errors.append(f"measurement mapping key differs for {name}")
|
||||
if spec.view is not None and spec.view not in view_names:
|
||||
errors.append(f"measurement {name} uses an unknown view")
|
||||
for primary, validation in profile.measurement.cross_view_sources.items():
|
||||
if primary not in measurement_names or validation not in measurement_names:
|
||||
errors.append("cross-view measurement source is unknown")
|
||||
|
||||
zero = profile.zero
|
||||
if zero.active_joints & zero.passive_joints:
|
||||
errors.append("active and passive joints must be disjoint")
|
||||
all_joints = zero.active_joints | zero.passive_joints
|
||||
if not zero.active_joints.issubset(command.command_index_by_joint):
|
||||
errors.append("every active joint must bind to a command channel")
|
||||
if not set(zero.direct_zero_joints).issubset(zero.active_joints):
|
||||
errors.append("direct zero targets must be active joints")
|
||||
if not set(zero.axis_joints).issubset(all_joints):
|
||||
errors.append("axis targets must be known joints")
|
||||
if not zero.mechanical_endpoint_joints.issubset(zero.active_joints):
|
||||
errors.append("mechanical endpoint targets must be active joints")
|
||||
if not zero.post_solve_endpoint_joints.issubset(zero.active_joints):
|
||||
errors.append("post-solve endpoint targets must be active joints")
|
||||
if not set(zero.mimic_source_by_joint).issubset(zero.passive_joints):
|
||||
errors.append("mimic targets must be passive joints")
|
||||
if not set(zero.mimic_source_by_joint.values()).issubset(all_joints):
|
||||
errors.append("mimic sources must be known joints")
|
||||
if not set(zero.endpoint_anchor_by_joint).issubset(zero.active_joints):
|
||||
errors.append("endpoint anchors must target active joints")
|
||||
if not set(zero.endpoint_anchor_by_joint.values()).issubset(
|
||||
{
|
||||
"upper_at_end",
|
||||
"lower_at_start",
|
||||
"zero_at_start",
|
||||
"cad_range_center",
|
||||
}
|
||||
):
|
||||
errors.append("endpoint anchor policy is unsupported")
|
||||
if not zero.fitted_mimic_joints.issubset(zero.passive_joints):
|
||||
errors.append("fitted mimic targets must be passive joints")
|
||||
if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint):
|
||||
errors.append("fitted mimic target has no source mapping")
|
||||
if not set(zero.coupling_model_by_joint).issubset(
|
||||
zero.mimic_source_by_joint
|
||||
):
|
||||
errors.append("coupling model target has no source mapping")
|
||||
if not set(zero.coupling_model_by_joint.values()).issubset(
|
||||
{"linear_mimic", "quadratic_runtime"}
|
||||
):
|
||||
errors.append("coupling model policy is unsupported")
|
||||
|
||||
scopes = set(profile.scope.calibrate_joints)
|
||||
if profile.scope.default_scope not in scopes:
|
||||
errors.append("default scope is not declared")
|
||||
if scopes != set(profile.scope.frozen_joints):
|
||||
errors.append("scope calibration and frozen mappings must align")
|
||||
for name in scopes:
|
||||
selected = profile.scope.calibrate_joints[name]
|
||||
frozen = profile.scope.frozen_joints[name]
|
||||
if selected & frozen or selected | frozen != zero.active_joints:
|
||||
errors.append(f"scope {name} must partition all active joints")
|
||||
|
||||
artifacts = profile.artifacts
|
||||
if artifacts.output_schema_version < 1:
|
||||
errors.append("artifact schema version must be positive")
|
||||
for label, filename in (
|
||||
("calibration", artifacts.calibration_filename),
|
||||
("corrected URDF", artifacts.corrected_urdf_filename),
|
||||
("publication pointer", artifacts.publication_pointer),
|
||||
):
|
||||
if not filename or PurePath(filename).name != filename:
|
||||
errors.append(f"{label} filename must not contain a directory")
|
||||
if not profile.namespace.startswith("/"):
|
||||
errors.append("runtime namespace must be absolute")
|
||||
if profile.joint_coverage:
|
||||
valid_coverage = {
|
||||
"measured_static_dynamic",
|
||||
"measured_dynamic_cad_static",
|
||||
"transferred_static_dynamic",
|
||||
"transferred_dynamic_cad_static",
|
||||
"cad_nominal",
|
||||
"mimic_nominal",
|
||||
}
|
||||
if set(profile.joint_coverage) != all_joints:
|
||||
errors.append("joint coverage must describe every profile joint")
|
||||
if not set(profile.joint_coverage.values()).issubset(valid_coverage):
|
||||
errors.append("joint coverage contains an unsupported status")
|
||||
|
||||
if errors:
|
||||
raise ProfileValidationError("; ".join(errors))
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Normalized records shared by online evaluation and offline replay."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SampleRecord:
|
||||
task_key: str
|
||||
measurement: str
|
||||
view: str
|
||||
cycle: int
|
||||
direction: str
|
||||
command_u8: int
|
||||
timestamp_ns: int
|
||||
values: Mapping[str, Any]
|
||||
quality: Mapping[str, float] = field(default_factory=dict)
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if not self.task_key or not self.measurement or not self.view:
|
||||
raise ValueError("sample task, measurement, and view are required")
|
||||
if self.cycle < 0 or not 0 <= self.command_u8 <= 255:
|
||||
raise ValueError("sample cycle or command is out of range")
|
||||
if self.timestamp_ns < 0:
|
||||
raise ValueError("sample timestamp must be non-negative")
|
||||
@@ -0,0 +1,192 @@
|
||||
"""Canonical command/feedback schema for calibration observations.
|
||||
|
||||
The hand command and its measured motor feedback are different physical
|
||||
domains. Durable samples always retain both. Fitting code may still use the
|
||||
historical ``command_u8`` key, but it is created only as an explicit projection
|
||||
of a canonical record at the fitting boundary.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Any, Iterable, Literal, Mapping
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
SAMPLE_KINDS = frozenset(
|
||||
{
|
||||
'sample',
|
||||
'baseline_hold_sample',
|
||||
'steady_command_sample',
|
||||
'palm_axis_sample',
|
||||
}
|
||||
)
|
||||
|
||||
FitDomain = Literal['default', 'requested', 'feedback']
|
||||
|
||||
|
||||
class SampleDataContractError(ValueError):
|
||||
"""A calibration observation mixes or omits command domains."""
|
||||
|
||||
|
||||
def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float:
|
||||
try:
|
||||
number = float(value)
|
||||
except (TypeError, ValueError) as error:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be numeric'
|
||||
) from error
|
||||
if not math.isfinite(number) or not 0.0 <= number <= 255.0:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]'
|
||||
)
|
||||
if integral:
|
||||
rounded = int(round(number))
|
||||
if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9):
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be an integer command'
|
||||
)
|
||||
return rounded
|
||||
return number
|
||||
|
||||
|
||||
def explicit_domain_value(
|
||||
source: Mapping[str, Any], domain: Literal['requested', 'feedback']
|
||||
) -> int | float:
|
||||
"""Read and validate one explicitly named domain from any observation."""
|
||||
field = (
|
||||
'requested_command_u8' if domain == 'requested' else 'feedback_u8'
|
||||
)
|
||||
if field not in source or source[field] is None:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:observation is missing explicit {field}'
|
||||
)
|
||||
return _finite_u8(source[field], field, integral=domain == 'requested')
|
||||
|
||||
|
||||
def canonical_sample_record(
|
||||
source: Mapping[str, Any],
|
||||
*,
|
||||
allow_legacy_command: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Return one durable, unambiguous calibration observation.
|
||||
|
||||
``allow_legacy_command`` is restricted to importing historical sessions
|
||||
and unit fixtures. New online observations must provide both explicit
|
||||
fields and therefore cannot silently reinterpret ``command_u8``.
|
||||
"""
|
||||
record = dict(source)
|
||||
kind = str(record.get('kind', ''))
|
||||
if not kind and allow_legacy_command:
|
||||
# Old in-memory steady-curve fixtures predate durable sample kinds.
|
||||
# This adapter is never enabled by the new online/import contract.
|
||||
kind = 'steady_command_sample'
|
||||
record['kind'] = kind
|
||||
if kind not in SAMPLE_KINDS:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}'
|
||||
)
|
||||
|
||||
requested = record.get('requested_command_u8')
|
||||
feedback = record.get('feedback_u8')
|
||||
legacy = record.get('command_u8')
|
||||
if requested is None or feedback is None:
|
||||
if not allow_legacy_command or legacy is None:
|
||||
missing = [
|
||||
name
|
||||
for name, value in (
|
||||
('requested_command_u8', requested),
|
||||
('feedback_u8', feedback),
|
||||
)
|
||||
if value is None
|
||||
]
|
||||
raise SampleDataContractError(
|
||||
'DATA-CONTRACT-701:'
|
||||
f'{kind} is missing explicit {",".join(missing)}'
|
||||
)
|
||||
# Historical in-memory records used requested commands for settled
|
||||
# checkpoints and feedback bins for dense/baseline/palm observations.
|
||||
if requested is None:
|
||||
requested = legacy
|
||||
if feedback is None:
|
||||
feedback = legacy
|
||||
|
||||
record.pop('command_u8', None)
|
||||
record['requested_command_u8'] = explicit_domain_value(
|
||||
{'requested_command_u8': requested}, 'requested'
|
||||
)
|
||||
record['feedback_u8'] = explicit_domain_value(
|
||||
{'feedback_u8': feedback}, 'feedback'
|
||||
)
|
||||
return record
|
||||
|
||||
|
||||
def fitting_sample_record(
|
||||
source: Mapping[str, Any],
|
||||
*,
|
||||
domain: FitDomain = 'default',
|
||||
allow_legacy_command: bool = False,
|
||||
snap_requested_endpoints: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Project a canonical sample into the legacy curve-fitter interface."""
|
||||
record = canonical_sample_record(
|
||||
source, allow_legacy_command=allow_legacy_command
|
||||
)
|
||||
kind = str(record['kind'])
|
||||
selected = domain
|
||||
if selected == 'default':
|
||||
selected = (
|
||||
'requested' if kind == 'steady_command_sample' else 'feedback'
|
||||
)
|
||||
if selected not in {'requested', 'feedback'}:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}'
|
||||
)
|
||||
requested = int(record['requested_command_u8'])
|
||||
if selected == 'requested' or (
|
||||
snap_requested_endpoints and requested in {0, 255}
|
||||
):
|
||||
index = requested
|
||||
else:
|
||||
index = int(
|
||||
np.clip(np.rint(float(record['feedback_u8'])), 0, 255)
|
||||
)
|
||||
record['command_u8'] = index
|
||||
return record
|
||||
|
||||
|
||||
def fitting_sample_records(
|
||||
records: Iterable[Mapping[str, Any]],
|
||||
*,
|
||||
domain: FitDomain = 'default',
|
||||
allow_legacy_command: bool = False,
|
||||
snap_requested_endpoints: bool = False,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Project several canonical samples into one explicit fitting domain."""
|
||||
return [
|
||||
fitting_sample_record(
|
||||
record,
|
||||
domain=domain,
|
||||
allow_legacy_command=allow_legacy_command,
|
||||
snap_requested_endpoints=snap_requested_endpoints,
|
||||
)
|
||||
for record in records
|
||||
]
|
||||
|
||||
|
||||
def validate_sample_records(
|
||||
records: Iterable[Mapping[str, Any]],
|
||||
*,
|
||||
allow_legacy_command: bool = False,
|
||||
) -> None:
|
||||
"""Validate a collection without changing its representation."""
|
||||
for index, record in enumerate(records):
|
||||
try:
|
||||
canonical_sample_record(
|
||||
record, allow_legacy_command=allow_legacy_command
|
||||
)
|
||||
except SampleDataContractError as error:
|
||||
raise SampleDataContractError(
|
||||
f'{error};record_index={index}'
|
||||
) from error
|
||||
@@ -0,0 +1,19 @@
|
||||
"""Shared task direction vocabulary."""
|
||||
|
||||
DIRECTION_DECREASING = "decreasing"
|
||||
DIRECTION_INCREASING = "increasing"
|
||||
DIRECTIONS: tuple[str, ...] = (
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
)
|
||||
|
||||
PHASE_ROOT = "root"
|
||||
PHASE_TIP = "tip"
|
||||
|
||||
__all__ = [
|
||||
"DIRECTION_DECREASING",
|
||||
"DIRECTION_INCREASING",
|
||||
"DIRECTIONS",
|
||||
"PHASE_ROOT",
|
||||
"PHASE_TIP",
|
||||
]
|
||||
@@ -0,0 +1,5 @@
|
||||
"""Pure curve and axis fitting."""
|
||||
|
||||
from .curve import FitResult, isotonic_nonincreasing
|
||||
|
||||
__all__ = ["FitResult", "isotonic_nonincreasing"]
|
||||
@@ -0,0 +1,70 @@
|
||||
"""Model-independent curve fitting result and monotonic projection."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..geometry import delta_rotation_vector
|
||||
|
||||
|
||||
def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
|
||||
"""Unweighted PAVA projection onto non-increasing values."""
|
||||
original = np.asarray(values, dtype=float)
|
||||
if original.ndim != 1 or not np.all(np.isfinite(original)):
|
||||
raise ValueError("values must be a finite vector")
|
||||
negated = -original
|
||||
levels: list[float] = []
|
||||
weights: list[int] = []
|
||||
starts: list[int] = []
|
||||
for index, value in enumerate(negated):
|
||||
levels.append(float(value))
|
||||
weights.append(1)
|
||||
starts.append(index)
|
||||
while len(levels) >= 2 and levels[-2] > levels[-1]:
|
||||
total_weight = weights[-2] + weights[-1]
|
||||
merged = (
|
||||
levels[-2] * weights[-2] + levels[-1] * weights[-1]
|
||||
) / total_weight
|
||||
levels[-2:] = [merged]
|
||||
weights[-2:] = [total_weight]
|
||||
starts.pop()
|
||||
projected = np.empty_like(original)
|
||||
for block_index, (level, start) in enumerate(zip(levels, starts)):
|
||||
end = (
|
||||
starts[block_index + 1]
|
||||
if block_index + 1 < len(starts)
|
||||
else len(original)
|
||||
)
|
||||
projected[start:end] = -level
|
||||
return projected
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FitResult:
|
||||
joints: dict[str, dict[str, Any]]
|
||||
axes: dict[str, tuple[float, float, float]]
|
||||
references: dict[str, tuple[float, float, float, float]]
|
||||
ip_coupling: dict[str, float]
|
||||
max_monotonic_correction_rad: float
|
||||
max_hysteresis_rad: float
|
||||
measurement_mode: str = "rotation"
|
||||
trajectory_models: dict[str, Any] = field(default_factory=dict)
|
||||
trajectory_quality: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
def measure_from_reference(
|
||||
self,
|
||||
joint_name: str,
|
||||
observed_quaternion_xyzw: Sequence[float],
|
||||
reference_quaternion_xyzw: Sequence[float] | None = None,
|
||||
) -> float:
|
||||
reference = (
|
||||
reference_quaternion_xyzw
|
||||
if reference_quaternion_xyzw is not None
|
||||
else self.references[joint_name]
|
||||
)
|
||||
vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
|
||||
axis = np.asarray(self.axes[joint_name], dtype=float)
|
||||
return float(vector @ axis)
|
||||
@@ -0,0 +1,41 @@
|
||||
"""Pure geometry used by online and offline calibration."""
|
||||
|
||||
from .extrinsics import (
|
||||
CameraCalibrationIdentity,
|
||||
CameraExtrinsics,
|
||||
camera_info_fingerprint,
|
||||
load_camera_extrinsics,
|
||||
matrix_payload,
|
||||
transform_matrix,
|
||||
validate_camera_extrinsics_payload,
|
||||
)
|
||||
from .rotation import (
|
||||
delta_rotation_vector,
|
||||
fit_rotation_axis,
|
||||
image_plane_tag_quaternion_xyzw,
|
||||
normalize_quaternion_xyzw,
|
||||
relative_quaternion_xyzw,
|
||||
robust_rotation_summary,
|
||||
rotation_inlier_fraction,
|
||||
rotation_rms_rad,
|
||||
rotation_spread_rad,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"CameraCalibrationIdentity",
|
||||
"CameraExtrinsics",
|
||||
"camera_info_fingerprint",
|
||||
"delta_rotation_vector",
|
||||
"fit_rotation_axis",
|
||||
"image_plane_tag_quaternion_xyzw",
|
||||
"load_camera_extrinsics",
|
||||
"matrix_payload",
|
||||
"normalize_quaternion_xyzw",
|
||||
"relative_quaternion_xyzw",
|
||||
"robust_rotation_summary",
|
||||
"rotation_inlier_fraction",
|
||||
"rotation_rms_rad",
|
||||
"rotation_spread_rad",
|
||||
"transform_matrix",
|
||||
"validate_camera_extrinsics_payload",
|
||||
]
|
||||
+66
-81
@@ -13,9 +13,6 @@ import yaml
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
||||
|
||||
|
||||
def camera_info_fingerprint(
|
||||
*,
|
||||
width: int,
|
||||
@@ -81,66 +78,61 @@ class CameraCalibrationIdentity:
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ThreeCameraExtrinsics:
|
||||
"""Transforms points from each camera optical frame into front optical."""
|
||||
class CameraExtrinsics:
|
||||
"""Transforms every declared camera into one Profile-selected reference."""
|
||||
|
||||
cameras: Mapping[str, CameraCalibrationIdentity]
|
||||
front_from_view: Mapping[str, np.ndarray]
|
||||
reference_view: str
|
||||
reference_from_view: Mapping[str, np.ndarray]
|
||||
quality: Mapping[str, float]
|
||||
|
||||
def transform(self, view: str) -> np.ndarray:
|
||||
if view not in self.front_from_view:
|
||||
if view not in self.reference_from_view:
|
||||
raise KeyError(f"extrinsics do not contain view {view}")
|
||||
return np.asarray(self.front_from_view[view], dtype=float).copy()
|
||||
|
||||
def camera_matches(
|
||||
self,
|
||||
view: str,
|
||||
*,
|
||||
serial_number: str,
|
||||
width: int,
|
||||
height: int,
|
||||
intrinsics_sha256: str,
|
||||
) -> bool:
|
||||
expected = self.cameras.get(view)
|
||||
return bool(
|
||||
expected is not None
|
||||
and expected.serial_number == str(serial_number)
|
||||
and expected.width == int(width)
|
||||
and expected.height == int(height)
|
||||
and expected.intrinsics_sha256 == str(intrinsics_sha256)
|
||||
)
|
||||
return np.asarray(self.reference_from_view[view], dtype=float).copy()
|
||||
|
||||
|
||||
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
||||
def validate_camera_extrinsics_payload(
|
||||
payload: Mapping[str, Any],
|
||||
*,
|
||||
required_views: Sequence[str],
|
||||
reference_view: str,
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||
) -> None:
|
||||
if int(payload.get("schema_version", -1)) != 1:
|
||||
raise ValueError("camera extrinsics schema_version must be 1")
|
||||
if payload.get("reference_view") != "front":
|
||||
raise ValueError("camera extrinsics reference_view must be front")
|
||||
reference = str(reference_view)
|
||||
if payload.get("reference_view") != reference:
|
||||
raise ValueError(
|
||||
"camera extrinsics reference_view differs from the Profile"
|
||||
)
|
||||
cameras = payload.get("cameras")
|
||||
transforms = payload.get("front_from_view")
|
||||
transforms = payload.get(f"{reference}_from_view")
|
||||
quality = payload.get("quality")
|
||||
if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
|
||||
raise ValueError("camera extrinsics must contain front/side/top cameras")
|
||||
if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
|
||||
raise ValueError("camera extrinsics must contain all three transforms")
|
||||
views = tuple(str(view) for view in required_views)
|
||||
if not views or len(set(views)) != len(views):
|
||||
raise ValueError("required extrinsic views must be non-empty and unique")
|
||||
if reference not in views:
|
||||
raise ValueError("extrinsic reference view is not required")
|
||||
if not isinstance(cameras, Mapping) or set(cameras) != set(views):
|
||||
raise ValueError("camera extrinsics differ from the Profile views")
|
||||
if not isinstance(transforms, Mapping) or set(transforms) != set(views):
|
||||
raise ValueError("camera transforms differ from the Profile views")
|
||||
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
|
||||
raise ValueError("camera extrinsics quality is not passed")
|
||||
quality_limits = {
|
||||
"reprojection_rms_px": 1.2,
|
||||
"maximum_rotation_repeatability_deg": 0.3,
|
||||
"maximum_translation_repeatability_m": 0.0015,
|
||||
}
|
||||
for key, limit in quality_limits.items():
|
||||
for key, limit in dict(quality_limits or {}).items():
|
||||
value = float(quality.get(key, float("inf")))
|
||||
if not np.isfinite(value) or value > limit:
|
||||
raise ValueError(
|
||||
f"camera extrinsics {key}={value} exceeds {limit}"
|
||||
)
|
||||
for key in ("front_side_captures", "front_top_captures"):
|
||||
if int(quality.get(key, 0)) < 15:
|
||||
raise ValueError(f"camera extrinsics {key} must be at least 15")
|
||||
for view in VIEWS:
|
||||
for key, minimum in dict(minimum_capture_counts or {}).items():
|
||||
if int(quality.get(key, 0)) < int(minimum):
|
||||
raise ValueError(
|
||||
f"camera extrinsics {key} must be at least {minimum}"
|
||||
)
|
||||
for view in views:
|
||||
identity = cameras[view]
|
||||
if not isinstance(identity, Mapping):
|
||||
raise ValueError(f"{view} camera identity must be an object")
|
||||
@@ -158,14 +150,23 @@ def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
||||
transform.get("translation_xyz_m", ()),
|
||||
transform.get("quaternion_xyzw", ()),
|
||||
)
|
||||
if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
|
||||
raise ValueError("front_from_view.front must be identity")
|
||||
serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
|
||||
if len(set(serials)) != len(VIEWS):
|
||||
if view == reference and not np.allclose(
|
||||
matrix, np.eye(4), atol=1.0e-9
|
||||
):
|
||||
raise ValueError("reference-view transform must be identity")
|
||||
serials = [str(cameras[view]["serial_number"]) for view in views]
|
||||
if len(set(serials)) != len(views):
|
||||
raise ValueError("camera extrinsics serial numbers must be unique")
|
||||
|
||||
|
||||
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
|
||||
def load_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
required_views: Sequence[str],
|
||||
reference_view: str,
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||
) -> CameraExtrinsics:
|
||||
source = Path(path).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"camera extrinsics file does not exist: {source}")
|
||||
@@ -173,7 +174,15 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
|
||||
payload = yaml.safe_load(stream)
|
||||
if not isinstance(payload, Mapping):
|
||||
raise ValueError("camera extrinsics file must contain an object")
|
||||
validate_extrinsics_payload(payload)
|
||||
validate_camera_extrinsics_payload(
|
||||
payload,
|
||||
required_views=required_views,
|
||||
reference_view=reference_view,
|
||||
quality_limits=quality_limits,
|
||||
minimum_capture_counts=minimum_capture_counts,
|
||||
)
|
||||
views = tuple(str(view) for view in required_views)
|
||||
transform_key = f"{reference_view}_from_view"
|
||||
cameras = {
|
||||
view: CameraCalibrationIdentity(
|
||||
serial_number=str(payload["cameras"][view]["serial_number"]),
|
||||
@@ -183,45 +192,21 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
|
||||
payload["cameras"][view]["intrinsics_sha256"]
|
||||
),
|
||||
)
|
||||
for view in VIEWS
|
||||
for view in views
|
||||
}
|
||||
transforms = {
|
||||
view: transform_matrix(
|
||||
payload["front_from_view"][view]["translation_xyz_m"],
|
||||
payload["front_from_view"][view]["quaternion_xyzw"],
|
||||
payload[transform_key][view]["translation_xyz_m"],
|
||||
payload[transform_key][view]["quaternion_xyzw"],
|
||||
)
|
||||
for view in VIEWS
|
||||
for view in views
|
||||
}
|
||||
return ThreeCameraExtrinsics(
|
||||
return CameraExtrinsics(
|
||||
cameras=cameras,
|
||||
front_from_view=transforms,
|
||||
reference_view=str(reference_view),
|
||||
reference_from_view=transforms,
|
||||
quality={
|
||||
str(key): float(value) if isinstance(value, (int, float)) else value
|
||||
for key, value in payload["quality"].items()
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def dump_three_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
cameras: Mapping[str, Mapping[str, Any]],
|
||||
front_from_view: Mapping[str, Sequence[Sequence[float]]],
|
||||
quality: Mapping[str, Any],
|
||||
) -> None:
|
||||
payload = {
|
||||
"schema_version": 1,
|
||||
"reference_view": "front",
|
||||
"cameras": {view: dict(cameras[view]) for view in VIEWS},
|
||||
"front_from_view": {
|
||||
view: matrix_payload(front_from_view[view]) for view in VIEWS
|
||||
},
|
||||
"quality": dict(quality),
|
||||
}
|
||||
validate_extrinsics_payload(payload)
|
||||
destination = Path(path).expanduser().resolve()
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary = destination.with_suffix(destination.suffix + ".tmp")
|
||||
with temporary.open("w", encoding="utf-8") as stream:
|
||||
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
|
||||
temporary.replace(destination)
|
||||
+427
-22
@@ -477,6 +477,11 @@ def select_static_rigid_group_initialization(
|
||||
relative_translation_scale_m: float,
|
||||
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
|
||||
normal_alignment_scale_rad: float = math.radians(5.0),
|
||||
task_reference_pairs: Mapping[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray]
|
||||
] | None = None,
|
||||
task_reference_rotation_scale_rad: float = math.radians(1.0),
|
||||
task_reference_translation_scale_m: float = 0.01,
|
||||
) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
|
||||
"""Select a static multi-Tag IPPE branch path in bounded time.
|
||||
|
||||
@@ -497,13 +502,18 @@ def select_static_rigid_group_initialization(
|
||||
normal_pair_names = tuple(
|
||||
(str(first), str(second)) for first, second in normal_alignment_pairs
|
||||
)
|
||||
task_references = dict(task_reference_pairs or {})
|
||||
if not frames:
|
||||
raise ValueError("at least one PnP frame is required")
|
||||
if not role_names or len(set(role_names)) != len(role_names):
|
||||
raise ValueError("roles must be non-empty and unique")
|
||||
if any(
|
||||
parent not in role_names or child not in role_names
|
||||
for parent, child in (*pair_names, *normal_pair_names)
|
||||
for parent, child in (
|
||||
*pair_names,
|
||||
*normal_pair_names,
|
||||
*task_references,
|
||||
)
|
||||
):
|
||||
raise ValueError("geometry pairs must reference roles")
|
||||
scales = (
|
||||
@@ -513,6 +523,8 @@ def select_static_rigid_group_initialization(
|
||||
float(relative_rotation_scale_rad),
|
||||
float(relative_translation_scale_m),
|
||||
float(normal_alignment_scale_rad),
|
||||
float(task_reference_rotation_scale_rad),
|
||||
float(task_reference_translation_scale_m),
|
||||
)
|
||||
if min(scales) <= 0.0:
|
||||
raise ValueError("static initialization scales must be positive")
|
||||
@@ -523,6 +535,8 @@ def select_static_rigid_group_initialization(
|
||||
relative_rotation_scale,
|
||||
relative_translation_scale,
|
||||
normal_scale,
|
||||
task_reference_rotation_scale,
|
||||
task_reference_translation_scale,
|
||||
) = scales
|
||||
|
||||
combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
|
||||
@@ -578,6 +592,16 @@ def select_static_rigid_group_initialization(
|
||||
+ float(np.linalg.norm(translation - reference_translation))
|
||||
/ relative_translation_scale
|
||||
)
|
||||
for pair, (task_rotation, task_translation) in task_references.items():
|
||||
rotation, translation = _relative_pose(
|
||||
combination[pair[0]], combination[pair[1]]
|
||||
)
|
||||
score += (
|
||||
float((task_rotation.inv() * rotation).magnitude())
|
||||
/ task_reference_rotation_scale
|
||||
+ float(np.linalg.norm(translation - task_translation))
|
||||
/ task_reference_translation_scale
|
||||
)
|
||||
return float(score)
|
||||
|
||||
best_total = float("inf")
|
||||
@@ -628,6 +652,9 @@ def select_static_rigid_group_initialization(
|
||||
quality = dict(quality)
|
||||
quality["total_cost"] = float(best_total)
|
||||
quality["initialization_search"] = "static_reference"
|
||||
quality["task_reference_used"] = (
|
||||
"true" if task_references else "false"
|
||||
)
|
||||
return selected_path, quality
|
||||
|
||||
|
||||
@@ -886,11 +913,15 @@ class SquareTagPoseTracker:
|
||||
self.last_candidates_by_role: dict[
|
||||
str, tuple[SquareTagPose, ...]
|
||||
] = {}
|
||||
self.last_candidate_diagnostics_by_role: dict[
|
||||
str, dict[str, float | int]
|
||||
] = {}
|
||||
self.branch_correction_counts: dict[str, int] = {}
|
||||
|
||||
def reset(self) -> None:
|
||||
self._previous.clear()
|
||||
self.last_candidates_by_role.clear()
|
||||
self.last_candidate_diagnostics_by_role.clear()
|
||||
self.branch_correction_counts.clear()
|
||||
|
||||
def estimate(
|
||||
@@ -911,28 +942,91 @@ class SquareTagPoseTracker:
|
||||
)
|
||||
except (ValueError, cv2.error):
|
||||
self.last_candidates_by_role[str(role)] = ()
|
||||
self.last_candidate_diagnostics_by_role[str(role)] = {
|
||||
"solved_candidate_count": 0,
|
||||
"reprojection_candidate_count": 0,
|
||||
"independent_tilt_candidate_count": 0,
|
||||
"maximum_reprojection_error_px": float(
|
||||
self.maximum_reprojection_error_px
|
||||
),
|
||||
"maximum_independent_tilt_deg": math.degrees(
|
||||
self.maximum_tag_tilt_rad
|
||||
),
|
||||
}
|
||||
return None, "pnp_solve_failed"
|
||||
if not candidates:
|
||||
self.last_candidates_by_role[str(role)] = ()
|
||||
self.last_candidate_diagnostics_by_role[str(role)] = {
|
||||
"solved_candidate_count": 0,
|
||||
"reprojection_candidate_count": 0,
|
||||
"independent_tilt_candidate_count": 0,
|
||||
"maximum_reprojection_error_px": float(
|
||||
self.maximum_reprojection_error_px
|
||||
),
|
||||
"maximum_independent_tilt_deg": math.degrees(
|
||||
self.maximum_tag_tilt_rad
|
||||
),
|
||||
}
|
||||
return None, "pnp_solve_failed"
|
||||
usable_candidates: list[SquareTagPose] = []
|
||||
for candidate in candidates:
|
||||
reprojection_candidates = [
|
||||
candidate
|
||||
for candidate in candidates
|
||||
if candidate.reprojection_error_px
|
||||
<= self.maximum_reprojection_error_px
|
||||
]
|
||||
candidate_tilts_rad: list[float] = []
|
||||
independent_candidates: list[SquareTagPose] = []
|
||||
for candidate in reprojection_candidates:
|
||||
normal = Rotation.from_quat(
|
||||
candidate.quaternion_xyzw
|
||||
).as_matrix()[:, 2]
|
||||
tilt = math.acos(
|
||||
float(np.clip(abs(normal[2]), 0.0, 1.0))
|
||||
)
|
||||
if (
|
||||
candidate.reprojection_error_px
|
||||
<= self.maximum_reprojection_error_px
|
||||
and tilt <= self.maximum_tag_tilt_rad
|
||||
):
|
||||
usable_candidates.append(candidate)
|
||||
candidate_tilts_rad.append(float(tilt))
|
||||
if tilt <= self.maximum_tag_tilt_rad:
|
||||
independent_candidates.append(candidate)
|
||||
|
||||
# Candidate generation and candidate selection have different
|
||||
# contracts. The per-Tag tilt limit protects a pose used without any
|
||||
# other geometry, but it must not erase a finite, low-reprojection
|
||||
# IPPE solution before SquareTagGroupPoseTracker can evaluate it
|
||||
# against the fixed palm reference, the articulated chain and the
|
||||
# preceding group pose. At a strongly oblique view the planar
|
||||
# ambiguity is usually smaller, and rejecting both branches at a
|
||||
# fixed angle caused deterministic mid-sweep holes despite continuous
|
||||
# image detections. Group tracking therefore receives every
|
||||
# reprojection-valid candidate; independent tracking below retains the
|
||||
# original tilt safety gate.
|
||||
self.last_candidates_by_role[str(role)] = tuple(
|
||||
usable_candidates
|
||||
reprojection_candidates
|
||||
)
|
||||
if not usable_candidates:
|
||||
diagnostics: dict[str, float | int] = {
|
||||
"solved_candidate_count": len(candidates),
|
||||
"reprojection_candidate_count": len(reprojection_candidates),
|
||||
"independent_tilt_candidate_count": len(independent_candidates),
|
||||
"minimum_reprojection_error_px": float(
|
||||
min(
|
||||
candidate.reprojection_error_px
|
||||
for candidate in candidates
|
||||
)
|
||||
),
|
||||
"maximum_reprojection_error_px": float(
|
||||
self.maximum_reprojection_error_px
|
||||
),
|
||||
"maximum_independent_tilt_deg": math.degrees(
|
||||
self.maximum_tag_tilt_rad
|
||||
),
|
||||
}
|
||||
if candidate_tilts_rad:
|
||||
diagnostics["minimum_candidate_tilt_deg"] = math.degrees(
|
||||
min(candidate_tilts_rad)
|
||||
)
|
||||
diagnostics["maximum_candidate_tilt_deg"] = math.degrees(
|
||||
max(candidate_tilts_rad)
|
||||
)
|
||||
self.last_candidate_diagnostics_by_role[str(role)] = diagnostics
|
||||
if not independent_candidates:
|
||||
return None, "no_pose_within_reprojection_or_tilt_limit"
|
||||
|
||||
previous_record = self._previous.get(str(role))
|
||||
@@ -944,7 +1038,7 @@ class SquareTagPoseTracker:
|
||||
previous = previous_pose
|
||||
|
||||
selected, reason = select_continuous_pose(
|
||||
usable_candidates,
|
||||
independent_candidates,
|
||||
previous=previous,
|
||||
maximum_reprojection_error_px=(
|
||||
self.maximum_reprojection_error_px
|
||||
@@ -1013,6 +1107,13 @@ class SquareTagGroupPoseTracker:
|
||||
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
|
||||
normal_alignment_scale_rad: float = math.radians(5.0),
|
||||
maximum_normal_alignment_rad: float | None = None,
|
||||
return_reference_rotation_scale_rad: float = math.radians(1.0),
|
||||
return_reference_maximum_command_gap_u8: int = 8,
|
||||
coupled_rotation_pairs: Sequence[
|
||||
tuple[str, str, str, str, float]
|
||||
] = (),
|
||||
coupled_rotation_scale_rad: float = math.radians(3.0),
|
||||
maximum_coupled_rotation_residual_rad: float | None = None,
|
||||
) -> None:
|
||||
self.roles = tuple(str(role) for role in roles)
|
||||
self.adjacent_pairs = tuple(
|
||||
@@ -1023,6 +1124,22 @@ class SquareTagGroupPoseTracker:
|
||||
(str(first), str(second))
|
||||
for first, second in normal_alignment_pairs
|
||||
)
|
||||
self.coupled_rotation_pairs = tuple(
|
||||
(
|
||||
str(driver_parent),
|
||||
str(driver_child),
|
||||
str(follower_parent),
|
||||
str(follower_child),
|
||||
float(multiplier),
|
||||
)
|
||||
for (
|
||||
driver_parent,
|
||||
driver_child,
|
||||
follower_parent,
|
||||
follower_child,
|
||||
multiplier,
|
||||
) in coupled_rotation_pairs
|
||||
)
|
||||
if not self.roles or len(set(self.roles)) != len(self.roles):
|
||||
raise ValueError("roles must be non-empty and unique")
|
||||
if any(
|
||||
@@ -1054,6 +1171,20 @@ class SquareTagGroupPoseTracker:
|
||||
if maximum_normal_alignment_rad is None
|
||||
else float(maximum_normal_alignment_rad)
|
||||
)
|
||||
self.return_reference_rotation_scale_rad = float(
|
||||
return_reference_rotation_scale_rad
|
||||
)
|
||||
self.return_reference_maximum_command_gap_u8 = int(
|
||||
return_reference_maximum_command_gap_u8
|
||||
)
|
||||
self.coupled_rotation_scale_rad = float(
|
||||
coupled_rotation_scale_rad
|
||||
)
|
||||
self.maximum_coupled_rotation_residual_rad = (
|
||||
None
|
||||
if maximum_coupled_rotation_residual_rad is None
|
||||
else float(maximum_coupled_rotation_residual_rad)
|
||||
)
|
||||
reset_seconds = float(reset_after_seconds)
|
||||
if min(
|
||||
self.maximum_pose_jump_rad,
|
||||
@@ -1062,6 +1193,8 @@ class SquareTagGroupPoseTracker:
|
||||
self.relative_translation_scale_m,
|
||||
self.reprojection_scale_px,
|
||||
self.normal_alignment_scale_rad,
|
||||
self.return_reference_rotation_scale_rad,
|
||||
self.coupled_rotation_scale_rad,
|
||||
reset_seconds,
|
||||
) <= 0.0:
|
||||
raise ValueError("group tracking scales must be positive")
|
||||
@@ -1069,11 +1202,33 @@ class SquareTagGroupPoseTracker:
|
||||
raise ValueError("reprojection_weight must be non-negative")
|
||||
if self.initialization_frames < 1:
|
||||
raise ValueError("initialization_frames must be positive")
|
||||
if self.return_reference_maximum_command_gap_u8 < 0:
|
||||
raise ValueError(
|
||||
"return reference maximum command gap must be non-negative"
|
||||
)
|
||||
if (
|
||||
self.maximum_normal_alignment_rad is not None
|
||||
and self.maximum_normal_alignment_rad <= 0.0
|
||||
):
|
||||
raise ValueError("maximum normal alignment must be positive")
|
||||
if any(
|
||||
role not in self.roles
|
||||
for coupling in self.coupled_rotation_pairs
|
||||
for role in coupling[:4]
|
||||
):
|
||||
raise ValueError("coupled rotation pairs must reference roles")
|
||||
if any(
|
||||
multiplier <= 0.0
|
||||
for *_, multiplier in self.coupled_rotation_pairs
|
||||
):
|
||||
raise ValueError("coupled rotation multipliers must be positive")
|
||||
if (
|
||||
self.maximum_coupled_rotation_residual_rad is not None
|
||||
and self.maximum_coupled_rotation_residual_rad <= 0.0
|
||||
):
|
||||
raise ValueError(
|
||||
"maximum coupled rotation residual must be positive"
|
||||
)
|
||||
self.reset_after_ns = int(reset_seconds * 1_000_000_000)
|
||||
self._previous: dict[str, SquareTagPose] = {}
|
||||
self._previous_stamp_ns: int | None = None
|
||||
@@ -1083,28 +1238,214 @@ class SquareTagGroupPoseTracker:
|
||||
self._initial_stamps_ns: list[int] = []
|
||||
self.last_initialization_quality: dict[str, float | str] = {}
|
||||
self.branch_correction_counts: dict[str, int] = {}
|
||||
self._decreasing_relative_rotations: dict[
|
||||
int, dict[tuple[str, str], Rotation]
|
||||
] = {}
|
||||
self._coupled_reference_rotations: dict[
|
||||
tuple[str, str], Rotation
|
||||
] = {}
|
||||
self._task_reference_relative_poses: dict[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray]
|
||||
] = {}
|
||||
self.last_missing_roles: tuple[str, ...] = ()
|
||||
|
||||
def reset(self) -> None:
|
||||
def reset(self, *, preserve_task_reference: bool = False) -> None:
|
||||
self._previous.clear()
|
||||
self._previous_stamp_ns = None
|
||||
self._initial_candidates.clear()
|
||||
self._initial_stamps_ns.clear()
|
||||
self.last_initialization_quality.clear()
|
||||
self.branch_correction_counts.clear()
|
||||
self._decreasing_relative_rotations.clear()
|
||||
self._coupled_reference_rotations.clear()
|
||||
self.last_missing_roles = ()
|
||||
if not preserve_task_reference:
|
||||
self._task_reference_relative_poses.clear()
|
||||
|
||||
def _task_reference_cost(
|
||||
self, combination: Mapping[str, SquareTagPose]
|
||||
) -> float:
|
||||
residual = 0.0
|
||||
for pair, (expected_rotation, expected_translation) in (
|
||||
self._task_reference_relative_poses.items()
|
||||
):
|
||||
rotation, translation = _relative_pose(
|
||||
combination[pair[0]], combination[pair[1]]
|
||||
)
|
||||
residual += (
|
||||
float((expected_rotation.inv() * rotation).magnitude())
|
||||
/ self.return_reference_rotation_scale_rad
|
||||
+ float(np.linalg.norm(translation - expected_translation))
|
||||
/ self.relative_translation_scale_m
|
||||
)
|
||||
return residual
|
||||
|
||||
def _coupled_rotation_residuals(
|
||||
self, combination: Mapping[str, SquareTagPose]
|
||||
) -> tuple[float, ...]:
|
||||
if not self.coupled_rotation_pairs:
|
||||
return ()
|
||||
residuals: list[float] = []
|
||||
for (
|
||||
driver_parent,
|
||||
driver_child,
|
||||
follower_parent,
|
||||
follower_child,
|
||||
multiplier,
|
||||
) in self.coupled_rotation_pairs:
|
||||
driver_pair = (driver_parent, driver_child)
|
||||
follower_pair = (follower_parent, follower_child)
|
||||
if (
|
||||
driver_pair not in self._coupled_reference_rotations
|
||||
or follower_pair not in self._coupled_reference_rotations
|
||||
):
|
||||
return ()
|
||||
driver_rotation = _relative_pose(
|
||||
combination[driver_parent], combination[driver_child]
|
||||
)[0]
|
||||
follower_rotation = _relative_pose(
|
||||
combination[follower_parent], combination[follower_child]
|
||||
)[0]
|
||||
driver_travel = (
|
||||
self._coupled_reference_rotations[driver_pair].inv()
|
||||
* driver_rotation
|
||||
).magnitude()
|
||||
follower_travel = (
|
||||
self._coupled_reference_rotations[follower_pair].inv()
|
||||
* follower_rotation
|
||||
).magnitude()
|
||||
residuals.append(
|
||||
abs(float(follower_travel) - multiplier * float(driver_travel))
|
||||
)
|
||||
return tuple(residuals)
|
||||
|
||||
def _informative_coupled_rotation_costs(
|
||||
self,
|
||||
combinations: Sequence[Mapping[str, SquareTagPose]],
|
||||
) -> tuple[float, ...]:
|
||||
"""Return branch costs only while the weak coupling prior is credible.
|
||||
|
||||
The URDF mimic ratio is useful for distinguishing two planar-IPPE
|
||||
branches, but it is not measurement truth for a passive joint. Once
|
||||
every otherwise viable combination disagrees with that ratio, using
|
||||
it would bias the measured curve (and previously rejected every
|
||||
frame). In that case fall back to visual continuity for this frame.
|
||||
"""
|
||||
residuals = tuple(
|
||||
self._coupled_rotation_residuals(combination)
|
||||
for combination in combinations
|
||||
)
|
||||
if not residuals or not any(residuals):
|
||||
return tuple(0.0 for _ in combinations)
|
||||
if (
|
||||
self.maximum_coupled_rotation_residual_rad is not None
|
||||
and not any(
|
||||
values
|
||||
and max(values)
|
||||
<= self.maximum_coupled_rotation_residual_rad
|
||||
for values in residuals
|
||||
)
|
||||
):
|
||||
return tuple(0.0 for _ in combinations)
|
||||
return tuple(
|
||||
sum(values) / self.coupled_rotation_scale_rad
|
||||
for values in residuals
|
||||
)
|
||||
|
||||
def _return_reference(
|
||||
self, command_u8: int | None
|
||||
) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]:
|
||||
if command_u8 is None or not self._decreasing_relative_rotations:
|
||||
return {}
|
||||
command = int(command_u8)
|
||||
nearest = min(
|
||||
self._decreasing_relative_rotations,
|
||||
key=lambda candidate: abs(candidate - command),
|
||||
)
|
||||
if (
|
||||
abs(nearest - command)
|
||||
> self.return_reference_maximum_command_gap_u8
|
||||
):
|
||||
return {}
|
||||
references = self._decreasing_relative_rotations[nearest]
|
||||
commands = sorted(self._decreasing_relative_rotations)
|
||||
axes: dict[tuple[str, str], np.ndarray | None] = {}
|
||||
for pair in self.adjacent_pairs:
|
||||
endpoint_delta = (
|
||||
self._decreasing_relative_rotations[commands[-1]][pair].inv()
|
||||
* self._decreasing_relative_rotations[commands[0]][pair]
|
||||
).as_rotvec()
|
||||
norm = float(np.linalg.norm(endpoint_delta))
|
||||
axes[pair] = (
|
||||
None
|
||||
if norm < math.radians(5.0)
|
||||
else endpoint_delta / norm
|
||||
)
|
||||
return {
|
||||
pair: (rotation, axes[pair])
|
||||
for pair, rotation in references.items()
|
||||
}
|
||||
|
||||
def _return_reference_cost(
|
||||
self,
|
||||
combination: Mapping[str, SquareTagPose],
|
||||
reference: Mapping[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray | None]
|
||||
],
|
||||
) -> float:
|
||||
residual = 0.0
|
||||
for pair, (expected, motion_axis) in reference.items():
|
||||
vector = (
|
||||
expected.inv()
|
||||
* _relative_pose(
|
||||
combination[pair[0]], combination[pair[1]]
|
||||
)[0]
|
||||
).as_rotvec()
|
||||
if motion_axis is not None:
|
||||
# The outbound trajectory identifies the physical one-DOF
|
||||
# motion axis. Do not penalize return travel along that axis:
|
||||
# it may contain real mechanical hysteresis that calibration
|
||||
# must measure. A planar-IPPE mirror branch appears primarily
|
||||
# as a large orthogonal tilt and is rejected by this residual.
|
||||
vector = vector - motion_axis * float(vector @ motion_axis)
|
||||
residual += float(np.linalg.norm(vector))
|
||||
return residual / self.return_reference_rotation_scale_rad
|
||||
|
||||
def select(
|
||||
self,
|
||||
candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
|
||||
*,
|
||||
stamp_ns: int,
|
||||
trajectory_command_u8: int | None = None,
|
||||
trajectory_direction: str | None = None,
|
||||
) -> tuple[dict[str, SquareTagPose] | None, str]:
|
||||
"""Return one mutually consistent pose for every configured role."""
|
||||
direction = (
|
||||
None
|
||||
if trajectory_direction is None
|
||||
else str(trajectory_direction)
|
||||
)
|
||||
if direction not in {None, "decreasing", "increasing"}:
|
||||
raise ValueError(
|
||||
"trajectory_direction must be decreasing or increasing"
|
||||
)
|
||||
return_reference = (
|
||||
self._return_reference(trajectory_command_u8)
|
||||
if direction == "increasing"
|
||||
else {}
|
||||
)
|
||||
candidate_lists = [
|
||||
tuple(candidates_by_role.get(role, ()))
|
||||
for role in self.roles
|
||||
]
|
||||
if any(not candidates for candidates in candidate_lists):
|
||||
self.last_missing_roles = tuple(
|
||||
role
|
||||
for role, candidates in zip(self.roles, candidate_lists)
|
||||
if not candidates
|
||||
)
|
||||
if self.last_missing_roles:
|
||||
return None, "group_missing_pose_candidates"
|
||||
self.last_missing_roles = ()
|
||||
|
||||
combinations = [
|
||||
dict(zip(self.roles, combination))
|
||||
@@ -1175,6 +1516,15 @@ class SquareTagGroupPoseTracker:
|
||||
normal_alignment_scale_rad=(
|
||||
self.normal_alignment_scale_rad
|
||||
),
|
||||
task_reference_pairs=(
|
||||
self._task_reference_relative_poses
|
||||
),
|
||||
task_reference_rotation_scale_rad=(
|
||||
self.return_reference_rotation_scale_rad
|
||||
),
|
||||
task_reference_translation_scale_m=(
|
||||
self.relative_translation_scale_m
|
||||
),
|
||||
)
|
||||
)
|
||||
selected = selected_path[-1]
|
||||
@@ -1195,23 +1545,31 @@ class SquareTagGroupPoseTracker:
|
||||
):
|
||||
return None, "group_normal_alignment"
|
||||
else:
|
||||
coupling_costs = self._informative_coupled_rotation_costs(
|
||||
combinations
|
||||
)
|
||||
selected = min(
|
||||
combinations,
|
||||
key=lambda combination: (
|
||||
zip(combinations, coupling_costs),
|
||||
key=lambda item: (
|
||||
sum(
|
||||
pose.reprojection_error_px
|
||||
for pose in combination.values()
|
||||
for pose in item[0].values()
|
||||
)
|
||||
/ self.reprojection_scale_px
|
||||
+ sum(
|
||||
_normal_alignment_rad(
|
||||
combination[first], combination[second]
|
||||
item[0][first], item[0][second]
|
||||
)
|
||||
for first, second in self.normal_alignment_pairs
|
||||
)
|
||||
/ self.normal_alignment_scale_rad
|
||||
+ self._return_reference_cost(
|
||||
item[0], return_reference
|
||||
)
|
||||
+ item[1]
|
||||
+ self._task_reference_cost(item[0])
|
||||
),
|
||||
)
|
||||
)[0]
|
||||
maximum_alignment = max(
|
||||
(
|
||||
_normal_alignment_rad(
|
||||
@@ -1238,7 +1596,7 @@ class SquareTagGroupPoseTracker:
|
||||
)
|
||||
for pair in self.adjacent_pairs
|
||||
}
|
||||
scored: list[tuple[float, dict[str, SquareTagPose]]] = []
|
||||
base_scored: list[tuple[float, dict[str, SquareTagPose]]] = []
|
||||
for combination in combinations:
|
||||
absolute_rotation_motion = 0.0
|
||||
absolute_translation_motion = 0.0
|
||||
@@ -1305,11 +1663,23 @@ class SquareTagGroupPoseTracker:
|
||||
+ relative_translation_motion
|
||||
/ self.relative_translation_scale_m
|
||||
+ self.reprojection_weight * reprojection_penalty
|
||||
+ self._return_reference_cost(
|
||||
combination, return_reference
|
||||
)
|
||||
)
|
||||
scored.append((float(score), combination))
|
||||
base_scored.append((float(score), combination))
|
||||
|
||||
if not scored:
|
||||
if not base_scored:
|
||||
return None, "group_pose_jump"
|
||||
coupling_costs = self._informative_coupled_rotation_costs(
|
||||
[combination for _, combination in base_scored]
|
||||
)
|
||||
scored = [
|
||||
(base_score + coupling_cost, combination)
|
||||
for (base_score, combination), coupling_cost in zip(
|
||||
base_scored, coupling_costs
|
||||
)
|
||||
]
|
||||
selected = min(scored, key=lambda item: item[0])[1]
|
||||
|
||||
aligned: dict[str, SquareTagPose] = {}
|
||||
@@ -1343,4 +1713,39 @@ class SquareTagGroupPoseTracker:
|
||||
|
||||
self._previous = aligned
|
||||
self._previous_stamp_ns = stamp
|
||||
if (
|
||||
direction == "decreasing"
|
||||
and trajectory_command_u8 is not None
|
||||
and not self._coupled_reference_rotations
|
||||
):
|
||||
for (
|
||||
driver_parent,
|
||||
driver_child,
|
||||
follower_parent,
|
||||
follower_child,
|
||||
_multiplier,
|
||||
) in self.coupled_rotation_pairs:
|
||||
for pair in (
|
||||
(driver_parent, driver_child),
|
||||
(follower_parent, follower_child),
|
||||
):
|
||||
self._coupled_reference_rotations[pair] = _relative_pose(
|
||||
aligned[pair[0]], aligned[pair[1]]
|
||||
)[0]
|
||||
if direction == "decreasing" and trajectory_command_u8 is not None:
|
||||
self._decreasing_relative_rotations[
|
||||
int(trajectory_command_u8)
|
||||
] = {
|
||||
pair: _relative_pose(
|
||||
aligned[pair[0]], aligned[pair[1]]
|
||||
)[0]
|
||||
for pair in self.adjacent_pairs
|
||||
}
|
||||
if not self._task_reference_relative_poses:
|
||||
self._task_reference_relative_poses = {
|
||||
pair: _relative_pose(
|
||||
aligned[pair[0]], aligned[pair[1]]
|
||||
)
|
||||
for pair in self.adjacent_pairs
|
||||
}
|
||||
return dict(aligned), ""
|
||||
@@ -0,0 +1,157 @@
|
||||
"""Pure quaternion summaries and rotation-axis fitting."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Sequence
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
|
||||
quaternion = np.asarray(values, dtype=float)
|
||||
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
||||
raise ValueError("quaternion must contain four finite xyzw values")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if norm < 1e-12:
|
||||
raise ValueError("quaternion norm is zero")
|
||||
return quaternion / norm
|
||||
|
||||
|
||||
def relative_quaternion_xyzw(
|
||||
parent_camera_quaternion: Sequence[float],
|
||||
child_camera_quaternion: Sequence[float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
"""Compute parent-to-child orientation from two camera-to-Tag rotations."""
|
||||
parent = Rotation.from_quat(
|
||||
normalize_quaternion_xyzw(parent_camera_quaternion)
|
||||
)
|
||||
child = Rotation.from_quat(
|
||||
normalize_quaternion_xyzw(child_camera_quaternion)
|
||||
)
|
||||
quaternion = (parent.inv() * child).as_quat()
|
||||
return tuple(float(value) for value in quaternion)
|
||||
|
||||
|
||||
def image_plane_tag_quaternion_xyzw(
|
||||
corners_xy: Sequence[Sequence[float]],
|
||||
) -> tuple[float, float, float, float]:
|
||||
"""Estimate Tag orientation about the optical axis from ordered corners."""
|
||||
corners = np.asarray(corners_xy, dtype=float)
|
||||
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
|
||||
raise ValueError("corners_xy must contain four finite xy points")
|
||||
x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
|
||||
if float(np.linalg.norm(x_axis)) < 1e-9:
|
||||
raise ValueError("tag x-axis is degenerate")
|
||||
angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
|
||||
quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
|
||||
return tuple(float(value) for value in quaternion)
|
||||
|
||||
|
||||
def robust_rotation_summary(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
) -> tuple[tuple[float, float, float, float], float]:
|
||||
"""Return a robust orientation and maximum angular residual in radians."""
|
||||
if not quaternions_xyzw:
|
||||
raise ValueError("at least one quaternion is required")
|
||||
rotations = Rotation.from_quat(
|
||||
np.asarray(
|
||||
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
reference = rotations[0]
|
||||
delta_vectors = (reference.inv() * rotations).as_rotvec()
|
||||
median_delta = np.median(delta_vectors, axis=0)
|
||||
robust = reference * Rotation.from_rotvec(median_delta)
|
||||
residuals = (robust.inv() * rotations).magnitude()
|
||||
maximum = float(np.max(residuals)) if residuals.size else 0.0
|
||||
return tuple(float(value) for value in robust.as_quat()), maximum
|
||||
|
||||
|
||||
def rotation_spread_rad(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
) -> float:
|
||||
"""Return the maximum geodesic residual around a robust orientation."""
|
||||
_, spread = robust_rotation_summary(quaternions_xyzw)
|
||||
return spread
|
||||
|
||||
|
||||
def rotation_rms_rad(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
*,
|
||||
outlier_threshold_rad: float | None = None,
|
||||
) -> float:
|
||||
"""Return RMS geodesic noise around a robust orientation."""
|
||||
robust, _ = robust_rotation_summary(quaternions_xyzw)
|
||||
reference = Rotation.from_quat(robust)
|
||||
rotations = Rotation.from_quat(
|
||||
np.asarray(
|
||||
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
residuals = (reference.inv() * rotations).magnitude()
|
||||
if outlier_threshold_rad is not None:
|
||||
threshold = float(outlier_threshold_rad)
|
||||
if threshold <= 0.0:
|
||||
raise ValueError("outlier_threshold_rad must be positive")
|
||||
residuals = residuals[residuals <= threshold]
|
||||
if residuals.size == 0:
|
||||
return float("inf")
|
||||
return float(np.sqrt(np.mean(np.square(residuals))))
|
||||
|
||||
|
||||
def rotation_inlier_fraction(
|
||||
quaternions_xyzw: Sequence[Sequence[float]],
|
||||
*,
|
||||
outlier_threshold_rad: float,
|
||||
) -> float:
|
||||
"""Return the fraction close to the robust orientation."""
|
||||
threshold = float(outlier_threshold_rad)
|
||||
if threshold <= 0.0:
|
||||
raise ValueError("outlier_threshold_rad must be positive")
|
||||
robust, _ = robust_rotation_summary(quaternions_xyzw)
|
||||
reference = Rotation.from_quat(robust)
|
||||
rotations = Rotation.from_quat(
|
||||
np.asarray(
|
||||
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
|
||||
dtype=float,
|
||||
)
|
||||
)
|
||||
residuals = (reference.inv() * rotations).magnitude()
|
||||
return float(np.mean(residuals <= threshold))
|
||||
|
||||
|
||||
def delta_rotation_vector(
|
||||
reference_xyzw: Sequence[float],
|
||||
observed_xyzw: Sequence[float],
|
||||
) -> np.ndarray:
|
||||
reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
|
||||
observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
|
||||
return (reference.inv() * observed).as_rotvec()
|
||||
|
||||
|
||||
def fit_rotation_axis(
|
||||
vectors: Sequence[Sequence[float]],
|
||||
commands: Sequence[int],
|
||||
) -> np.ndarray:
|
||||
"""Fit and orient the single rotational axis used by one command sweep."""
|
||||
matrix = np.asarray(vectors, dtype=float)
|
||||
command_values = np.asarray(commands, dtype=int)
|
||||
if matrix.ndim != 2 or matrix.shape[1] != 3:
|
||||
raise ValueError("vectors must have shape (N, 3)")
|
||||
if command_values.shape != (matrix.shape[0],):
|
||||
raise ValueError("commands must match vectors")
|
||||
useful = np.linalg.norm(matrix, axis=1) > 1e-6
|
||||
if int(np.count_nonzero(useful)) < 3:
|
||||
raise ValueError("insufficient non-zero rotations to fit an axis")
|
||||
_, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
|
||||
axis = vh[0]
|
||||
projections = matrix @ axis
|
||||
low = projections[command_values <= 16]
|
||||
high = projections[command_values >= 239]
|
||||
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
|
||||
axis = -axis
|
||||
return axis / np.linalg.norm(axis)
|
||||
@@ -0,0 +1,15 @@
|
||||
"""Task acceptance and final-session solver contracts."""
|
||||
|
||||
from .interfaces import (
|
||||
SessionSolution,
|
||||
SessionSolver,
|
||||
TaskEvaluation,
|
||||
TaskEvaluator,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"SessionSolution",
|
||||
"SessionSolver",
|
||||
"TaskEvaluation",
|
||||
"TaskEvaluator",
|
||||
]
|
||||
@@ -0,0 +1,42 @@
|
||||
"""Shared evaluator and final-solver interfaces."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any, Mapping, Protocol, Sequence
|
||||
|
||||
from ..domain import CalibrationProfile, SampleRecord, TaskSpec
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TaskEvaluation:
|
||||
accepted: bool
|
||||
failures: tuple[Mapping[str, Any], ...] = ()
|
||||
rescan_measurements: frozenset[str] = frozenset()
|
||||
rescan_cycles: frozenset[int] = frozenset()
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SessionSolution:
|
||||
passed: bool
|
||||
calibration: Mapping[str, Any]
|
||||
zero_offsets_rad: Mapping[str, float]
|
||||
failures: tuple[Mapping[str, Any], ...] = ()
|
||||
diagnostics: Mapping[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
class TaskEvaluator(Protocol):
|
||||
def evaluate_task(
|
||||
self,
|
||||
profile: CalibrationProfile,
|
||||
task: TaskSpec,
|
||||
samples: Sequence[SampleRecord],
|
||||
) -> TaskEvaluation: ...
|
||||
|
||||
|
||||
class SessionSolver(Protocol):
|
||||
def solve_session(
|
||||
self,
|
||||
profile: CalibrationProfile,
|
||||
samples: Sequence[SampleRecord],
|
||||
) -> SessionSolution: ...
|
||||
@@ -0,0 +1,22 @@
|
||||
"""URDF correction authorization and validation types."""
|
||||
|
||||
from .plan import UrdfCorrectionPlan, build_correction_plan
|
||||
from .patch import (
|
||||
MujocoEqualityPatch,
|
||||
UrdfJointPatch,
|
||||
UrdfPatchSet,
|
||||
apply_urdf_patch_text,
|
||||
materialize_relative_mesh_assets,
|
||||
write_urdf_patches,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"MujocoEqualityPatch",
|
||||
"UrdfCorrectionPlan",
|
||||
"UrdfJointPatch",
|
||||
"UrdfPatchSet",
|
||||
"apply_urdf_patch_text",
|
||||
"build_correction_plan",
|
||||
"materialize_relative_mesh_assets",
|
||||
"write_urdf_patches",
|
||||
]
|
||||
@@ -0,0 +1,315 @@
|
||||
"""Byte-preserving, declarative URDF patch application.
|
||||
|
||||
Model profiles decide *what* values are authorized. This module owns the
|
||||
shared mechanics of locating those fields in the original XML text, changing
|
||||
only the declared attributes, materializing mesh resources and atomically
|
||||
publishing a new file.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import shutil
|
||||
from typing import Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UrdfJointPatch:
|
||||
"""Authorized attribute replacements inside one top-level URDF joint."""
|
||||
|
||||
origin_rpy: str | None = None
|
||||
limit_lower: str | None = None
|
||||
limit_upper: str | None = None
|
||||
mimic_multiplier: str | None = None
|
||||
mimic_offset: str | None = None
|
||||
|
||||
def replacements(self) -> tuple[tuple[str, str, str], ...]:
|
||||
values = (
|
||||
("origin", "rpy", self.origin_rpy),
|
||||
("limit", "lower", self.limit_lower),
|
||||
("limit", "upper", self.limit_upper),
|
||||
("mimic", "multiplier", self.mimic_multiplier),
|
||||
("mimic", "offset", self.mimic_offset),
|
||||
)
|
||||
return tuple(
|
||||
(element, attribute, str(value))
|
||||
for element, attribute, value in values
|
||||
if value is not None
|
||||
)
|
||||
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MujocoEqualityPatch:
|
||||
"""Replacement and optional topology assertion for one equality joint."""
|
||||
|
||||
polycoef: str
|
||||
expected_joint1: str | None = None
|
||||
expected_joint2: str | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UrdfPatchSet:
|
||||
"""Complete declarative edit set for one generated URDF."""
|
||||
|
||||
joints: Mapping[str, UrdfJointPatch]
|
||||
mujoco_equalities: Mapping[str, MujocoEqualityPatch] = field(
|
||||
default_factory=dict
|
||||
)
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
empty = [
|
||||
name for name, patch in self.joints.items() if not patch.replacements()
|
||||
]
|
||||
if empty:
|
||||
raise ValueError(
|
||||
"URDF joint patch contains no replacements: "
|
||||
+ ",".join(sorted(empty))
|
||||
)
|
||||
|
||||
|
||||
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] + str(value) + block[end:]
|
||||
|
||||
|
||||
def apply_urdf_patch_text(
|
||||
original_text: str,
|
||||
root: ET.Element,
|
||||
patches: UrdfPatchSet,
|
||||
) -> str:
|
||||
"""Apply declared patches without serializing unaffected XML."""
|
||||
top_level_joints = {
|
||||
str(joint.get("name")): joint for joint in root.findall("joint")
|
||||
}
|
||||
missing_joints = set(patches.joints) - set(top_level_joints)
|
||||
if missing_joints:
|
||||
raise ValueError(
|
||||
"source URDF is missing target joints: "
|
||||
+ ",".join(sorted(missing_joints))
|
||||
)
|
||||
|
||||
equality_nodes = {
|
||||
str(joint.get("name")): joint
|
||||
for joint in root.findall("./mujoco/equality/joint")
|
||||
}
|
||||
missing_equalities = set(patches.mujoco_equalities) - set(equality_nodes)
|
||||
if missing_equalities:
|
||||
raise ValueError(
|
||||
"source URDF is missing MuJoCo equalities: "
|
||||
+ ",".join(sorted(missing_equalities))
|
||||
)
|
||||
for name, patch in patches.mujoco_equalities.items():
|
||||
node = equality_nodes[name]
|
||||
if (
|
||||
patch.expected_joint1 is not None
|
||||
and node.get("joint1") != patch.expected_joint1
|
||||
):
|
||||
raise ValueError(f"MuJoCo equality joint1 differs for {name}")
|
||||
if (
|
||||
patch.expected_joint2 is not None
|
||||
and node.get("joint2") != patch.expected_joint2
|
||||
):
|
||||
raise ValueError(f"MuJoCo equality joint2 differs for {name}")
|
||||
|
||||
# Requiring the URDF ``type`` attribute excludes transmission and MuJoCo
|
||||
# elements which also use the tag name ``joint``.
|
||||
joint_pattern = re.compile(
|
||||
r"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
|
||||
r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
|
||||
r".*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
applied_joints: set[str] = set()
|
||||
|
||||
def replace_joint(match: re.Match[str]) -> str:
|
||||
name = match.group("name")
|
||||
patch = patches.joints.get(name)
|
||||
if patch is None:
|
||||
return match.group(0)
|
||||
if name in applied_joints:
|
||||
raise ValueError(f"duplicate top-level URDF joint text: {name}")
|
||||
block = match.group(0)
|
||||
for element, attribute, value in patch.replacements():
|
||||
block = _replace_attribute(block, element, attribute, value)
|
||||
applied_joints.add(name)
|
||||
return block
|
||||
|
||||
corrected = joint_pattern.sub(replace_joint, original_text)
|
||||
if applied_joints != set(patches.joints):
|
||||
missing = set(patches.joints) - applied_joints
|
||||
raise ValueError(
|
||||
"could not locate every target joint in source URDF text: "
|
||||
+ ",".join(sorted(missing))
|
||||
)
|
||||
|
||||
applied_equalities: set[str] = set()
|
||||
for name, patch in patches.mujoco_equalities.items():
|
||||
equality_pattern = re.compile(
|
||||
rf"(<joint\b[^>]*\bname\s*=\s*([\"']))"
|
||||
rf"{re.escape(name)}\2[^>]*>",
|
||||
re.DOTALL,
|
||||
)
|
||||
matches = list(equality_pattern.finditer(corrected))
|
||||
if len(matches) != 1:
|
||||
raise ValueError(f"could not uniquely locate MuJoCo equality {name}")
|
||||
match = matches[0]
|
||||
replacement = _replace_attribute(
|
||||
match.group(0), "joint", "polycoef", patch.polycoef
|
||||
)
|
||||
corrected = corrected[: match.start()] + replacement + corrected[match.end() :]
|
||||
applied_equalities.add(name)
|
||||
if applied_equalities != set(patches.mujoco_equalities):
|
||||
raise ValueError("could not apply every MuJoCo equality patch")
|
||||
return corrected
|
||||
|
||||
|
||||
def _files_have_identical_contents(left: Path, right: Path) -> bool:
|
||||
if left.stat().st_size != right.stat().st_size:
|
||||
return False
|
||||
with left.open("rb") as left_stream, right.open("rb") as right_stream:
|
||||
while True:
|
||||
left_chunk = left_stream.read(1024 * 1024)
|
||||
right_chunk = right_stream.read(1024 * 1024)
|
||||
if left_chunk != right_chunk:
|
||||
return False
|
||||
if not left_chunk:
|
||||
return True
|
||||
|
||||
|
||||
def materialize_relative_mesh_assets(
|
||||
*, source: Path, output: Path, urdf_root: ET.Element
|
||||
) -> tuple[Path, ...]:
|
||||
"""Copy safe relative mesh resources beside the generated URDF."""
|
||||
filenames = sorted(
|
||||
{
|
||||
str(mesh.get("filename", "")).strip()
|
||||
for mesh in urdf_root.findall(".//mesh")
|
||||
if str(mesh.get("filename", "")).strip()
|
||||
}
|
||||
)
|
||||
materialized: list[Path] = []
|
||||
for filename in filenames:
|
||||
if "://" in filename or filename.startswith("package:"):
|
||||
continue
|
||||
relative = Path(filename)
|
||||
if relative.is_absolute() or ".." in relative.parts:
|
||||
raise ValueError(
|
||||
f"URDF mesh path must be a safe relative path or URI: {filename}"
|
||||
)
|
||||
source_asset = (source.parent / relative).resolve()
|
||||
if not source_asset.is_file():
|
||||
raise ValueError(f"URDF mesh resource does not exist: {source_asset}")
|
||||
destination_asset = (output / relative).resolve()
|
||||
try:
|
||||
destination_asset.relative_to(output)
|
||||
except ValueError as error:
|
||||
raise ValueError(
|
||||
f"URDF mesh destination escapes output directory: {filename}"
|
||||
) from error
|
||||
if destination_asset == source_asset:
|
||||
materialized.append(destination_asset)
|
||||
continue
|
||||
destination_asset.parent.mkdir(parents=True, exist_ok=True)
|
||||
if destination_asset.exists():
|
||||
if not destination_asset.is_file() or not _files_have_identical_contents(
|
||||
source_asset, destination_asset
|
||||
):
|
||||
raise ValueError(
|
||||
"refusing to overwrite a different mesh resource: "
|
||||
f"{destination_asset}"
|
||||
)
|
||||
materialized.append(destination_asset)
|
||||
continue
|
||||
temporary_asset = destination_asset.with_name(
|
||||
f".{destination_asset.name}.{os.getpid()}.tmp"
|
||||
)
|
||||
if temporary_asset.exists():
|
||||
raise ValueError(f"temporary mesh path is occupied: {temporary_asset}")
|
||||
try:
|
||||
shutil.copy2(source_asset, temporary_asset)
|
||||
os.replace(temporary_asset, destination_asset)
|
||||
finally:
|
||||
if temporary_asset.exists():
|
||||
temporary_asset.unlink()
|
||||
materialized.append(destination_asset)
|
||||
return tuple(materialized)
|
||||
|
||||
|
||||
def _copy_complete_mesh_directory(source: Path, output: Path) -> None:
|
||||
source_meshes = source.parent / "meshes"
|
||||
if not source_meshes.is_dir():
|
||||
return
|
||||
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)
|
||||
|
||||
|
||||
def write_urdf_patches(
|
||||
*,
|
||||
source_urdf: str | Path,
|
||||
destination_urdf: str | Path,
|
||||
patches: UrdfPatchSet,
|
||||
forbidden_source_stem_patterns: Sequence[str] = (),
|
||||
copy_complete_mesh_directory: bool = False,
|
||||
) -> Path:
|
||||
"""Validate and atomically materialize one patched URDF."""
|
||||
source = Path(source_urdf).expanduser().resolve()
|
||||
destination = Path(destination_urdf).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"source URDF does not exist: {source}")
|
||||
for pattern in forbidden_source_stem_patterns:
|
||||
if re.search(str(pattern), source.stem, re.IGNORECASE):
|
||||
raise ValueError(
|
||||
"source URDF must be the immutable original CAD URDF"
|
||||
)
|
||||
if destination == source or destination.exists():
|
||||
raise ValueError(f"refusing to overwrite URDF: {destination}")
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
tree = ET.parse(source)
|
||||
root = tree.getroot()
|
||||
corrected = apply_urdf_patch_text(
|
||||
source.read_text(encoding="utf-8"), root, patches
|
||||
)
|
||||
materialize_relative_mesh_assets(
|
||||
source=source, output=destination.parent, urdf_root=root
|
||||
)
|
||||
if copy_complete_mesh_directory:
|
||||
_copy_complete_mesh_directory(source, destination.parent)
|
||||
temporary = destination.with_suffix(destination.suffix + ".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 destination
|
||||
|
||||
|
||||
__all__ = [
|
||||
"MujocoEqualityPatch",
|
||||
"UrdfJointPatch",
|
||||
"UrdfPatchSet",
|
||||
"apply_urdf_patch_text",
|
||||
"materialize_relative_mesh_assets",
|
||||
"write_urdf_patches",
|
||||
]
|
||||
@@ -0,0 +1,104 @@
|
||||
"""One authorization plan shared by URDF writers and validators."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
import hashlib
|
||||
from pathlib import Path
|
||||
from typing import Mapping
|
||||
|
||||
from ..domain import CalibrationProfile
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UrdfCorrectionPlan:
|
||||
source_sha256: str
|
||||
allowed_active_joints: frozenset[str]
|
||||
endpoint_limit_joints: frozenset[str]
|
||||
mimic_source_by_joint: Mapping[str, str]
|
||||
frozen_joints: frozenset[str]
|
||||
frozen_offsets_rad: Mapping[str, float] = field(default_factory=dict)
|
||||
forbid_calibrated_source: bool = True
|
||||
forbid_overwrite: bool = True
|
||||
preserve_passive_joints: bool = True
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if len(self.source_sha256) != 64 or any(
|
||||
character not in "0123456789abcdef"
|
||||
for character in self.source_sha256.lower()
|
||||
):
|
||||
raise ValueError("source URDF SHA-256 is invalid")
|
||||
if self.allowed_active_joints & self.frozen_joints:
|
||||
raise ValueError("allowed and frozen URDF joints overlap")
|
||||
applied = self.allowed_active_joints | set(self.frozen_offsets_rad)
|
||||
if not set(self.frozen_offsets_rad).issubset(self.frozen_joints):
|
||||
raise ValueError("frozen offsets must belong to frozen joints")
|
||||
if not self.endpoint_limit_joints.issubset(applied):
|
||||
raise ValueError("endpoint limit joint is not an applied active joint")
|
||||
if set(self.mimic_source_by_joint) & self.allowed_active_joints:
|
||||
raise ValueError("dependent mimic joints cannot be active edit targets")
|
||||
|
||||
def authorize_offsets(self, offsets_rad: Mapping[str, float]) -> None:
|
||||
required = self.allowed_active_joints | set(self.frozen_offsets_rad)
|
||||
unexpected = set(offsets_rad) - required
|
||||
if unexpected:
|
||||
raise ValueError(
|
||||
"URDF correction contains unauthorized joints: "
|
||||
+ ", ".join(sorted(unexpected))
|
||||
)
|
||||
missing = required - set(offsets_rad)
|
||||
if missing:
|
||||
raise ValueError(
|
||||
"URDF correction is missing active joints: "
|
||||
+ ", ".join(sorted(missing))
|
||||
)
|
||||
changed_frozen = {
|
||||
name
|
||||
for name, expected in self.frozen_offsets_rad.items()
|
||||
if abs(float(offsets_rad[name]) - float(expected)) > 1.0e-12
|
||||
}
|
||||
if changed_frozen:
|
||||
raise ValueError(
|
||||
"URDF correction changed frozen offsets: "
|
||||
+ ", ".join(sorted(changed_frozen))
|
||||
)
|
||||
|
||||
def verify_source(self, source_urdf: str | Path) -> None:
|
||||
digest = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest()
|
||||
if digest != self.source_sha256.lower():
|
||||
raise ValueError("source URDF SHA-256 differs from correction plan")
|
||||
|
||||
|
||||
def build_correction_plan(
|
||||
profile: CalibrationProfile,
|
||||
*,
|
||||
source_sha256: str,
|
||||
scope: str,
|
||||
frozen_offsets_rad: Mapping[str, float] | None = None,
|
||||
) -> UrdfCorrectionPlan:
|
||||
"""Build one scope-aware edit authorization from typed policies."""
|
||||
selected = profile.scope.selected_joints(scope)
|
||||
frozen = profile.scope.frozen_joints[str(scope)]
|
||||
expected_frozen = {
|
||||
str(name): float(value)
|
||||
for name, value in dict(frozen_offsets_rad or {}).items()
|
||||
}
|
||||
if set(expected_frozen) != set(frozen):
|
||||
missing = set(frozen) - set(expected_frozen)
|
||||
extra = set(expected_frozen) - set(frozen)
|
||||
raise ValueError(
|
||||
"frozen URDF offset state differs from scope policy: "
|
||||
f"missing={','.join(sorted(missing)) or '-'};"
|
||||
f"extra={','.join(sorted(extra)) or '-'}"
|
||||
)
|
||||
applied = selected | frozen
|
||||
return UrdfCorrectionPlan(
|
||||
source_sha256=source_sha256,
|
||||
allowed_active_joints=selected,
|
||||
endpoint_limit_joints=(
|
||||
profile.zero.mechanical_endpoint_joints & applied
|
||||
),
|
||||
mimic_source_by_joint=profile.zero.mimic_source_by_joint,
|
||||
frozen_joints=frozen | profile.zero.cad_frozen_joints,
|
||||
frozen_offsets_rad=expected_frozen,
|
||||
)
|
||||
@@ -0,0 +1,150 @@
|
||||
"""Three-view compatibility policy over generic camera extrinsics."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from .core.geometry.extrinsics import (
|
||||
CameraCalibrationIdentity,
|
||||
CameraExtrinsics,
|
||||
camera_info_fingerprint,
|
||||
load_camera_extrinsics,
|
||||
matrix_payload,
|
||||
transform_matrix,
|
||||
validate_camera_extrinsics_payload,
|
||||
)
|
||||
|
||||
|
||||
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
||||
_QUALITY_LIMITS = {
|
||||
"reprojection_rms_px": 1.2,
|
||||
"maximum_rotation_repeatability_deg": 0.3,
|
||||
"maximum_translation_repeatability_m": 0.0015,
|
||||
}
|
||||
_MINIMUM_CAPTURE_COUNTS = {
|
||||
"front_side_captures": 15,
|
||||
"front_top_captures": 15,
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ThreeCameraExtrinsics:
|
||||
"""Transforms points from each camera optical frame into front optical."""
|
||||
|
||||
cameras: Mapping[str, CameraCalibrationIdentity]
|
||||
front_from_view: Mapping[str, np.ndarray]
|
||||
quality: Mapping[str, float]
|
||||
|
||||
def transform(self, view: str) -> np.ndarray:
|
||||
if view not in self.front_from_view:
|
||||
raise KeyError(f"extrinsics do not contain view {view}")
|
||||
return np.asarray(self.front_from_view[view], dtype=float).copy()
|
||||
|
||||
def camera_matches(
|
||||
self,
|
||||
view: str,
|
||||
*,
|
||||
serial_number: str,
|
||||
width: int,
|
||||
height: int,
|
||||
intrinsics_sha256: str,
|
||||
) -> bool:
|
||||
expected = self.cameras.get(view)
|
||||
return bool(
|
||||
expected is not None
|
||||
and expected.serial_number == str(serial_number)
|
||||
and expected.width == int(width)
|
||||
and expected.height == int(height)
|
||||
and expected.intrinsics_sha256 == str(intrinsics_sha256)
|
||||
)
|
||||
|
||||
|
||||
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
||||
validate_camera_extrinsics_payload(
|
||||
payload,
|
||||
required_views=VIEWS,
|
||||
reference_view="front",
|
||||
quality_limits=_QUALITY_LIMITS,
|
||||
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
|
||||
)
|
||||
|
||||
|
||||
def load_three_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
minimum_capture_counts: Mapping[str, int] | None = None,
|
||||
) -> ThreeCameraExtrinsics:
|
||||
generic = load_camera_extrinsics(
|
||||
path,
|
||||
required_views=VIEWS,
|
||||
reference_view="front",
|
||||
quality_limits=(
|
||||
_QUALITY_LIMITS if quality_limits is None else quality_limits
|
||||
),
|
||||
minimum_capture_counts=(
|
||||
_MINIMUM_CAPTURE_COUNTS
|
||||
if minimum_capture_counts is None
|
||||
else minimum_capture_counts
|
||||
),
|
||||
)
|
||||
return ThreeCameraExtrinsics(
|
||||
cameras=generic.cameras,
|
||||
front_from_view=generic.reference_from_view,
|
||||
quality=generic.quality,
|
||||
)
|
||||
|
||||
|
||||
def dump_three_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
cameras: Mapping[str, Mapping[str, Any]],
|
||||
front_from_view: Mapping[str, Sequence[Sequence[float]]],
|
||||
quality: Mapping[str, Any],
|
||||
quality_limits: Mapping[str, float] | None = None,
|
||||
) -> None:
|
||||
payload = {
|
||||
"schema_version": 1,
|
||||
"reference_view": "front",
|
||||
"cameras": {view: dict(cameras[view]) for view in VIEWS},
|
||||
"front_from_view": {
|
||||
view: matrix_payload(front_from_view[view]) for view in VIEWS
|
||||
},
|
||||
"quality": dict(quality),
|
||||
}
|
||||
validate_camera_extrinsics_payload(
|
||||
payload,
|
||||
required_views=VIEWS,
|
||||
reference_view="front",
|
||||
quality_limits=(
|
||||
_QUALITY_LIMITS if quality_limits is None else quality_limits
|
||||
),
|
||||
minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
|
||||
)
|
||||
destination = Path(path).expanduser().resolve()
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary = destination.with_suffix(destination.suffix + ".tmp")
|
||||
with temporary.open("w", encoding="utf-8") as stream:
|
||||
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
|
||||
temporary.replace(destination)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CameraCalibrationIdentity",
|
||||
"CameraExtrinsics",
|
||||
"ThreeCameraExtrinsics",
|
||||
"VIEWS",
|
||||
"camera_info_fingerprint",
|
||||
"dump_three_camera_extrinsics",
|
||||
"load_camera_extrinsics",
|
||||
"load_three_camera_extrinsics",
|
||||
"matrix_payload",
|
||||
"transform_matrix",
|
||||
"validate_camera_extrinsics_payload",
|
||||
"validate_extrinsics_payload",
|
||||
]
|
||||
+36
-5
@@ -605,6 +605,9 @@ class ThreeCameraExtrinsicsNode(Node):
|
||||
self.declare_parameter("minimum_captures_per_pair", 15)
|
||||
self.declare_parameter("maximum_pair_skew_ms", 100.0)
|
||||
self.declare_parameter("maximum_reprojection_rms_px", 1.2)
|
||||
self.declare_parameter(
|
||||
"maximum_candidate_pair_reprojection_rms_px", 1.5
|
||||
)
|
||||
self.declare_parameter(
|
||||
"maximum_single_camera_reprojection_rms_px", 1.5
|
||||
)
|
||||
@@ -640,6 +643,9 @@ class ThreeCameraExtrinsicsNode(Node):
|
||||
self.maximum_reprojection_rms_px = float(
|
||||
value("maximum_reprojection_rms_px")
|
||||
)
|
||||
self.maximum_candidate_pair_reprojection_rms_px = float(
|
||||
value("maximum_candidate_pair_reprojection_rms_px")
|
||||
)
|
||||
self.maximum_single_camera_reprojection_rms_px = float(
|
||||
value("maximum_single_camera_reprojection_rms_px")
|
||||
)
|
||||
@@ -671,13 +677,21 @@ class ThreeCameraExtrinsicsNode(Node):
|
||||
if self.maximum_reprojection_rms_px <= 0.0:
|
||||
raise ValueError("maximum_reprojection_rms_px must be positive")
|
||||
if (
|
||||
self.maximum_single_camera_reprojection_rms_px
|
||||
self.maximum_candidate_pair_reprojection_rms_px
|
||||
< self.maximum_reprojection_rms_px
|
||||
):
|
||||
raise ValueError(
|
||||
"maximum_single_camera_reprojection_rms_px must be at "
|
||||
"maximum_candidate_pair_reprojection_rms_px must be at "
|
||||
"least maximum_reprojection_rms_px"
|
||||
)
|
||||
if (
|
||||
self.maximum_single_camera_reprojection_rms_px
|
||||
< self.maximum_candidate_pair_reprojection_rms_px
|
||||
):
|
||||
raise ValueError(
|
||||
"maximum_single_camera_reprojection_rms_px must be at "
|
||||
"least maximum_candidate_pair_reprojection_rms_px"
|
||||
)
|
||||
if self.gui_refresh_hz <= 0.0 or self.gui_refresh_hz > 10.0:
|
||||
raise ValueError("gui_refresh_hz must be in (0, 10]")
|
||||
if self.auto_capture_stable_seconds < 0.5:
|
||||
@@ -869,14 +883,20 @@ class ThreeCameraExtrinsicsNode(Node):
|
||||
selected.reprojection_rms_px,
|
||||
self.maximum_single_camera_reprojection_rms_px,
|
||||
)
|
||||
if camera_rms_failed or pair_rms > self.maximum_reprojection_rms_px:
|
||||
if (
|
||||
camera_rms_failed
|
||||
or pair_rms
|
||||
> self.maximum_candidate_pair_reprojection_rms_px
|
||||
):
|
||||
return PairAssessment(
|
||||
ready=False,
|
||||
reason_code="reprojection_rms_too_large",
|
||||
message=(
|
||||
f"当前组未通过采集门限,未计入。单相机上限="
|
||||
f"{self.maximum_single_camera_reprojection_rms_px:.3f}px,"
|
||||
f"组合上限={self.maximum_reprojection_rms_px:.3f}px;"
|
||||
"候选组合上限="
|
||||
f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px,"
|
||||
f"最终批次上限={self.maximum_reprojection_rms_px:.3f}px;"
|
||||
f"组合RMS={pair_rms:.3f}px,"
|
||||
f"front={front.reprojection_rms_px:.3f}px,"
|
||||
f"{other}={selected.reprojection_rms_px:.3f}px;"
|
||||
@@ -1352,7 +1372,9 @@ class ThreeCameraExtrinsicsNode(Node):
|
||||
)
|
||||
cv2.putText(
|
||||
canvas,
|
||||
f"Pair RMS {pair_rms}/{self.maximum_reprojection_rms_px:.3f}px "
|
||||
"Pair RMS "
|
||||
f"{pair_rms}/"
|
||||
f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px "
|
||||
f"Skew {skew}/{self.maximum_pair_skew_ns / 1e6:.1f}ms "
|
||||
f"Batch RMS {batch_rms}/{self.maximum_reprojection_rms_px:.3f}px "
|
||||
f"stability {rotation}/"
|
||||
@@ -1526,6 +1548,15 @@ class ThreeCameraExtrinsicsNode(Node):
|
||||
cameras=self.camera_identities,
|
||||
front_from_view=transforms,
|
||||
quality=quality,
|
||||
quality_limits={
|
||||
"reprojection_rms_px": self.maximum_reprojection_rms_px,
|
||||
"maximum_rotation_repeatability_deg": math.degrees(
|
||||
self.maximum_rotation_repeatability_rad
|
||||
),
|
||||
"maximum_translation_repeatability_m": (
|
||||
self.maximum_translation_repeatability_m
|
||||
),
|
||||
},
|
||||
)
|
||||
response.success = True
|
||||
response.message = f"外参已保存:{self.output_file};{quality}"
|
||||
@@ -0,0 +1,7 @@
|
||||
"""One-release module alias for the relocated model profile implementation."""
|
||||
|
||||
import sys
|
||||
|
||||
from .models.g20 import profile as _implementation
|
||||
|
||||
sys.modules[__name__] = _implementation
|
||||
+1
-1
@@ -203,7 +203,7 @@ def configure_fastdds_large_image_transport() -> Path:
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
profile = (
|
||||
Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
|
||||
Path(get_package_share_directory("linkerhand_calibration"))
|
||||
/ "config"
|
||||
/ "fastdds_large_images.xml"
|
||||
)
|
||||
@@ -0,0 +1,17 @@
|
||||
"""Model- and side-specific calibration policies."""
|
||||
|
||||
from .registry import (
|
||||
EngineBindings,
|
||||
ProfileRegistry,
|
||||
RegisteredProfile,
|
||||
get_default_registry,
|
||||
)
|
||||
from .runtime_schema import validate_schema_v6_runtime_payload
|
||||
|
||||
__all__ = [
|
||||
"EngineBindings",
|
||||
"ProfileRegistry",
|
||||
"RegisteredProfile",
|
||||
"get_default_registry",
|
||||
"validate_schema_v6_runtime_payload",
|
||||
]
|
||||
@@ -0,0 +1,15 @@
|
||||
"""Registered profiles for this hand family."""
|
||||
|
||||
from ..registry import ProfileRegistry
|
||||
|
||||
|
||||
def register_profiles(registry: ProfileRegistry) -> None:
|
||||
from .legacy_11 import build_left_profile, build_right_profile
|
||||
from .right_19 import build_profile
|
||||
|
||||
registry.register(build_profile())
|
||||
registry.register(build_left_profile())
|
||||
registry.register(build_right_profile())
|
||||
|
||||
|
||||
__all__ = ["register_profiles"]
|
||||
@@ -0,0 +1,219 @@
|
||||
"""Adapt reviewed family profiles to the shared typed contract."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from ...core import (
|
||||
CalibrationProfile,
|
||||
CommandLayout,
|
||||
MeasurementPolicy,
|
||||
MeasurementSpec,
|
||||
MotionPolicy,
|
||||
ProfileKey,
|
||||
QualityPolicy,
|
||||
ScopePolicy,
|
||||
TagSpec,
|
||||
TaskSpec,
|
||||
ViewSpec,
|
||||
VisionRigSpec,
|
||||
ZeroSolvePolicy,
|
||||
)
|
||||
from .profile import MIMIC_DERIVED_FINGER_DIPS
|
||||
from ..registry import EngineBindings, RegisteredProfile
|
||||
from .artifacts import build_artifact_policy
|
||||
from .motion import (
|
||||
build_calibration_motion_command,
|
||||
build_calibration_preparation_waypoints,
|
||||
build_calibration_return_waypoints,
|
||||
)
|
||||
|
||||
|
||||
_HARD_THRESHOLD_KEYS = frozenset(
|
||||
{
|
||||
"minimum_detection_rate",
|
||||
"maximum_reprojection_error_px",
|
||||
"maximum_axis_cycle_difference_rad",
|
||||
"maximum_pose_line_rms_m",
|
||||
"maximum_hysteresis_rad",
|
||||
"maximum_validation_error_rad",
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
def _run_cli(args: list[str] | None = None) -> None:
|
||||
from .runner import main
|
||||
|
||||
main(args)
|
||||
|
||||
|
||||
def _run_node(args: list[str] | None = None) -> None:
|
||||
from .node import main
|
||||
|
||||
main(args)
|
||||
|
||||
|
||||
def adapt_profile(
|
||||
*,
|
||||
key: ProfileKey,
|
||||
namespace: str,
|
||||
hand_profile,
|
||||
zero_profile,
|
||||
mechanical_endpoint_joints: frozenset[str] = frozenset(),
|
||||
post_solve_endpoint_joints: frozenset[str] = frozenset(),
|
||||
) -> RegisteredProfile:
|
||||
fixed_by_view = {
|
||||
view: frozenset(roles)
|
||||
for view, roles in hand_profile.preflight_view_roles.items()
|
||||
}
|
||||
views = tuple(
|
||||
ViewSpec(
|
||||
name=view,
|
||||
tags=tuple(
|
||||
TagSpec(
|
||||
role=role,
|
||||
tag_id=int(tag_id),
|
||||
fixed_reference=role in fixed_by_view.get(view, frozenset()),
|
||||
)
|
||||
for role, tag_id in roles.items()
|
||||
),
|
||||
)
|
||||
for view, roles in hand_profile.view_tags.items()
|
||||
)
|
||||
record_specs = hand_profile.record_specs
|
||||
command_index_by_joint = {
|
||||
name: int(spec.motor_index) for name, spec in record_specs.items()
|
||||
}
|
||||
command_index_by_joint.update(
|
||||
{
|
||||
name: int(spec.motor_index)
|
||||
for name, spec in hand_profile.joint_specs.items()
|
||||
}
|
||||
)
|
||||
measurements = {
|
||||
name: MeasurementSpec(
|
||||
joint=name,
|
||||
kind=str(spec.zero_kind or "curve"),
|
||||
view=spec.view,
|
||||
parent_role=spec.parent_role,
|
||||
child_role=spec.child_role,
|
||||
validation_source=(hand_profile.axis_validation_sources or {}).get(
|
||||
name
|
||||
),
|
||||
pose_axis_line_required=bool(
|
||||
spec.pose_axis_line_required
|
||||
),
|
||||
)
|
||||
for name, spec in record_specs.items()
|
||||
}
|
||||
tasks = tuple(
|
||||
TaskSpec(
|
||||
key=spec.key,
|
||||
view=spec.view,
|
||||
command_index=int(spec.motor_index),
|
||||
joints=tuple(spec.joints),
|
||||
auxiliary_commands=tuple(spec.auxiliary_commands),
|
||||
validation_only=bool(spec.validation_only),
|
||||
)
|
||||
for spec in hand_profile.sweep_specs
|
||||
)
|
||||
active = frozenset(hand_profile.active_joints)
|
||||
passive = frozenset(hand_profile.passive_joints)
|
||||
thumb = frozenset(name for name in active if name.startswith("thumb_"))
|
||||
fingers = active - thumb
|
||||
typed = CalibrationProfile(
|
||||
key=key,
|
||||
namespace=namespace,
|
||||
command=CommandLayout(
|
||||
names=tuple(hand_profile.command_names),
|
||||
baseline_u8=tuple(int(value) for value in hand_profile.baseline_command),
|
||||
command_index_by_joint=command_index_by_joint,
|
||||
disabled_indices=frozenset(
|
||||
index
|
||||
for index, name in enumerate(hand_profile.command_names)
|
||||
if name.startswith("reserved_")
|
||||
),
|
||||
),
|
||||
vision=VisionRigSpec(
|
||||
views=views,
|
||||
common_frame="calibration_common",
|
||||
extrinsic_reference_view=views[0].name,
|
||||
extrinsics_quality_limits={
|
||||
"reprojection_rms_px": 1.2,
|
||||
"maximum_rotation_repeatability_deg": 0.3,
|
||||
"maximum_translation_repeatability_m": 0.0015,
|
||||
},
|
||||
minimum_capture_counts={
|
||||
"front_side_captures": 15,
|
||||
"front_top_captures": 15,
|
||||
},
|
||||
),
|
||||
motion=MotionPolicy(
|
||||
tasks=tasks,
|
||||
precheck_sweeps=bool(hand_profile.precheck_sweeps),
|
||||
steady_command_checkpoints=bool(
|
||||
hand_profile.steady_command_checkpoints
|
||||
),
|
||||
),
|
||||
measurement=MeasurementPolicy(
|
||||
measurements=measurements,
|
||||
cross_view_sources=dict(
|
||||
hand_profile.axis_validation_sources or {}
|
||||
),
|
||||
image_curve_joints=frozenset(
|
||||
hand_profile.image_trajectory_joints
|
||||
),
|
||||
directional_zero=bool(hand_profile.directional_zero),
|
||||
cross_view_roll_curve=bool(hand_profile.cross_view_roll_curve),
|
||||
stable_cross_view_cone_bias=bool(
|
||||
hand_profile.stable_cross_view_cone_bias
|
||||
),
|
||||
),
|
||||
zero=ZeroSolvePolicy(
|
||||
active_joints=active,
|
||||
passive_joints=passive,
|
||||
direct_zero_joints=tuple(zero_profile.direct_zero_joints),
|
||||
axis_joints=tuple(zero_profile.axis_joints),
|
||||
mechanical_endpoint_joints=mechanical_endpoint_joints,
|
||||
post_solve_endpoint_joints=post_solve_endpoint_joints,
|
||||
mimic_source_by_joint={
|
||||
target: source
|
||||
for target, source in MIMIC_DERIVED_FINGER_DIPS.items()
|
||||
if target in passive and source in active
|
||||
},
|
||||
cad_frozen_joints=frozenset(
|
||||
passive - set(zero_profile.static_output_zero_offsets_rad)
|
||||
),
|
||||
),
|
||||
quality=QualityPolicy(
|
||||
training_cycles=(0, 1, 2),
|
||||
holdout_cycle=3 if key.layout != "legacy_11" else None,
|
||||
hard_threshold_keys=_HARD_THRESHOLD_KEYS,
|
||||
isolated_holdout=bool(hand_profile.isolated_holdout),
|
||||
),
|
||||
scope=ScopePolicy(
|
||||
calibrate_joints={
|
||||
"full": active,
|
||||
"thumb": thumb,
|
||||
"fingers": fingers,
|
||||
},
|
||||
frozen_joints={
|
||||
"full": frozenset(),
|
||||
"thumb": fingers,
|
||||
"fingers": thumb,
|
||||
},
|
||||
),
|
||||
artifacts=build_artifact_policy(
|
||||
frozenset(hand_profile.capabilities)
|
||||
),
|
||||
)
|
||||
return RegisteredProfile(
|
||||
profile=typed,
|
||||
engine=EngineBindings(
|
||||
hand_profile=hand_profile,
|
||||
zero_profile=zero_profile,
|
||||
motion_command=build_calibration_motion_command,
|
||||
preparation_waypoints=build_calibration_preparation_waypoints,
|
||||
return_waypoints=build_calibration_return_waypoints,
|
||||
cli_main=_run_cli,
|
||||
node_main=_run_node,
|
||||
),
|
||||
)
|
||||
@@ -0,0 +1,30 @@
|
||||
"""Runtime JSON and corrected-URDF naming policy."""
|
||||
|
||||
from ...core import ArtifactPolicy
|
||||
|
||||
|
||||
def build_artifact_policy(
|
||||
compatibility_tokens: frozenset[str],
|
||||
) -> ArtifactPolicy:
|
||||
return ArtifactPolicy(
|
||||
output_schema_version=4,
|
||||
calibration_filename="g20_{side}_{serial_number}_calibration.json",
|
||||
corrected_urdf_filename=(
|
||||
"linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf"
|
||||
),
|
||||
protected_input_fields=frozenset(
|
||||
{
|
||||
"source_urdf_sha256",
|
||||
"camera_extrinsics_sha256",
|
||||
"calibration_config_sha256",
|
||||
"tag_config_sha256",
|
||||
}
|
||||
),
|
||||
session_compatibility_tokens=frozenset(compatibility_tokens),
|
||||
publish_corrected_urdf=(
|
||||
"urdf_zero_publication" in compatibility_tokens
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["build_artifact_policy"]
|
||||
@@ -0,0 +1,26 @@
|
||||
"""Reviewed command-channel layouts for this hand family."""
|
||||
|
||||
G20_COMMAND_NAMES: tuple[str, ...] = (
|
||||
"thumb_cmc_pitch",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"reserved_11",
|
||||
"reserved_12",
|
||||
"reserved_13",
|
||||
"reserved_14",
|
||||
"thumb_mcp",
|
||||
"index_pip",
|
||||
"middle_pip",
|
||||
"ring_pip",
|
||||
"pinky_pip",
|
||||
)
|
||||
|
||||
__all__ = ["G20_COMMAND_NAMES"]
|
||||
@@ -0,0 +1,144 @@
|
||||
"""Read-only regression checks for the reviewed hardware sessions."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
_PASS_SESSIONS = {
|
||||
"20260830_181154": "full",
|
||||
"20260831_141123": "thumb",
|
||||
"20260831_163843": "thumb",
|
||||
}
|
||||
_FAIL_SESSIONS = {
|
||||
"20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"),
|
||||
"20260831_142322": ("VAL-QUALITY-501", "zero_model_validation_failed"),
|
||||
}
|
||||
|
||||
|
||||
def _sha256_file(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _summary(session: Path) -> dict[str, Any]:
|
||||
value = json.loads(
|
||||
(session / "calibration_summary_zh.json").read_text(encoding="utf-8")
|
||||
)
|
||||
if not isinstance(value, dict):
|
||||
raise ValueError(f"invalid golden summary: {session}")
|
||||
return value
|
||||
|
||||
|
||||
def validate_golden_sessions(
|
||||
serial_root: str | Path,
|
||||
*,
|
||||
replay_full_session: bool = True,
|
||||
) -> dict[str, Any]:
|
||||
"""Prove PASS/FAIL, hashes, partial freezes, and failure non-publication."""
|
||||
root = Path(serial_root).expanduser().resolve()
|
||||
results: dict[str, Any] = {}
|
||||
for session_id, scope in _PASS_SESSIONS.items():
|
||||
session = root / session_id
|
||||
summary = _summary(session)
|
||||
if summary.get("result") != "PASS":
|
||||
raise ValueError(f"golden PASS changed: {session_id}")
|
||||
if summary.get("calibration_scope") != scope:
|
||||
raise ValueError(f"golden scope changed: {session_id}")
|
||||
artifacts = summary["artifacts"]
|
||||
hashes = summary["hashes"]
|
||||
calibration_json = session / artifacts["json"]
|
||||
corrected_urdf = session / artifacts["urdf"]
|
||||
actual_json_hash = _sha256_file(calibration_json)
|
||||
actual_urdf_hash = _sha256_file(corrected_urdf)
|
||||
if actual_json_hash != hashes["calibration_json_sha256"]:
|
||||
raise ValueError(f"golden JSON hash changed: {session_id}")
|
||||
if actual_urdf_hash != hashes["corrected_urdf_sha256"]:
|
||||
raise ValueError(f"golden URDF hash changed: {session_id}")
|
||||
if session_id == "20260831_141123" and len(
|
||||
summary.get("preserved_certified_zero_joints", ())
|
||||
) != 12:
|
||||
raise ValueError("merged thumb no longer freezes all finger zeros")
|
||||
if session_id == "20260831_163843":
|
||||
payload = json.loads(calibration_json.read_text(encoding="utf-8"))
|
||||
if payload.get("artifact_type") != (
|
||||
"g20_right_standalone_thumb_calibration"
|
||||
):
|
||||
raise ValueError("standalone thumb artifact type changed")
|
||||
results[session_id] = {
|
||||
"result": "PASS",
|
||||
"json_sha256": actual_json_hash,
|
||||
"urdf_sha256": actual_urdf_hash,
|
||||
}
|
||||
|
||||
for session_id, (error_code, reason) in _FAIL_SESSIONS.items():
|
||||
session = root / session_id
|
||||
summary = _summary(session)
|
||||
if (
|
||||
summary.get("result") != "FAIL"
|
||||
or summary.get("error_code") != error_code
|
||||
or summary.get("reason") != reason
|
||||
):
|
||||
raise ValueError(f"golden failure decision changed: {session_id}")
|
||||
results[session_id] = {
|
||||
"result": "FAIL",
|
||||
"error_code": error_code,
|
||||
"reason": reason,
|
||||
}
|
||||
|
||||
failure_ids = set(_FAIL_SESSIONS)
|
||||
for pointer_name in ("latest_passed", "latest_thumb_passed"):
|
||||
pointer = root / pointer_name
|
||||
if pointer.exists() and pointer.resolve().name in failure_ids:
|
||||
raise ValueError(f"failure session was published through {pointer_name}")
|
||||
|
||||
if replay_full_session:
|
||||
from .offline_replay import replay_session
|
||||
|
||||
session_id = "20260830_181154"
|
||||
replay = replay_session(root / session_id, write_outputs=False)
|
||||
expected = results[session_id]
|
||||
if replay["computed_final_json_sha256"] != expected["json_sha256"]:
|
||||
raise ValueError(
|
||||
"full-session replay JSON is not byte-identical: "
|
||||
f"computed={replay['computed_final_json_sha256']} "
|
||||
f"expected={expected['json_sha256']}"
|
||||
)
|
||||
if replay["corrected_urdf_sha256"] != expected["urdf_sha256"]:
|
||||
raise ValueError(
|
||||
"full-session replay URDF is not byte-identical: "
|
||||
f"computed={replay['corrected_urdf_sha256']} "
|
||||
f"expected={expected['urdf_sha256']}"
|
||||
)
|
||||
results[session_id]["offline_replay"] = "byte_identical"
|
||||
return results
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Validate the five reviewed calibration sessions"
|
||||
)
|
||||
parser.add_argument("serial_root")
|
||||
parser.add_argument("--no-replay", action="store_true")
|
||||
selected = parser.parse_args(args)
|
||||
print(
|
||||
json.dumps(
|
||||
validate_golden_sessions(
|
||||
selected.serial_root,
|
||||
replay_full_session=not selected.no_replay,
|
||||
),
|
||||
ensure_ascii=False,
|
||||
indent=2,
|
||||
sort_keys=True,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,35 @@
|
||||
"""One-release typed wrappers for the legacy 11-Tag layouts."""
|
||||
|
||||
from ...core import ProfileKey
|
||||
from .profile import get_hand_calibration_profile
|
||||
from ...urdf_zero import get_zero_calibration_profile
|
||||
from ..registry import RegisteredProfile
|
||||
from ._adapter import adapt_profile
|
||||
|
||||
|
||||
LEFT_KEY = ProfileKey("G20", "left", "legacy_11", 1)
|
||||
RIGHT_KEY = ProfileKey("G20", "right", "legacy_11", 1)
|
||||
|
||||
|
||||
def build_left_profile() -> RegisteredProfile:
|
||||
hand = get_hand_calibration_profile(LEFT_KEY.side, LEFT_KEY.layout)
|
||||
return adapt_profile(
|
||||
key=LEFT_KEY,
|
||||
namespace="/g20_calibration",
|
||||
hand_profile=hand,
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
LEFT_KEY.side, LEFT_KEY.layout
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def build_right_profile() -> RegisteredProfile:
|
||||
hand = get_hand_calibration_profile(RIGHT_KEY.side, RIGHT_KEY.layout)
|
||||
return adapt_profile(
|
||||
key=RIGHT_KEY,
|
||||
namespace="/g20_calibration",
|
||||
hand_profile=hand,
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
RIGHT_KEY.side, RIGHT_KEY.layout
|
||||
),
|
||||
)
|
||||
@@ -0,0 +1,15 @@
|
||||
"""Reviewed motion and safe-waypoint strategy exports."""
|
||||
|
||||
from .profile import (
|
||||
build_calibration_motion_command,
|
||||
build_calibration_preparation_waypoints,
|
||||
build_calibration_return_waypoints,
|
||||
build_calibration_speed_profile,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"build_calibration_motion_command",
|
||||
"build_calibration_preparation_waypoints",
|
||||
"build_calibration_return_waypoints",
|
||||
"build_calibration_speed_profile",
|
||||
]
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,914 @@
|
||||
"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime, timezone
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
from typing import Any, Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .profile import (
|
||||
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
get_hand_calibration_profile,
|
||||
validate_compact_payload,
|
||||
)
|
||||
from ...product import ProductConfig, sha256_file
|
||||
from ...storage import atomic_write_json
|
||||
from ...core.urdf import UrdfCorrectionPlan, build_correction_plan
|
||||
from .zero_solver import (
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
get_zero_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
PASSIVE_JOINTS = frozenset(
|
||||
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
||||
)
|
||||
VISUALLY_MEASURED_FINGER_DIPS = frozenset(
|
||||
{"index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
||||
)
|
||||
ACTIVE_ZERO_JOINTS = frozenset(
|
||||
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
|
||||
)
|
||||
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
||||
) - ACTIVE_ZERO_JOINTS
|
||||
SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9
|
||||
|
||||
|
||||
def _load_json(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = json.load(stream)
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"{path} must contain a JSON object")
|
||||
return payload
|
||||
|
||||
|
||||
def _joint_blocks(text: str) -> dict[str, str]:
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
|
||||
|
||||
|
||||
def _mask_origin_rpy(block: str) -> str:
|
||||
return re.sub(
|
||||
r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_RPY__\2",
|
||||
block,
|
||||
count=1,
|
||||
)
|
||||
|
||||
|
||||
def _mask_active_origin_rpy_fields(text: str) -> str:
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
|
||||
def replace(match: re.Match[str]) -> str:
|
||||
block = match.group(0)
|
||||
if match.group("name") not in ACTIVE_ZERO_JOINTS:
|
||||
return block
|
||||
masked = _mask_origin_rpy(block)
|
||||
if masked == block:
|
||||
raise ValueError(
|
||||
f"active joint {match.group('name')} has no origin.rpy"
|
||||
)
|
||||
return masked
|
||||
|
||||
return pattern.sub(replace, text)
|
||||
|
||||
|
||||
def _mask_endpoint_coordinate_fields(text: str) -> str:
|
||||
"""Mask only limit/mimic fields induced by endpoint zero coordinates."""
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
|
||||
def replace(match: re.Match[str]) -> str:
|
||||
block = match.group(0)
|
||||
name = match.group("name")
|
||||
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS:
|
||||
return re.sub(
|
||||
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_UPPER__\2",
|
||||
block,
|
||||
count=1,
|
||||
)
|
||||
mimic = re.search(
|
||||
r"<mimic\b[^>]*\bjoint\s*=\s*([\"'])(?P<source>[^\"']+)\1[^>]*>",
|
||||
block,
|
||||
re.DOTALL,
|
||||
)
|
||||
if (
|
||||
mimic is not None
|
||||
and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
):
|
||||
return re.sub(
|
||||
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_MIMIC_OFFSET__\2",
|
||||
block,
|
||||
count=1,
|
||||
)
|
||||
return block
|
||||
|
||||
return pattern.sub(replace, text)
|
||||
|
||||
|
||||
def _triplet(value: str) -> np.ndarray:
|
||||
result = np.asarray([float(item) for item in value.split()], dtype=float)
|
||||
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
||||
raise ValueError(f"invalid URDF triplet: {value}")
|
||||
return result
|
||||
|
||||
|
||||
def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(Path(path)).getroot().findall("joint")
|
||||
}
|
||||
|
||||
|
||||
def _verify_expected_origin_offsets(
|
||||
source: str | Path,
|
||||
corrected: str | Path,
|
||||
expected_offsets_rad: Mapping[str, float],
|
||||
) -> None:
|
||||
offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
|
||||
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
|
||||
not math.isfinite(value) for value in offsets.values()
|
||||
):
|
||||
raise ValueError("expected offsets must contain all finite active static-zero values")
|
||||
before = _joint_elements(source)
|
||||
after = _joint_elements(corrected)
|
||||
maximum_rotation_error = 0.0
|
||||
for name, original_joint in before.items():
|
||||
corrected_joint = after[name]
|
||||
original_origin = original_joint.find("origin")
|
||||
corrected_origin = corrected_joint.find("origin")
|
||||
if original_origin is None or corrected_origin is None:
|
||||
if original_origin is not corrected_origin:
|
||||
raise ValueError(f"corrected URDF changed origin presence for {name}")
|
||||
continue
|
||||
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
|
||||
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
|
||||
if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
|
||||
raise ValueError(f"corrected URDF changed origin.xyz for {name}")
|
||||
original_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
corrected_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
expected_rotation = original_rotation
|
||||
if name in offsets:
|
||||
axis_node = original_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"source URDF joint {name} has a degenerate axis")
|
||||
expected_rotation = expected_rotation * Rotation.from_rotvec(
|
||||
axis / norm * offsets[name]
|
||||
)
|
||||
error = float((expected_rotation.inv() * corrected_rotation).magnitude())
|
||||
maximum_rotation_error = max(maximum_rotation_error, error)
|
||||
# Schema v4 intentionally stores zero offsets at eight decimal places.
|
||||
# Older sessions wrote the corrected URDF from the full-precision solve,
|
||||
# so comparing that file with the published JSON necessarily permits half
|
||||
# of one last-place unit. This is about 2.9e-7 degrees and is far below
|
||||
# any calibration or URDF numerical significance.
|
||||
if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD:
|
||||
raise ValueError(
|
||||
"corrected URDF origin.rpy does not match the published zero offsets"
|
||||
)
|
||||
|
||||
|
||||
def verify_corrected_urdf(
|
||||
source: str | Path,
|
||||
corrected: str | Path,
|
||||
*,
|
||||
expected_offsets_rad: Mapping[str, float] | None = None,
|
||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||
correction_plan: UrdfCorrectionPlan | None = None,
|
||||
) -> tuple[str, ...]:
|
||||
"""Prove that only active-joint origin.rpy attributes changed.
|
||||
|
||||
Passive joint blocks, including their mimic elements, are compared as raw
|
||||
UTF-8 text so formatting and numeric spelling are protected as well.
|
||||
"""
|
||||
source_text = Path(source).read_text(encoding="utf-8")
|
||||
corrected_text = Path(corrected).read_text(encoding="utf-8")
|
||||
if correction_plan is not None:
|
||||
correction_plan.verify_source(source)
|
||||
if expected_offsets_rad is None:
|
||||
raise ValueError(
|
||||
"correction-plan validation requires expected offsets"
|
||||
)
|
||||
correction_plan.authorize_offsets(expected_offsets_rad)
|
||||
before = _joint_blocks(source_text)
|
||||
after = _joint_blocks(corrected_text)
|
||||
if set(before) != set(after):
|
||||
raise ValueError("corrected URDF changed the joint set")
|
||||
# Text patchers may normalize only the final newline. It has no URDF/XML
|
||||
# semantics; every joint block and every non-rpy byte remains guarded
|
||||
# below.
|
||||
masked_source = _mask_endpoint_coordinate_fields(
|
||||
_mask_active_origin_rpy_fields(source_text)
|
||||
).rstrip("\r\n")
|
||||
masked_corrected = _mask_endpoint_coordinate_fields(
|
||||
_mask_active_origin_rpy_fields(corrected_text)
|
||||
).rstrip("\r\n")
|
||||
if masked_source != masked_corrected:
|
||||
raise ValueError(
|
||||
"corrected URDF changed content outside active origin.rpy or "
|
||||
"endpoint-calibrated fields"
|
||||
)
|
||||
changed: list[str] = []
|
||||
for name in before:
|
||||
if before[name] == after[name]:
|
||||
continue
|
||||
if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS:
|
||||
raise ValueError(f"corrected URDF changed protected joint {name}")
|
||||
if name in ACTIVE_ZERO_JOINTS:
|
||||
changed.append(name)
|
||||
if expected_offsets_rad is not None:
|
||||
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
|
||||
endpoint_offsets = {
|
||||
str(name): float(value)
|
||||
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
||||
}
|
||||
if endpoint_offsets:
|
||||
if correction_plan is not None and not set(endpoint_offsets).issubset(
|
||||
correction_plan.endpoint_limit_joints
|
||||
):
|
||||
raise ValueError(
|
||||
"endpoint offsets are not authorized by the correction plan"
|
||||
)
|
||||
source_joints = _joint_elements(source)
|
||||
corrected_joints = _joint_elements(corrected)
|
||||
for name, offset in endpoint_offsets.items():
|
||||
original_limit = source_joints[name].find("limit")
|
||||
corrected_limit = corrected_joints[name].find("limit")
|
||||
expected_upper = float(original_limit.get("upper")) - offset
|
||||
# Schema v4 stores the corresponding zero at eight decimal
|
||||
# places, while the URDF is written from the full-precision solve.
|
||||
# Match the half-last-place tolerance used for origin rotations.
|
||||
if (
|
||||
abs(float(corrected_limit.get("upper")) - expected_upper)
|
||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
):
|
||||
raise ValueError(f"corrected URDF has invalid {name} upper limit")
|
||||
for name in PASSIVE_JOINTS:
|
||||
original_mimic = source_joints[name].find("mimic")
|
||||
corrected_mimic = corrected_joints[name].find("mimic")
|
||||
if original_mimic is None or corrected_mimic is None:
|
||||
continue
|
||||
source_name = str(original_mimic.get("joint"))
|
||||
multiplier = float(original_mimic.get("multiplier", "1"))
|
||||
expected = float(original_mimic.get("offset", "0"))
|
||||
expected += multiplier * endpoint_offsets.get(source_name, 0.0)
|
||||
# The active endpoint offset comes from schema-v4 JSON rounded to
|
||||
# eight decimal places, while the URDF mimic was written from the
|
||||
# full-precision solve. Propagate exactly the same accepted
|
||||
# quantization through the mimic multiplier; retain a much
|
||||
# smaller allowance for XML decimal formatting itself.
|
||||
tolerance = (
|
||||
abs(multiplier)
|
||||
* SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
+ 1.0e-12
|
||||
)
|
||||
if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance:
|
||||
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
|
||||
return tuple(sorted(changed))
|
||||
|
||||
|
||||
def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
|
||||
"""Return every session-local mesh after proving it resolves safely."""
|
||||
path = Path(urdf).expanduser().resolve()
|
||||
root = ET.parse(path).getroot()
|
||||
resources: dict[str, Path] = {}
|
||||
for mesh in root.findall(".//mesh"):
|
||||
filename = str(mesh.get("filename", "")).strip()
|
||||
if not filename or "://" in filename or filename.startswith("package:"):
|
||||
continue
|
||||
relative = Path(filename)
|
||||
if relative.is_absolute() or ".." in relative.parts:
|
||||
raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
|
||||
resolved = (path.parent / relative).resolve()
|
||||
try:
|
||||
resolved.relative_to(path.parent)
|
||||
except ValueError as error:
|
||||
raise ValueError(f"URDF mesh escapes the session: {filename}") from error
|
||||
if not resolved.is_file():
|
||||
raise ValueError(f"URDF mesh resource is missing: {filename}")
|
||||
resources[relative.as_posix()] = resolved
|
||||
return dict(sorted(resources.items()))
|
||||
|
||||
|
||||
def validate_runtime_curves_against_urdf_limits(
|
||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||
) -> None:
|
||||
"""Reject a curve whose q leaves its runtime URDF coordinate limits.
|
||||
|
||||
Curves in schema v4 are expressed in the corrected URDF joint coordinate,
|
||||
not in the source-CAD coordinate. Endpoint zero calibration can therefore
|
||||
move a corrected coordinate limit while preserving the same physical CAD
|
||||
endpoint; callers publishing a calibrated pair must pass that corrected
|
||||
URDF here.
|
||||
"""
|
||||
validate_compact_payload(payload)
|
||||
runtime_joints = _joint_elements(runtime_urdf)
|
||||
for name, calibration in payload["joints"].items():
|
||||
joint = runtime_joints.get(str(name))
|
||||
if joint is None:
|
||||
raise ValueError(f"runtime URDF is missing joint {name}")
|
||||
limit = joint.find("limit")
|
||||
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
|
||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||
curve = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
minimum = float(np.min(curve))
|
||||
maximum = float(np.max(curve))
|
||||
tolerance = 1.0e-7
|
||||
if minimum < lower - tolerance or maximum > upper + tolerance:
|
||||
raise ValueError(
|
||||
f"runtime curve exceeds runtime URDF limit for {name}: "
|
||||
f"[{minimum:.9g}, {maximum:.9g}] not within "
|
||||
f"[{lower:.9g}, {upper:.9g}]"
|
||||
)
|
||||
|
||||
|
||||
def clamp_compact_payload_to_urdf_limits(
|
||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||
) -> tuple[dict[str, Any], dict[str, int]]:
|
||||
"""Return a schema-preserving payload bounded in its runtime coordinates."""
|
||||
result = copy.deepcopy(dict(payload))
|
||||
validate_compact_payload(result)
|
||||
runtime_joints = _joint_elements(runtime_urdf)
|
||||
clipped_by_joint: dict[str, int] = {}
|
||||
for name, calibration in result["joints"].items():
|
||||
joint = runtime_joints.get(str(name))
|
||||
limit = None if joint is None else joint.find("limit")
|
||||
if (
|
||||
limit is None
|
||||
or limit.get("lower") is None
|
||||
or limit.get("upper") is None
|
||||
):
|
||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||
source = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
bounded = np.clip(source, lower, upper)
|
||||
count = int(np.count_nonzero(bounded != source))
|
||||
if count:
|
||||
clipped_by_joint[str(name)] = count
|
||||
calibration["angle_rad"] = [
|
||||
round(float(value), 8) for value in bounded
|
||||
]
|
||||
validate_compact_payload(result)
|
||||
return result, clipped_by_joint
|
||||
|
||||
|
||||
def build_mujoco_validation_commands(
|
||||
baseline: Sequence[int],
|
||||
) -> dict[str, Any]:
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 commands")
|
||||
base = [int(value) for value in baseline]
|
||||
if any(not 0 <= value <= 255 for value in base):
|
||||
raise ValueError("baseline commands must be in [0, 255]")
|
||||
|
||||
def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
|
||||
values = list(base)
|
||||
for index, value in changes.items():
|
||||
values[int(index)] = int(value)
|
||||
return {"name": name, "label_zh": label, "command_u8": values}
|
||||
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"topic": "/g20/cb_right_hand_control_cmd",
|
||||
"serial_number": "G20_RIGHT_001",
|
||||
"poses": [
|
||||
pose("all_open", "全开", {}),
|
||||
pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
|
||||
pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
|
||||
pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
|
||||
pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
|
||||
pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
|
||||
pose(
|
||||
"half_grip",
|
||||
"四指半握",
|
||||
{1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
|
||||
),
|
||||
pose(
|
||||
"light_pinch",
|
||||
"轻捏",
|
||||
{0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
|
||||
),
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
|
||||
parent = Path(root).resolve()
|
||||
target = Path(session).resolve()
|
||||
if target.parent != parent:
|
||||
raise ValueError("session pointer target must be a direct child of the serial root")
|
||||
if not target.is_dir():
|
||||
raise ValueError(f"session directory does not exist: {target}")
|
||||
if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}:
|
||||
raise ValueError("unsupported session pointer name")
|
||||
parent.mkdir(parents=True, exist_ok=True)
|
||||
destination = parent / name
|
||||
temporary = parent / f".{name}.{os.getpid()}.tmp"
|
||||
if temporary.is_symlink():
|
||||
temporary.unlink()
|
||||
elif temporary.exists():
|
||||
raise ValueError(f"temporary pointer path is occupied: {temporary}")
|
||||
os.symlink(target.name, temporary, target_is_directory=True)
|
||||
os.replace(temporary, destination)
|
||||
return destination
|
||||
|
||||
|
||||
def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||
validate_compact_payload(payload)
|
||||
result = {
|
||||
name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
}
|
||||
if set(result) != ACTIVE_ZERO_JOINTS or any(
|
||||
not math.isfinite(value) for value in result.values()
|
||||
):
|
||||
raise ValueError(
|
||||
"payload does not contain all finite observable active zero offsets"
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||
"""Validate a standalone thumb artifact and expand CAD finger zeros."""
|
||||
if (
|
||||
payload.get("schema_version") != 1
|
||||
or payload.get("artifact_type")
|
||||
!= "g20_right_standalone_thumb_calibration"
|
||||
or payload.get("side") != "right"
|
||||
or payload.get("non_thumb_zero_policy") != "source_cad_unchanged"
|
||||
or not bool(payload.get("quality", {}).get("passed"))
|
||||
):
|
||||
raise ValueError("standalone thumb JSON contract is invalid")
|
||||
joints = payload.get("joints")
|
||||
expected_joint_names = {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
}
|
||||
if not isinstance(joints, Mapping) or set(joints) != expected_joint_names:
|
||||
raise ValueError("standalone thumb JSON has the wrong joint set")
|
||||
thumb_names = expected_joint_names - {"thumb_ip"}
|
||||
result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS}
|
||||
for name in thumb_names:
|
||||
try:
|
||||
value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||
except (KeyError, TypeError, ValueError) as error:
|
||||
raise ValueError(
|
||||
f"standalone thumb JSON is missing zero for {name}"
|
||||
) from error
|
||||
curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"standalone thumb JSON curve is invalid: {name}")
|
||||
result[name] = value
|
||||
passive_curve = np.asarray(
|
||||
joints["thumb_ip"].get("angle_rad", ()), dtype=float
|
||||
)
|
||||
if (
|
||||
passive_curve.shape != (256,)
|
||||
or not np.all(np.isfinite(passive_curve))
|
||||
or not bool(joints["thumb_ip"].get("passive"))
|
||||
or any(not math.isfinite(value) for value in result.values())
|
||||
):
|
||||
raise ValueError("standalone thumb JSON passive curve is invalid")
|
||||
return result
|
||||
|
||||
|
||||
def verify_partial_scope_preserves_certified_zeros(
|
||||
*,
|
||||
scope: str,
|
||||
source_session: str | Path,
|
||||
serial_root: str | Path | None = None,
|
||||
serial_number: str,
|
||||
current_offsets: Mapping[str, float],
|
||||
) -> None:
|
||||
"""Reject a partial artifact that changes any certified non-target zero."""
|
||||
selected = str(scope).strip().lower()
|
||||
if selected not in {"thumb", "fingers"}:
|
||||
return
|
||||
source = Path(source_session).expanduser()
|
||||
if not source.is_absolute():
|
||||
if serial_root is None:
|
||||
raise ValueError(
|
||||
"relative partial base session requires its serial root"
|
||||
)
|
||||
source = Path(serial_root).expanduser() / source
|
||||
source = source.resolve()
|
||||
base_payload = _load_json(
|
||||
source / f"g20_right_{serial_number}_calibration.json"
|
||||
)
|
||||
base_offsets = (
|
||||
standalone_thumb_offsets(base_payload)
|
||||
if base_payload.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
else active_offsets(base_payload)
|
||||
)
|
||||
preserve_thumb = selected == "fingers"
|
||||
changed = {
|
||||
name: abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
if name.startswith("thumb_") == preserve_thumb
|
||||
and abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
}
|
||||
if changed:
|
||||
raise ValueError(
|
||||
f"partial {selected} calibration changed certified non-target "
|
||||
"zeros: "
|
||||
+ ",".join(sorted(changed))
|
||||
)
|
||||
|
||||
|
||||
def compare_session_offsets(
|
||||
first: Mapping[str, Any],
|
||||
second: Mapping[str, Any],
|
||||
*,
|
||||
maximum_difference_rad: float,
|
||||
) -> dict[str, float]:
|
||||
first_standalone = (
|
||||
first.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
second_standalone = (
|
||||
second.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
if first_standalone != second_standalone:
|
||||
raise ValueError("cannot compare standalone thumb and full-hand artifacts")
|
||||
left = (
|
||||
standalone_thumb_offsets(first)
|
||||
if first_standalone
|
||||
else active_offsets(first)
|
||||
)
|
||||
right = (
|
||||
standalone_thumb_offsets(second)
|
||||
if second_standalone
|
||||
else active_offsets(second)
|
||||
)
|
||||
differences = {
|
||||
name: abs(left[name] - right[name])
|
||||
for name in sorted(left)
|
||||
}
|
||||
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
|
||||
if failed:
|
||||
details = ", ".join(
|
||||
f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
|
||||
)
|
||||
raise ValueError(f"independent-session static-zero mismatch: {details}")
|
||||
return differences
|
||||
|
||||
|
||||
def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
|
||||
directory = Path(session).resolve()
|
||||
urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
|
||||
if len(urdfs) != 1:
|
||||
raise ValueError("session must contain exactly one corrected URDF")
|
||||
return {
|
||||
"json": directory / f"g20_right_{serial_number}_calibration.json",
|
||||
"urdf": urdfs[0],
|
||||
"summary": directory / "calibration_summary_zh.json",
|
||||
"commands": directory / "mujoco_validation_commands.json",
|
||||
"raw": directory / "raw_samples.jsonl",
|
||||
"log": directory / "calibration.log",
|
||||
}
|
||||
|
||||
|
||||
def find_compatible_prior_session(
|
||||
config: ProductConfig,
|
||||
current: str | Path,
|
||||
current_payload: Mapping[str, Any],
|
||||
) -> tuple[Path | None, dict[str, float]]:
|
||||
current_path = Path(current).resolve()
|
||||
for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
|
||||
if candidate.resolve() == current_path:
|
||||
continue
|
||||
json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
if not json_path.is_file() or not summary_path.is_file():
|
||||
continue
|
||||
try:
|
||||
summary = _load_json(summary_path)
|
||||
if not bool(summary.get("quality", {}).get("passed")):
|
||||
continue
|
||||
payload = _load_json(json_path)
|
||||
differences = compare_session_offsets(
|
||||
payload,
|
||||
current_payload,
|
||||
maximum_difference_rad=config.static_repeatability_rad,
|
||||
)
|
||||
except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
|
||||
continue
|
||||
return candidate, differences
|
||||
return None, {}
|
||||
|
||||
|
||||
def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
|
||||
# The formal product uses the independently held-out fourth sweep cycle.
|
||||
# The optional eight-pose check remains a developer diagnostic because the
|
||||
# axis-line zero solve does not establish an absolute Cartesian hand-base
|
||||
# transform. When explicitly enabled, retain its strict coverage and
|
||||
# error checks.
|
||||
if not bool(combination.get("enabled")):
|
||||
return
|
||||
position_p95 = float(combination.get("position_p95_m", float("inf")))
|
||||
orientation_p95 = float(
|
||||
combination.get("orientation_p95_rad", float("inf"))
|
||||
)
|
||||
required = {str(value) for value in combination.get("required_targets", ())}
|
||||
expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
|
||||
observations = combination.get("observation_counts")
|
||||
validations = combination.get("validation_counts")
|
||||
if required != expected:
|
||||
raise ValueError("combination validation has the wrong required-target set")
|
||||
if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
|
||||
raise ValueError("combination validation is missing per-target coverage")
|
||||
missing_observations = sorted(
|
||||
key for key in expected if int(observations.get(key, 0)) < 2
|
||||
)
|
||||
missing_validations = sorted(
|
||||
key for key in expected if int(validations.get(key, 0)) < 1
|
||||
)
|
||||
if missing_observations or missing_validations:
|
||||
raise ValueError(
|
||||
"combination validation target coverage is incomplete: "
|
||||
f"observations={','.join(missing_observations) or '-'}; "
|
||||
f"validations={','.join(missing_validations) or '-'}"
|
||||
)
|
||||
if (
|
||||
not bool(combination.get("completed"))
|
||||
or int(combination.get("completed_poses", 0)) != 8
|
||||
or position_p95 > 0.003
|
||||
or orientation_p95 > math.radians(2.0)
|
||||
):
|
||||
raise ValueError(
|
||||
"eight-pose four-finger combination validation is incomplete or failed"
|
||||
)
|
||||
|
||||
|
||||
def finalize_session_artifacts(
|
||||
config: ProductConfig,
|
||||
session: str | Path,
|
||||
*,
|
||||
node_status: Mapping[str, Any],
|
||||
) -> tuple[dict[str, Any], bool]:
|
||||
"""Create diagnostics and update latest_passed only after every guard."""
|
||||
directory = Path(session).resolve()
|
||||
paths = session_artifact_paths(directory, config.serial_number)
|
||||
for name in ("json", "urdf", "raw", "log"):
|
||||
if not paths[name].is_file():
|
||||
raise ValueError(f"session is missing {name}: {paths[name]}")
|
||||
payload = _load_json(paths["json"])
|
||||
standalone_thumb = bool(
|
||||
payload.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
if standalone_thumb:
|
||||
standalone_thumb_offsets(payload)
|
||||
else:
|
||||
validate_compact_payload(payload)
|
||||
if payload.get("schema_version") != 4 or payload.get("side") != "right":
|
||||
raise ValueError("runtime JSON is not the compact right-hand schema v4")
|
||||
if not bool(payload.get("quality", {}).get("passed")):
|
||||
raise ValueError("runtime JSON quality is not passed")
|
||||
combination = node_status.get("combination_validation")
|
||||
if not isinstance(combination, Mapping):
|
||||
raise ValueError("node status is missing combination validation")
|
||||
_verify_combination_validation(combination)
|
||||
resume = node_status.get("resume", {})
|
||||
if not isinstance(resume, Mapping):
|
||||
raise ValueError("node status has invalid resume provenance")
|
||||
calibration_scope = str(
|
||||
resume.get("recalibration_scope", "full")
|
||||
).strip().lower()
|
||||
if calibration_scope not in {"full", "thumb", "fingers"}:
|
||||
raise ValueError("node status has an unsupported recalibration scope")
|
||||
recalibration_tasks = tuple(
|
||||
str(value) for value in resume.get("recalibration_task_keys", ())
|
||||
)
|
||||
if calibration_scope == "thumb" and not standalone_thumb and (
|
||||
not bool(resume.get("used"))
|
||||
or not str(resume.get("source_session", ""))
|
||||
or len(recalibration_tasks) != 4
|
||||
or any("thumb_" not in name for name in recalibration_tasks)
|
||||
):
|
||||
raise ValueError(
|
||||
"thumb recalibration is missing its passed base-session provenance"
|
||||
)
|
||||
if calibration_scope == "fingers" and (
|
||||
not bool(resume.get("used"))
|
||||
or not str(resume.get("source_session", ""))
|
||||
or len(recalibration_tasks) != 12
|
||||
or any("thumb_" in name for name in recalibration_tasks)
|
||||
):
|
||||
raise ValueError(
|
||||
"finger recalibration is missing its certified thumb-session "
|
||||
"provenance"
|
||||
)
|
||||
if standalone_thumb and calibration_scope != "thumb":
|
||||
raise ValueError("standalone thumb artifact has the wrong node scope")
|
||||
offsets = (
|
||||
standalone_thumb_offsets(payload)
|
||||
if standalone_thumb
|
||||
else active_offsets(payload)
|
||||
)
|
||||
if calibration_scope != "full" and not standalone_thumb:
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope=calibration_scope,
|
||||
source_session=str(resume.get("source_session", "")),
|
||||
serial_root=directory.parent,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=offsets,
|
||||
)
|
||||
endpoint_offsets = {
|
||||
name: offsets[name]
|
||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
}
|
||||
typed_profile = config.calibration_contract.typed_profile
|
||||
frozen_names = typed_profile.scope.frozen_joints[calibration_scope]
|
||||
correction_plan = build_correction_plan(
|
||||
typed_profile,
|
||||
source_sha256=config.source_urdf_sha256,
|
||||
scope=calibration_scope,
|
||||
frozen_offsets_rad={name: offsets[name] for name in frozen_names},
|
||||
)
|
||||
changed_joints = verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
correction_plan=correction_plan,
|
||||
)
|
||||
if standalone_thumb:
|
||||
clipped_runtime_joints = {}
|
||||
else:
|
||||
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
||||
payload, paths["urdf"]
|
||||
)
|
||||
if clipped_runtime_joints:
|
||||
atomic_write_json(paths["json"], payload)
|
||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
|
||||
mesh_hashes = {
|
||||
name: sha256_file(path) for name, path in mesh_resources.items()
|
||||
}
|
||||
commands = (
|
||||
{
|
||||
"artifact_type": "standalone_thumb_no_full_hand_validation",
|
||||
"poses": [],
|
||||
}
|
||||
if standalone_thumb
|
||||
else build_mujoco_validation_commands(payload["baseline_command_u8"])
|
||||
)
|
||||
atomic_write_json(paths["commands"], commands)
|
||||
|
||||
prior, differences = find_compatible_prior_session(config, directory, payload)
|
||||
release_ready = config.required_independent_passes == 1 or prior is not None
|
||||
quality = dict(payload["quality"])
|
||||
preserved_certified_zeros = sorted(
|
||||
name
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
if (
|
||||
calibration_scope == "thumb"
|
||||
and not name.startswith("thumb_")
|
||||
)
|
||||
or (
|
||||
calibration_scope == "fingers"
|
||||
and name.startswith("thumb_")
|
||||
)
|
||||
)
|
||||
if standalone_thumb:
|
||||
preserved_certified_zeros = []
|
||||
calibrated_zero_joints = (
|
||||
sorted(
|
||||
name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_")
|
||||
)
|
||||
if standalone_thumb
|
||||
else sorted(ACTIVE_ZERO_JOINTS)
|
||||
)
|
||||
summary: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"serial_number": config.serial_number,
|
||||
"session_id": f"{config.serial_number}_{directory.name}",
|
||||
"calibration_scope": calibration_scope,
|
||||
"inherited_base_session": (
|
||||
None
|
||||
if calibration_scope == "full" or standalone_thumb
|
||||
else str(resume.get("source_session"))
|
||||
),
|
||||
"freshly_calibrated_task_keys": list(recalibration_tasks),
|
||||
"preserved_certified_zero_joints": preserved_certified_zeros,
|
||||
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
|
||||
"quality": quality,
|
||||
"runtime_limit_clipped_bins": clipped_runtime_joints,
|
||||
"runtime_curve_domain": (
|
||||
"thumb_diagnostic_only"
|
||||
if standalone_thumb
|
||||
else "requested_command_u8"
|
||||
),
|
||||
"static_zero_calibrated_joints": calibrated_zero_joints,
|
||||
"non_thumb_zero_policy": (
|
||||
"source_cad_unchanged" if standalone_thumb else None
|
||||
),
|
||||
"retained_active_urdf_zero_joints": sorted(
|
||||
RETAINED_ACTIVE_ZERO_JOINTS
|
||||
),
|
||||
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
|
||||
"mimic_derived_dynamic_joints": [],
|
||||
"visually_measured_passive_joints": sorted(
|
||||
VISUALLY_MEASURED_FINGER_DIPS
|
||||
),
|
||||
"changed_urdf_joint_origins": list(changed_joints),
|
||||
"hashes": {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"corrected_urdf_sha256": sha256_file(paths["urdf"]),
|
||||
"mesh_resources_sha256": mesh_hashes,
|
||||
"calibration_json_sha256": sha256_file(paths["json"]),
|
||||
"raw_samples_sha256": sha256_file(paths["raw"]),
|
||||
},
|
||||
"holdout": {
|
||||
"training_cycles": [0, 1, 2],
|
||||
"validation_cycle": 3,
|
||||
"isolated": True,
|
||||
},
|
||||
"combination_validation": dict(combination),
|
||||
"formal_release": {
|
||||
"passed": release_ready,
|
||||
"required_independent_passes": config.required_independent_passes,
|
||||
"comparison_session": None if prior is None else prior.name,
|
||||
"maximum_static_difference_deg": (
|
||||
None if not differences else math.degrees(max(differences.values()))
|
||||
),
|
||||
},
|
||||
"node_status": dict(node_status),
|
||||
"created_at_utc": datetime.now(timezone.utc).isoformat(),
|
||||
"artifacts": {name: path.name for name, path in paths.items()},
|
||||
}
|
||||
atomic_write_json(paths["summary"], summary)
|
||||
# Recompute hashes after all files exist and validate the pair once more
|
||||
# immediately before the one atomic publication operation.
|
||||
if sha256_file(config.source_urdf) != config.source_urdf_sha256:
|
||||
raise ValueError("source URDF changed during calibration")
|
||||
verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
)
|
||||
# Revalidate the same coordinate contract immediately before publication:
|
||||
# verify_corrected_urdf proves that corrected endpoint limits map back to
|
||||
# the original physical CAD endpoints, while the runtime curves must stay
|
||||
# inside those corrected-coordinate limits.
|
||||
if not standalone_thumb:
|
||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||
final_mesh_hashes = {
|
||||
name: sha256_file(path)
|
||||
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
||||
}
|
||||
if final_mesh_hashes != mesh_hashes:
|
||||
raise ValueError("URDF mesh resources changed during publication")
|
||||
if release_ready:
|
||||
atomic_session_pointer(
|
||||
config.session_root,
|
||||
"latest_thumb_passed" if standalone_thumb else "latest_passed",
|
||||
directory,
|
||||
)
|
||||
return summary, release_ready
|
||||
@@ -0,0 +1,893 @@
|
||||
"""Chinese, operator-facing diagnostics for three-camera calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"IMPORTING_BASE": "正在读取基础标定会话",
|
||||
"REVALIDATING_INHERITED": "正在复核继承的四指数据",
|
||||
"RETURN_BASELINE": "正在恢复目标姿态",
|
||||
"PREPARE_SWEEP": "正在到达扫描起点",
|
||||
"SWEEP": "正在采集轨迹",
|
||||
"FITTING": "正在拟合轨迹和零位",
|
||||
"VALIDATION_MOVE": "正在移动到随机复测位置",
|
||||
"VALIDATION_CAPTURE": "正在采集随机复测数据",
|
||||
"PAUSED": "标定已暂停",
|
||||
"ABORTED": "标定已终止",
|
||||
"COMPLETE": "标定已完成",
|
||||
}
|
||||
|
||||
VIEW_NAMES_ZH = {
|
||||
"front": "正面",
|
||||
"side": "侧面",
|
||||
"top": "上面",
|
||||
}
|
||||
|
||||
JOINT_NAMES_ZH = {
|
||||
"thumb_cmc_pitch": "拇指CMC俯仰",
|
||||
"thumb_cmc_roll": "拇指CMC滚转",
|
||||
"thumb_mcp": "拇指MCP",
|
||||
"thumb_ip": "拇指IP(被动)",
|
||||
"index_mcp_roll": "食指MCP侧摆",
|
||||
"index_mcp_pitch": "食指MCP屈伸",
|
||||
"index_pip": "食指PIP",
|
||||
"index_dip": "食指DIP(被动)",
|
||||
"middle_mcp_roll": "中指MCP侧摆",
|
||||
"middle_mcp_pitch": "中指MCP屈伸",
|
||||
"middle_pip": "中指PIP",
|
||||
"middle_dip": "中指DIP(被动)",
|
||||
"ring_mcp_roll": "无名指MCP侧摆",
|
||||
"ring_mcp_pitch": "无名指MCP屈伸",
|
||||
"ring_pip": "无名指PIP",
|
||||
"ring_dip": "无名指DIP(被动)",
|
||||
"pinky_mcp_roll": "小指MCP侧摆",
|
||||
"pinky_mcp_pitch": "小指MCP屈伸",
|
||||
"pinky_pip": "小指PIP",
|
||||
"pinky_dip": "小指DIP(被动)",
|
||||
"thumb_cmc_yaw": "拇指CMC侧摆",
|
||||
"index_mcp_roll_side": "食指MCP侧摆(侧面校验)",
|
||||
"middle_mcp_roll_side": "中指MCP侧摆(侧面校验)",
|
||||
"ring_mcp_roll_side": "无名指MCP侧摆(侧面校验)",
|
||||
"pinky_mcp_roll_side": "小指MCP侧摆(侧面校验)",
|
||||
}
|
||||
|
||||
|
||||
def _format_u8(value: Any) -> str:
|
||||
if value is None:
|
||||
return "尚无反馈"
|
||||
return f"{float(value):.1f}"
|
||||
|
||||
|
||||
def _task_text(active: Mapping[str, Any]) -> str:
|
||||
if not active:
|
||||
return "尚无活动任务"
|
||||
view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
|
||||
if active.get("kind") == "fit_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位,{joint_text}拟合检查失败,"
|
||||
f"电机{active.get('motor_index')},"
|
||||
f"第{active.get('attempt', 1)}次尝试"
|
||||
)
|
||||
if active.get("kind") == "zero_model_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位,{joint_text}零位/URDF验证失败,"
|
||||
f"电机{active.get('motor_index')},不会自动重扫"
|
||||
)
|
||||
if active.get("kind") == "motion_stall":
|
||||
return (
|
||||
f"电机{active.get('motor_index', '?')}运动停滞,目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}"
|
||||
)
|
||||
if active.get("kind") == "cross_view_roll_diagnostic":
|
||||
return (
|
||||
f"{active.get('finger', '?')}侧摆跨机位诊断完成:"
|
||||
f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°,"
|
||||
f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°"
|
||||
)
|
||||
if active.get("kind") == "validation":
|
||||
return (
|
||||
f"{view}机位,随机复测,电机{active.get('motor_index')},"
|
||||
f"目标命令{active.get('command_u8')}"
|
||||
)
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
start = active.get("start_u8")
|
||||
target = active.get("target_u8")
|
||||
cycle = active.get("cycle", "?")
|
||||
repetitions = active.get("repetitions", "?")
|
||||
task = (
|
||||
f"{view}机位,{joint_text},电机{active.get('motor_index')},"
|
||||
f"第{cycle}/{repetitions}轮,{start}→{target}"
|
||||
)
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
if fit_attempt > 1:
|
||||
retry_cycles = active.get("fit_retry_cycles", [])
|
||||
if retry_cycles:
|
||||
task += "(补采异常轮" + "/".join(
|
||||
str(cycle) for cycle in retry_cycles
|
||||
) + ")"
|
||||
else:
|
||||
task += f"(拟合补采第{fit_attempt}次)"
|
||||
return task
|
||||
|
||||
|
||||
def three_camera_reason_zh(
|
||||
state: str,
|
||||
reason: str,
|
||||
active: Mapping[str, Any],
|
||||
) -> tuple[str, str]:
|
||||
"""Translate a reason code and provide one concrete operator action."""
|
||||
reason = str(reason)
|
||||
sample = active.get("sample", {}) if active else {}
|
||||
missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
|
||||
sample_range = (
|
||||
f"{_format_u8(sample.get('minimum_u8'))}~"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
tolerance = sample.get("endpoint_tolerance_u8", "?")
|
||||
|
||||
if reason.startswith("motor_state_stalled:"):
|
||||
fields = reason.split(":")
|
||||
context = fields[1] if len(fields) > 1 else "unknown"
|
||||
error_match = re.search(r"error_u8=([0-9.]+)", reason)
|
||||
error = error_match.group(1) if error_match else "未知"
|
||||
timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason)
|
||||
timeout_value = active.get("timeout_seconds")
|
||||
if timeout_value is None and timeout_match is not None:
|
||||
timeout_value = float(timeout_match.group(1))
|
||||
duration = (
|
||||
f"连续{float(timeout_value):g}秒"
|
||||
if timeout_value is not None
|
||||
else "在规定时间内"
|
||||
)
|
||||
motor = active.get("motor_index")
|
||||
if motor is not None:
|
||||
return (
|
||||
f"电机{motor}反馈{duration}没有向目标推进;目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8,"
|
||||
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
|
||||
f"(阶段={context})。程序已保持当前位置。",
|
||||
"若实际反馈是稳定的固件端点,应只配置该电机该端点的专用容差后"
|
||||
"重启;若仍在变化或有摩擦,则先排查机械问题,不要反复resume强推。",
|
||||
)
|
||||
return (
|
||||
f"电机反馈{duration}没有向目标推进;停止位置距目标{error}个u8"
|
||||
f"(阶段={context})。程序已保持当前位置,防止机械碰撞或摩擦加重。",
|
||||
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
|
||||
"正常端点,应配置该电机专用端点容差后重启标定;不要反复调用resume强推。",
|
||||
)
|
||||
|
||||
base_reason, separator, reason_detail = reason.partition(":")
|
||||
if base_reason in {
|
||||
"sweep_missing_endpoint_bin",
|
||||
"sweep_bins_too_few",
|
||||
"sweep_bin_gap_too_large",
|
||||
"task_precheck_missing_command_127",
|
||||
"task_precheck_detection_rate_too_low",
|
||||
"synchronised_tag_state_timeout",
|
||||
}:
|
||||
reason = base_reason
|
||||
detail_label = (
|
||||
JOINT_NAMES_ZH.get(reason_detail, reason_detail)
|
||||
if separator and reason_detail
|
||||
else ""
|
||||
)
|
||||
detail_prefix = f"{detail_label}:" if detail_label else ""
|
||||
|
||||
if "URDF zero offset reached the configured" in reason:
|
||||
bound_match = re.search(
|
||||
r"configured\s+([0-9.]+)\s+degree bound", reason
|
||||
)
|
||||
bound = bound_match.group(1) if bound_match else "配置的"
|
||||
hit_text = ""
|
||||
if "bound:" in reason:
|
||||
hit_text = reason.split("bound:", 1)[1].split(
|
||||
"; all_offsets:", 1
|
||||
)[0]
|
||||
for name, label in JOINT_NAMES_ZH.items():
|
||||
hit_text = hit_text.replace(name, label)
|
||||
hit_suffix = f";触边关节:{hit_text}" if hit_text else ""
|
||||
return (
|
||||
f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
|
||||
"零位结果,而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
|
||||
"不要调用resume,也不要增大零位边界。先确认Tag有效黑框边长、三相机"
|
||||
"内外参和原始CAD URDF;Tag尺寸修正后必须调用start重新采集,旧尺度"
|
||||
"产生的轨迹不能直接生成修正URDF。",
|
||||
)
|
||||
|
||||
if reason == "sweep_missing_endpoint_bin":
|
||||
missing_text = "、".join(str(value) for value in missing) or "0或255"
|
||||
return (
|
||||
f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据,但缺少"
|
||||
f"电机端点{missing_text}附近的有效分箱;采样到的实际电机范围为"
|
||||
f"{sample_range},端点容差为±{tolerance}。这通常表示电机虽然运动到"
|
||||
"端点,但该时刻没有同时取得有效Tag图像和电机状态。",
|
||||
"确认当前机位所需Tag在整个行程(尤其缺失端点)均可见,然后调用"
|
||||
"/g20_calibration/resume;程序会重新扫描当前方向,不要调用start。",
|
||||
)
|
||||
if reason == "sweep_bins_too_few":
|
||||
return (
|
||||
f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个,要求至少"
|
||||
f"{sample.get('minimum_bin_count', '?')}个;当前采样范围{sample_range}。",
|
||||
"检查Tag连续识别和电机状态频率,修正后调用resume重新扫描当前方向。",
|
||||
)
|
||||
if reason == "sweep_bin_gap_too_large":
|
||||
gap_start = sample.get("maximum_bin_gap_start_u8")
|
||||
gap_end = sample.get("maximum_bin_gap_end_u8")
|
||||
gap_range = (
|
||||
""
|
||||
if gap_start is None or gap_end is None
|
||||
else f"({gap_start}→{gap_end})"
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')},"
|
||||
f"{gap_range}允许值不超过"
|
||||
f"{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
"检查运动中Tag是否间歇丢失;修正遮挡、反光或对焦后调用resume。",
|
||||
)
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get(
|
||||
"group_pnp_rejection_counts", {}
|
||||
)
|
||||
missing_roles = active.get(
|
||||
"group_missing_candidate_roles", {}
|
||||
)
|
||||
candidate_diagnostics = active.get(
|
||||
"pnp_candidate_diagnostics", {}
|
||||
)
|
||||
view_details: list[str] = []
|
||||
for view, value in group_reasons.items():
|
||||
view_name = str(view)
|
||||
parts = [str(value)]
|
||||
missing = (
|
||||
missing_roles.get(view_name, ())
|
||||
if isinstance(missing_roles, Mapping)
|
||||
else ()
|
||||
)
|
||||
if isinstance(missing, Sequence) and not isinstance(
|
||||
missing, (str, bytes)
|
||||
) and missing:
|
||||
parts.append(
|
||||
"缺候选=" + ",".join(str(role) for role in missing)
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view_name)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = counts.get(str(name), 0) + int(
|
||||
count
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
view_candidates = (
|
||||
candidate_diagnostics.get(view_name, {})
|
||||
if isinstance(candidate_diagnostics, Mapping)
|
||||
else {}
|
||||
)
|
||||
if isinstance(view_candidates, Mapping) and missing:
|
||||
summaries: list[str] = []
|
||||
for role in missing:
|
||||
diagnostic = view_candidates.get(str(role), {})
|
||||
if not isinstance(diagnostic, Mapping):
|
||||
continue
|
||||
summaries.append(
|
||||
f"{role}(solve="
|
||||
f"{int(diagnostic.get('solved_candidate_count', 0))},"
|
||||
"reproj="
|
||||
f"{int(diagnostic.get('reprojection_candidate_count', 0))},"
|
||||
"tilt="
|
||||
f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})"
|
||||
)
|
||||
if summaries:
|
||||
parts.append("候选统计=" + ",".join(summaries))
|
||||
view_details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view_name, view_name)}="
|
||||
+ ";".join(parts)
|
||||
)
|
||||
reason_text = "、".join(view_details)
|
||||
return (
|
||||
f"{detail_prefix}已经取得部分有效轨迹,但Tag仍可见且反馈正常时,"
|
||||
"后续连续图像帧"
|
||||
"被整组PnP几何检查拒绝"
|
||||
f"({reason_text}),因此无法与电机状态形成有效轨迹帧。",
|
||||
"不要调整或反复粘贴Tag;保留当前会话中的"
|
||||
"group_pnp_candidate_event,"
|
||||
"按缺失角色的候选统计检查PnP分支逻辑。",
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
"按时间戳配对”的图像帧。",
|
||||
"查看下面活动机位的缺失Tag,确认状态话题仍在更新;修正后调用resume,"
|
||||
"程序会重扫当前方向。",
|
||||
)
|
||||
if reason == "task_precheck_missing_command_127":
|
||||
return (
|
||||
f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。",
|
||||
"检查中位姿态的Tag遮挡和反光;程序只会重扫当前物理任务。",
|
||||
)
|
||||
if reason == "task_precheck_detection_rate_too_low":
|
||||
return (
|
||||
f"{detail_prefix}低速预检的有效Tag识别率低于门限。",
|
||||
"检查该机位当前任务Tag的遮挡、反光和对焦;程序只会重扫当前物理任务。",
|
||||
)
|
||||
if reason == "sweep_start_position_timeout":
|
||||
return (
|
||||
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
|
||||
f"{active.get('start_u8')},当前实际值{_format_u8(active.get('actual_u8'))}。",
|
||||
"检查CAN、机械手使能和是否存在机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "sweep_start_tag_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
progress_by_view = active.get("pnp_initialization_progress", {})
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get("group_pnp_rejection_counts", {})
|
||||
if any(
|
||||
isinstance(value, Mapping) and bool(value)
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
)
|
||||
):
|
||||
details: list[str] = []
|
||||
views = set()
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
):
|
||||
if isinstance(value, Mapping):
|
||||
views.update(str(view) for view in value)
|
||||
for view in sorted(views):
|
||||
parts: list[str] = []
|
||||
progress = (
|
||||
progress_by_view.get(view)
|
||||
if isinstance(progress_by_view, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(progress, Mapping):
|
||||
parts.append(
|
||||
"初始化"
|
||||
f"{int(progress.get('accepted', 0))}/"
|
||||
f"{int(progress.get('required', 0))}"
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = (
|
||||
counts.get(str(name), 0) + int(count)
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
latest = (
|
||||
group_reasons.get(view)
|
||||
if isinstance(group_reasons, Mapping)
|
||||
else None
|
||||
)
|
||||
if latest and not str(latest).startswith(
|
||||
"group_initializing:"
|
||||
):
|
||||
parts.append(f"最后状态={latest}")
|
||||
if parts:
|
||||
details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view, view)}=" + ";".join(parts)
|
||||
)
|
||||
reason_text = "、".join(details) or "未形成完整初始化窗口"
|
||||
return (
|
||||
"被测电机已经到达扫描起点,所需Tag也可见,但三维PnP位姿初始化"
|
||||
f"没有完成({reason_text}),因此没有生成同步端点帧。",
|
||||
"不要根据可见性重复粘贴Tag;保留累计拒绝原因并检查PnP候选选择。",
|
||||
)
|
||||
return (
|
||||
"被测电机已经到达扫描起点,但当前任务所需的实时运动Tag没有形成足够的"
|
||||
"同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”,不会触发此错误。",
|
||||
"只检查标记为✗的实时运动Tag、反光和外部遮挡;不要移动相机或手掌底座。",
|
||||
)
|
||||
if reason == "sweep_timeout":
|
||||
return (
|
||||
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
|
||||
"检查电机实际值、Tag连续识别和标定速度,修正后调用resume。",
|
||||
)
|
||||
if reason == "return_baseline_timeout":
|
||||
return (
|
||||
"一个或多个标定电机未在规定时间返回基准命令。",
|
||||
"检查机械手状态、CAN和机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_move_timeout":
|
||||
return (
|
||||
"随机复测时电机未在规定时间到达目标命令。",
|
||||
"检查机械手状态和机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_capture_timeout":
|
||||
return (
|
||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||
"检查当前机位Tag可见性后调用resume。",
|
||||
)
|
||||
if reason == "palm_orientation_quality_failed":
|
||||
failures = active.get("failures", [])
|
||||
detail = (
|
||||
str(failures[0].get("reason", "方向观测不足"))
|
||||
if failures
|
||||
else "方向观测不足"
|
||||
)
|
||||
if "thumb_cmc_" in detail:
|
||||
return (
|
||||
"拇指CMC yaw无法由顶部Tag 8/9的零位邻近短轨迹稳定确定:"
|
||||
+ detail,
|
||||
"保持Tag安装不变;确保顶部Tag 8/9在拇指CMC pitch和roll"
|
||||
"从零位开始的前1/4行程持续可见后重新标定。",
|
||||
)
|
||||
return (
|
||||
"掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定:"
|
||||
+ detail,
|
||||
"保持Tag安装不变;让正面Tag 10–13在对应MCP-pitch起始段"
|
||||
"至少可见15°行程后重新标定。",
|
||||
)
|
||||
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
|
||||
metric_names = {
|
||||
"plane_rms_mm": "平面拟合RMS",
|
||||
"radial_rms_mm": "圆半径拟合RMS",
|
||||
"radius_mm": "拟合半径",
|
||||
"image_radial_rms_px": "二维圆半径拟合RMS",
|
||||
"image_radial_p95_px": "二维圆半径误差P95",
|
||||
"image_radius_px": "二维拟合半径",
|
||||
"arc_deg": "实测圆弧",
|
||||
"monotonic_correction_deg": "最大单调修正",
|
||||
"hysteresis_deg": "最大正反程差",
|
||||
"baseline_hysteresis_deg": "baseline正反程关节角差",
|
||||
"baseline_directional_gap_deg": "baseline方向分支间隙",
|
||||
"baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差",
|
||||
"cycle_travel_range_deg": "三轮行程差",
|
||||
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
|
||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||
"axis_radial_rms_mm": "三维圆半径RMS",
|
||||
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
|
||||
"axis_line_cycle_rms_mm": "四轮轴线位置RMS",
|
||||
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
||||
"axis_cycle_difference_deg": "各轮转轴方向极差",
|
||||
"cross_view_roll_curve": "正面/侧面关节角曲线差异RMS",
|
||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||
"zero_cycle_offset_range_deg": "训练轮零位极差",
|
||||
"zero_confidence_95_half_width_deg": "零位95%置信半宽",
|
||||
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
|
||||
"tag_valid_rate_percent": "所需Tag同时有效率",
|
||||
}
|
||||
metric_units = {
|
||||
"plane_rms_mm": "mm",
|
||||
"radial_rms_mm": "mm",
|
||||
"radius_mm": "mm",
|
||||
"image_radial_rms_px": "px",
|
||||
"image_radial_p95_px": "px",
|
||||
"image_radius_px": "px",
|
||||
"arc_deg": "°",
|
||||
"monotonic_correction_deg": "°",
|
||||
"hysteresis_deg": "°",
|
||||
"baseline_hysteresis_deg": "°",
|
||||
"baseline_directional_gap_deg": "°",
|
||||
"baseline_directional_gap_range_deg": "°",
|
||||
"cycle_travel_range_deg": "°",
|
||||
"rotation_orthogonal_rms_deg": "°",
|
||||
"axis_plane_rms_mm": "mm",
|
||||
"axis_radial_rms_mm": "mm",
|
||||
"axis_pose_line_rms_mm": "mm",
|
||||
"axis_line_cycle_rms_mm": "mm",
|
||||
"rotation_circle_axis_difference_deg": "°",
|
||||
"axis_cycle_difference_deg": "°",
|
||||
"third_cycle_axis_holdout_deg": "°",
|
||||
"third_cycle_axis_line_rms_mm": "mm",
|
||||
"third_cycle_trajectory_p95_deg": "°",
|
||||
"zero_cycle_offset_range_deg": "°",
|
||||
"zero_confidence_95_half_width_deg": "°",
|
||||
"state_image_sync_p95_ms": "ms",
|
||||
"tag_valid_rate_percent": "%",
|
||||
"cross_view_roll_curve": "°",
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
metric = str(failure.get("metric", ""))
|
||||
if metric in metric_names:
|
||||
comparison = str(failure.get("comparison", ""))
|
||||
requirement = "不超过" if comparison == "maximum" else "至少"
|
||||
unit = metric_units[metric]
|
||||
detail = (
|
||||
f"{joint}的{metric_names[metric]}为"
|
||||
f"{float(failure.get('actual', 0.0)):.2f}{unit},"
|
||||
f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
|
||||
)
|
||||
cycle_travel = failure.get("cycle_travel_deg", [])
|
||||
if cycle_travel:
|
||||
detail += "(各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_travel
|
||||
) + ")"
|
||||
cycle_values = failure.get("cycle_values_deg", [])
|
||||
if not cycle_values:
|
||||
cycle_values = failure.get("cycle_offset_deg", [])
|
||||
if cycle_values:
|
||||
detail += "(各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_values
|
||||
) + ")"
|
||||
details.append(detail)
|
||||
else:
|
||||
cycle = failure.get("cycle")
|
||||
cycle_text = "" if cycle is None else f"第{cycle}轮"
|
||||
details.append(
|
||||
f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败:"
|
||||
f"{failure.get('reason', '未知原因')}"
|
||||
)
|
||||
detail_text = ";".join(details) or "当前关节的轨迹拟合未通过"
|
||||
directional_gap_failure = any(
|
||||
str(failure.get("metric", "")).startswith(
|
||||
"baseline_directional_gap"
|
||||
)
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
if reason == "joint_fit_systematic_failure":
|
||||
cross_view_systematic = any(
|
||||
failure.get("classification")
|
||||
in {
|
||||
"stable_cross_view_installation_or_model_bias",
|
||||
"stable_cross_view_direction_conflict",
|
||||
}
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
suggestion = (
|
||||
"四轮都出现稳定的正面/侧面差异,属于Tag安装外参或跨视角模型偏差,"
|
||||
"继续重扫不会改善;检查Tag刚性安装与跨视角安装变换,不要放宽门限。"
|
||||
if cross_view_systematic
|
||||
else "各轮重复出现同一模型冲突,继续运动不会改善;程序已禁止自动重扫。"
|
||||
"请直接复制诊断块给开发者,不要放宽门限。"
|
||||
)
|
||||
else:
|
||||
source_task_names = set(active.get("source_task_names", []))
|
||||
thumb_yaw_source_retry = source_task_names == {
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
}
|
||||
suggestion = (
|
||||
"方向分支已由软件保留,不要放宽门限;请检查传动回差或高支架刚度,"
|
||||
"处理后重新执行一键标定命令,程序会从最近可靠断点继续。"
|
||||
if directional_gap_failure
|
||||
else (
|
||||
"保持顶部Tag 8/9无遮挡;程序只替换决定yaw零位的"
|
||||
"CMC pitch/roll顶部轴观测并重扫"
|
||||
f"{active.get('directions_to_rescan', 16)}个方向,"
|
||||
"不会无效重扫yaw侧摆。"
|
||||
if thumb_yaw_source_retry
|
||||
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令;"
|
||||
"程序只清除当前失败关节的数据并重扫"
|
||||
f"{active.get('directions_to_rescan', 6)}个方向,"
|
||||
"不需要手工调用ROS服务。"
|
||||
)
|
||||
)
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
suggestion,
|
||||
)
|
||||
if reason == "zero_model_validation_failed":
|
||||
reason_names = {
|
||||
"zero_offset_reached_configured_bound": "零位解触及安全边界",
|
||||
"zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
|
||||
"zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
|
||||
"zero_offset_cycle_difference_too_large": "三轮零位离散过大",
|
||||
"zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
|
||||
"zero_axis_cone_mismatch_too_large": (
|
||||
"父子轴夹角与原始URDF不一致,零位旋转无法解释"
|
||||
),
|
||||
"zero_phase_axis_line_residual_too_large": (
|
||||
"整段SE(3)运动无法稳定确定平行轴线相位"
|
||||
),
|
||||
"zero_offset_did_not_improve_with_95pct_confidence": (
|
||||
"第三轮留出验证未以95%置信度改善"
|
||||
),
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
if failure.get("metric") == "zero_guard":
|
||||
reason_text = reason_names.get(
|
||||
str(failure.get("reason")), str(failure.get("reason"))
|
||||
)
|
||||
if "actual_deg" in failure and "limit_deg" in failure:
|
||||
reason_text += (
|
||||
f"(估计{float(failure['actual_deg']):+.2f}°,"
|
||||
f"允许±{float(failure['limit_deg']):.2f}°)"
|
||||
)
|
||||
details.append(f"{joint}:{reason_text}")
|
||||
return (
|
||||
"轨迹采集已完成,但零位/URDF几何验证失败"
|
||||
+ ("(" + ";".join(details) + ")" if details else "")
|
||||
+ "。程序没有生成正式JSON或修正URDF。",
|
||||
"该类稳定模型失败不能靠重复运动修复,程序不会自动重扫;"
|
||||
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
|
||||
)
|
||||
if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪;此阶段不以Tag可见性阻止基准恢复。",
|
||||
"保持机械手运动范围无障碍;设备就绪后系统会先安全恢复基准形态,再检查掌部Tag。",
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查;全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "waiting_for_baseline_tags_after_recovery":
|
||||
return (
|
||||
"机械手已经稳定恢复到基准形态,正在用新采集的画面确认三台相机各自的固定掌部Tag。",
|
||||
"若某个掌部Tag持续缺失,只调整遮挡手指或检查Tag固定情况,不要移动相机和手掌底座。",
|
||||
)
|
||||
if reason == "locking_fixed_base_references":
|
||||
return (
|
||||
"基准形态Tag预检已通过,正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
|
||||
"无需操作;锁定完成后允许任务姿态遮挡固定掌部Tag。",
|
||||
)
|
||||
if reason == "fixed_base_reference_moved":
|
||||
return (
|
||||
"顶部Tag 8在本会话基准锁定后连续多帧发生角点位移;程序已立即保持机械手当前位置,"
|
||||
"本会话中已采集数据不再用于发布。",
|
||||
"Tag 8允许在下一次标定预检前重新摆放,但本次不能继续;固定Tag 8和顶部相机后"
|
||||
"重新启动新会话。",
|
||||
)
|
||||
if reason == "waiting_for_task_tags_at_sweep_start":
|
||||
return (
|
||||
"电机已到扫描起点,正在等待当前任务的实时运动Tag;显示为“锁”的固定掌部Tag"
|
||||
"允许被手指遮挡。",
|
||||
"只检查标记为✗的实时运动Tag;若均为✓或锁,程序会自动开始运动。",
|
||||
)
|
||||
if reason == "call_start_for_baseline_recovery":
|
||||
return (
|
||||
"相机数据和机械手反馈已就绪,等待一键程序触发安全基准恢复。",
|
||||
"保持机械手运动范围无障碍;程序会自动开始,无需手工调用ROS服务。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
return (
|
||||
"三机位预检已经通过,等待操作员确认开始。",
|
||||
"清空机械手运动范围后调用/g20_calibration/start。",
|
||||
)
|
||||
if reason == "operator_pause":
|
||||
return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
|
||||
if reason == "operator_abort":
|
||||
return "操作员终止了本次标定,程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
|
||||
if reason == "collecting_timestamp_synchronised_tag_centres":
|
||||
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作,保持相机、标签和底座不动。"
|
||||
if reason == "collecting_dedicated_baseline_hold":
|
||||
return (
|
||||
"正在从当前方向到达关节baseline并静止采集Tag与电机反馈;这批数据单独用于回差验收。",
|
||||
"无需操作,保持相机、标签和底座不动。",
|
||||
)
|
||||
if reason == "steady checkpoint target is missing":
|
||||
return (
|
||||
"首轮稳态检查点已经到达最终端点,但采集状态没有及时切换到端点完成阶段。",
|
||||
"程序已停止发布并保留已采样数据;这是软件状态切换问题,不需要调整相机、Tag或机械手。",
|
||||
)
|
||||
if reason == "cross_view_roll_front_failure_deferred":
|
||||
return (
|
||||
"正面侧摆回差不合格已保留,诊断模式将继续采集同一手指的侧面数据。",
|
||||
"无需操作;该诊断会锁定URDF发布。",
|
||||
)
|
||||
if reason == "cross_view_roll_diagnostic_complete":
|
||||
interpretation = str(active.get("interpretation", ""))
|
||||
explanations = {
|
||||
"both_views_confirm_direction_dependent_pose": (
|
||||
"正面和侧面都确认了方向相关姿态,优先判断为roll输出机构或共同下游链的真实回差。"
|
||||
),
|
||||
"front_only_difference_check_roll_tag_bracket_or_front_pnp": (
|
||||
"只有正面差异超限,优先检查roll Tag高支架刚度和正面PnP。"
|
||||
),
|
||||
"side_only_difference_check_side_tag_chain_or_side_pnp": (
|
||||
"只有侧面差异超限,优先检查侧面Tag链和侧面PnP。"
|
||||
),
|
||||
"both_views_within_formal_hysteresis_limit": (
|
||||
"两个机位的静止回差均满足正式门限。"
|
||||
),
|
||||
}
|
||||
return (
|
||||
explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。")
|
||||
+ " 本次为诊断会话,不会生成或发布URDF。",
|
||||
"保存当前状态和raw_samples.jsonl;根据两机位结论处理后重新启动正式标定。",
|
||||
)
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作,保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成,但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason.startswith("validated_endpoint_zero_state"):
|
||||
return (
|
||||
"轨迹和URDF零位验证已经通过,但发布前检测到端点零位状态缺失或与"
|
||||
"已验证模型不一致;这是程序内部状态生命周期错误,结果未发布。",
|
||||
"不要移动相机、Tag或机械手底座;保留当前会话并把原因码交给开发者。",
|
||||
)
|
||||
if reason.startswith("PUB-ARTIFACT-601:"):
|
||||
return (
|
||||
"标定节点已经生成通过质量门限的JSON和候选URDF,但一键程序在正式发布前"
|
||||
"发现这对产物的坐标、限位、哈希或资源一致性检查失败;原始URDF未被覆盖。",
|
||||
"不要重新标定相机或调整Tag;保留本会话产物和启动日志供开发者检查发布契约。",
|
||||
)
|
||||
if reason == "combination_pose_prediction_failed":
|
||||
return (
|
||||
"单关节、零位和URDF几何验证已通过,但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
|
||||
"程序会在原姿态重新初始化PnP并自动复测;若最终仍失败,请把raw_samples.jsonl中的"
|
||||
"combination_validation_failure记录交给开发者,不要重新采集16个单关节任务。",
|
||||
)
|
||||
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
|
||||
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
|
||||
if reason == "holding_same_finger_clearance_before_next_task":
|
||||
return (
|
||||
"同一根手指的上一项已经完成;相邻手指继续保持当前避让姿态,只调整"
|
||||
"被测关节以衔接下一项。",
|
||||
"无需操作,不要手动展开正在避让的手指。",
|
||||
)
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成,正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
|
||||
return f"未分类原因码:{reason}", "保留该原因码和启动终端日志用于进一步定位。"
|
||||
|
||||
|
||||
def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
"""Render the complete operator status; the JSON topic remains unchanged."""
|
||||
state = str(payload.get("state", ""))
|
||||
active = payload.get("active", {})
|
||||
reason_zh, action_zh = three_camera_reason_zh(
|
||||
state, str(payload.get("reason", "")), active
|
||||
)
|
||||
progress = float(payload.get("progress", 0.0))
|
||||
completed = payload.get("completed_sweeps", 0)
|
||||
total = payload.get("total_sweeps", 0)
|
||||
scan_progress = float(
|
||||
payload.get(
|
||||
"scan_progress",
|
||||
0.0 if not total else float(completed) / float(total),
|
||||
)
|
||||
)
|
||||
lines = [
|
||||
f"状态:{STATE_NAMES_ZH.get(state, state)}({state})",
|
||||
f"原因:{reason_zh}",
|
||||
f"建议:{action_zh}",
|
||||
f"总体进度:{progress:.1%}(计划扫描{completed}/{total}个方向,"
|
||||
f"扫描进度{scan_progress:.1%})",
|
||||
f"当前任务:{_task_text(active)}",
|
||||
]
|
||||
if state == "RETURN_BASELINE":
|
||||
baseline_command = payload.get("baseline_command_u8", [])
|
||||
return_command = payload.get("return_command_u8", baseline_command)
|
||||
label = "恢复姿态" if return_command != baseline_command else "基准姿态"
|
||||
lines.append(f"正在确认{label}:{return_command}")
|
||||
if active and active.get("kind") not in {
|
||||
"fit_failure",
|
||||
"zero_model_failure",
|
||||
"motion_stall",
|
||||
}:
|
||||
retry_count = int(active.get("automatic_retry_count", 0))
|
||||
if retry_count:
|
||||
lines.append(
|
||||
"自动重试:当前方向已自动重扫"
|
||||
f"{retry_count}/{active.get('automatic_retry_limit', '?')}次,"
|
||||
f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%},"
|
||||
f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
|
||||
)
|
||||
sample = active.get("sample", {})
|
||||
motion_progress = active.get("motion_progress")
|
||||
motion_text = (
|
||||
"未知" if motion_progress is None else f"{float(motion_progress):.1%}"
|
||||
)
|
||||
lines.append(
|
||||
"运动采样:"
|
||||
f"目标{active.get('target_u8', active.get('command_u8', '?'))},"
|
||||
f"实际{_format_u8(active.get('actual_u8'))},"
|
||||
f"本方向{motion_text},有效帧{active.get('valid_frames', 0)},"
|
||||
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}~"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
detection_frames = int(active.get("detection_frames", 0))
|
||||
if detection_frames:
|
||||
lines.append(
|
||||
"本方向Tag检出:"
|
||||
f"{float(active.get('detection_rate', 0.0)):.1%}"
|
||||
f"({active.get('detection_valid_frames', 0)}/"
|
||||
f"{detection_frames}帧)"
|
||||
)
|
||||
auxiliary = active.get("auxiliary_motors", [])
|
||||
if auxiliary:
|
||||
lines.append(
|
||||
"避挡姿态:"
|
||||
+ ",".join(
|
||||
f"电机{item.get('motor_index')}目标"
|
||||
f"{item.get('command_u8')}、实际"
|
||||
f"{_format_u8(item.get('actual_u8'))}"
|
||||
for item in auxiliary
|
||||
)
|
||||
)
|
||||
speed = active.get("speed", {})
|
||||
if speed:
|
||||
lines.append(
|
||||
"阶段速度:五指目标"
|
||||
f"{speed.get('commanded_finger_speed')},SDK报告"
|
||||
f"{speed.get('reported_finger_speed')}"
|
||||
)
|
||||
if active.get("sweep_timeout_seconds") is not None:
|
||||
lines.append(
|
||||
"运动保护:扫描超时"
|
||||
f"{float(active['sweep_timeout_seconds']):.1f}s,"
|
||||
"连续"
|
||||
f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
|
||||
"进展不足"
|
||||
f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
|
||||
"则立即暂停"
|
||||
)
|
||||
lines.append("机位:")
|
||||
for name, view in payload.get("views", {}).items():
|
||||
missing = view.get("missing_tag_ids", [])
|
||||
missing_text = "无" if not missing else ",".join(map(str, missing))
|
||||
lines.append(
|
||||
f"- {VIEW_NAMES_ZH.get(str(name), str(name))}:"
|
||||
f"{'就绪' if view.get('ready') else '等待'},"
|
||||
f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'},"
|
||||
f"{float(view.get('detection_hz', 0.0)):.1f}Hz,"
|
||||
f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%},"
|
||||
f"当前缺失Tag={missing_text}"
|
||||
)
|
||||
extrinsics_error = payload.get("camera_extrinsics_error")
|
||||
if extrinsics_error:
|
||||
lines.append(f"外参文件:{extrinsics_error}")
|
||||
lines.append(f"JSON结果:{payload.get('result_path') or '尚未生成'}")
|
||||
lines.append(
|
||||
f"修正URDF:{payload.get('corrected_urdf_path') or '尚未生成'}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Independent reviewed profile for the right 19-Tag product layout."""
|
||||
|
||||
from ...core import ProfileKey
|
||||
from .profile import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile
|
||||
from .zero_policy import (
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
||||
get_zero_calibration_profile,
|
||||
)
|
||||
from ..registry import RegisteredProfile
|
||||
from ._adapter import adapt_profile
|
||||
|
||||
|
||||
KEY = ProfileKey("G20", "right", G20_RIGHT_19_LAYOUT, 1)
|
||||
|
||||
|
||||
def build_profile() -> RegisteredProfile:
|
||||
hand = get_hand_calibration_profile(KEY.side, KEY.layout)
|
||||
zero = get_zero_calibration_profile(KEY.side, KEY.layout)
|
||||
return adapt_profile(
|
||||
key=KEY,
|
||||
namespace="/g20_calibration",
|
||||
hand_profile=hand,
|
||||
zero_profile=zero,
|
||||
mechanical_endpoint_joints=RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
post_solve_endpoint_joints=RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
||||
)
|
||||
@@ -0,0 +1,731 @@
|
||||
"""One-command runner for a registered hand-calibration product."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from datetime import datetime
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import traceback
|
||||
from typing import Any, Mapping
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from ...hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from ...operator_report import (
|
||||
ProgressEstimator,
|
||||
build_failure_report,
|
||||
render_progress_zh,
|
||||
)
|
||||
from ...product import ProductConfig, load_product_config, sha256_file
|
||||
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||
from ...storage import atomic_write_json
|
||||
|
||||
|
||||
EXIT_PASS = 0
|
||||
EXIT_QUALITY = 2
|
||||
EXIT_SAFETY = 3
|
||||
STATUS_TIMEOUT_SECONDS = 90.0
|
||||
FITTING_STATUS_TIMEOUT_SECONDS = 600.0
|
||||
|
||||
|
||||
def _status_timeout_seconds(status: Mapping[str, Any]) -> float:
|
||||
"""Return the watchdog deadline for the node's current phase.
|
||||
|
||||
Motion and acquisition are expected to publish twice a second and retain
|
||||
the strict transport watchdog. The final 3-D fit is intentionally a
|
||||
synchronous, CPU-bound operation, so its executor cannot service the
|
||||
status timer until the fit returns. The node publishes an explicit
|
||||
FITTING status immediately before entering that operation; allow that
|
||||
known phase enough time without weakening motion safety checks.
|
||||
"""
|
||||
if str(status.get("state", "")) == "FITTING":
|
||||
return FITTING_STATUS_TIMEOUT_SECONDS
|
||||
return STATUS_TIMEOUT_SECONDS
|
||||
|
||||
|
||||
def _calibration_node_exited_before_status(log_path: Path) -> bool:
|
||||
"""Detect a launch child crash while the parent launch is still alive."""
|
||||
try:
|
||||
with log_path.open("rb") as stream:
|
||||
stream.seek(0, os.SEEK_END)
|
||||
size = stream.tell()
|
||||
stream.seek(max(0, size - 65536), os.SEEK_SET)
|
||||
tail = stream.read().decode("utf-8", errors="replace")
|
||||
except OSError:
|
||||
return False
|
||||
return (
|
||||
"[three_camera_calibration_node-" in tail
|
||||
and "]: process has died" in tail
|
||||
)
|
||||
|
||||
|
||||
class CalibrationMonitor(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibration_product_runner")
|
||||
self.latest_status: dict[str, Any] = {}
|
||||
self.last_status_at = time.monotonic()
|
||||
self.start_requested = False
|
||||
self.start_future: Any = None
|
||||
self.abort_future: Any = None
|
||||
self.create_subscription(String, "/g20_calibration/status", self._status, 10)
|
||||
self.start_client = self.create_client(Trigger, "/g20_calibration/start")
|
||||
self.abort_client = self.create_client(Trigger, "/g20_calibration/abort")
|
||||
|
||||
def _status(self, message: String) -> None:
|
||||
try:
|
||||
payload = json.loads(message.data)
|
||||
except (TypeError, json.JSONDecodeError):
|
||||
return
|
||||
if isinstance(payload, dict):
|
||||
self.latest_status = payload
|
||||
self.last_status_at = time.monotonic()
|
||||
|
||||
def maybe_start(self) -> None:
|
||||
if self.start_requested or self.latest_status.get("state") != "WAIT_START":
|
||||
return
|
||||
if not self.start_client.service_is_ready():
|
||||
self.start_client.wait_for_service(timeout_sec=0.05)
|
||||
return
|
||||
self.start_requested = True
|
||||
self.start_future = self.start_client.call_async(Trigger.Request())
|
||||
|
||||
def abort(self) -> None:
|
||||
if not self.abort_client.service_is_ready():
|
||||
self.abort_client.wait_for_service(timeout_sec=1.0)
|
||||
if self.abort_client.service_is_ready():
|
||||
self.abort_future = self.abort_client.call_async(Trigger.Request())
|
||||
|
||||
|
||||
class ProgressConsole:
|
||||
def __init__(self, serial_number: str) -> None:
|
||||
self.serial_number = serial_number
|
||||
self.estimator = ProgressEstimator.start()
|
||||
self.last_text = ""
|
||||
self.last_issue = ""
|
||||
|
||||
def update(self, status: Mapping[str, Any]) -> None:
|
||||
text = render_progress_zh(self.serial_number, 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)
|
||||
reason = str(status.get("reason", ""))
|
||||
if reason.startswith("automatic_retry_") and reason != self.last_issue:
|
||||
self.last_issue = reason
|
||||
active = status.get("active", {})
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"⚠ 当前任务出现问题:{reason.removeprefix('automatic_retry_')}",
|
||||
f"系统处理:只重扫当前任务(第 {active.get('automatic_retry_count', 1)}/2 次)",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
|
||||
|
||||
def _default_product_config() -> Path:
|
||||
try:
|
||||
installed = Path(
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
) / "config" / "g20_right_product.yaml"
|
||||
if installed.is_file():
|
||||
return installed
|
||||
except Exception:
|
||||
pass
|
||||
return (
|
||||
Path.cwd()
|
||||
/ "src/linkerhand_calibration/config/g20_right_product.yaml"
|
||||
).resolve()
|
||||
|
||||
|
||||
def _launch_command(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
recalibration_scope: str = "full",
|
||||
) -> list[str]:
|
||||
values = {
|
||||
"model": config.model,
|
||||
"hand_type": config.side,
|
||||
"tag_layout": config.tag_layout,
|
||||
"serial_number": config.serial_number,
|
||||
"can_interface": config.can_interface,
|
||||
"session_dir": str(session),
|
||||
"output_root": str(config.output_root),
|
||||
"camera_extrinsics_file": str(config.camera_extrinsics),
|
||||
"source_urdf_path": str(config.source_urdf),
|
||||
"source_urdf_expected_sha256": config.source_urdf_sha256,
|
||||
"corrected_urdf_output_dir": str(session),
|
||||
"calibration_config": str(config.calibration_config),
|
||||
"tag_config": str(config.tag_config),
|
||||
"commands_enabled": "true",
|
||||
"start_cameras": "true",
|
||||
"start_sdk": "true",
|
||||
"record_bag": "false",
|
||||
"validation_enabled": "false",
|
||||
"recalibration_scope": recalibration_scope,
|
||||
}
|
||||
if resume_from is not None:
|
||||
values["resume_raw_samples_path"] = str(
|
||||
resume_from / "raw_samples.jsonl"
|
||||
)
|
||||
for view, camera in config.cameras.items():
|
||||
values[f"{view}_camera_serial"] = camera["serial_number"]
|
||||
values[f"{view}_camera_name"] = camera["camera_name"]
|
||||
values[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 values.items()),
|
||||
]
|
||||
|
||||
|
||||
def _stop_stack(process: subprocess.Popen[Any]) -> None:
|
||||
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 _write_trace(log_path: Path, error: BaseException) -> None:
|
||||
with log_path.open("a", encoding="utf-8") as stream:
|
||||
stream.write("\n[one-command exception]\n")
|
||||
traceback.print_exception(type(error), error, error.__traceback__, file=stream)
|
||||
|
||||
|
||||
def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None:
|
||||
monitor.abort()
|
||||
deadline = time.monotonic() + float(timeout_seconds)
|
||||
while time.monotonic() < deadline and rclpy.ok():
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status.get("state") == "ABORTED":
|
||||
return
|
||||
|
||||
|
||||
def _run_hardware_session(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
recalibration_scope: str = "full",
|
||||
) -> tuple[dict[str, Any], int]:
|
||||
session.mkdir(parents=True, exist_ok=False)
|
||||
(session / "raw_samples.jsonl").touch()
|
||||
log_path = session / "calibration.log"
|
||||
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
|
||||
atomic_session_pointer(config.session_root, "latest_attempt", session)
|
||||
monitor = CalibrationMonitor()
|
||||
console = ProgressConsole(config.serial_number)
|
||||
process: subprocess.Popen[Any] | None = None
|
||||
latest_status: dict[str, Any] = {
|
||||
"state": "PREFLIGHT",
|
||||
"reason": "starting_ros_stack",
|
||||
"progress": 0.0,
|
||||
"views": {},
|
||||
"feedback_hz": 0.0,
|
||||
}
|
||||
exit_code = EXIT_QUALITY
|
||||
try:
|
||||
process = subprocess.Popen(
|
||||
_launch_command(
|
||||
config,
|
||||
session,
|
||||
resume_from=resume_from,
|
||||
recalibration_scope=recalibration_scope,
|
||||
),
|
||||
cwd=config.workspace,
|
||||
stdout=log_stream,
|
||||
stderr=subprocess.STDOUT,
|
||||
text=True,
|
||||
start_new_session=True,
|
||||
)
|
||||
launched_at = time.monotonic()
|
||||
last_render = 0.0
|
||||
last_startup_log_check = 0.0
|
||||
while True:
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status:
|
||||
latest_status = monitor.latest_status
|
||||
monitor.maybe_start()
|
||||
now = time.monotonic()
|
||||
if now - last_render >= 0.5:
|
||||
console.update(latest_status)
|
||||
last_render = now
|
||||
if monitor.start_future is not None and monitor.start_future.done():
|
||||
response = monitor.start_future.result()
|
||||
if response is None or not response.success:
|
||||
message = "start service failed" if response is None else response.message
|
||||
raise RuntimeError(f"CFG-START-008:{message}")
|
||||
monitor.start_future = None
|
||||
state = str(latest_status.get("state", ""))
|
||||
if state == "COMPLETE":
|
||||
exit_code = EXIT_PASS
|
||||
break
|
||||
if state in {"PAUSED", "ABORTED"}:
|
||||
reason = str(latest_status.get("reason", "calibration_paused"))
|
||||
exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY
|
||||
if state == "PAUSED" and "stall" not in reason:
|
||||
# Ordinary quality failures return to the reviewed baseline
|
||||
# before the process tree is stopped. Mechanical stalls
|
||||
# deliberately skip this path and keep the current pose.
|
||||
failure_status = dict(latest_status)
|
||||
_request_safe_abort(monitor)
|
||||
latest_status = failure_status
|
||||
break
|
||||
if process.poll() is not None:
|
||||
raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}")
|
||||
if (
|
||||
not monitor.latest_status
|
||||
and now - last_startup_log_check >= 0.5
|
||||
):
|
||||
last_startup_log_check = now
|
||||
log_stream.flush()
|
||||
if _calibration_node_exited_before_status(log_path):
|
||||
raise RuntimeError(
|
||||
"CAM-STATUS-202:calibration node exited before status"
|
||||
)
|
||||
if (
|
||||
not monitor.latest_status
|
||||
and now - launched_at > STATUS_TIMEOUT_SECONDS
|
||||
):
|
||||
raise RuntimeError("CAM-STATUS-202:no calibration status received")
|
||||
if (
|
||||
monitor.latest_status
|
||||
and now - monitor.last_status_at
|
||||
> _status_timeout_seconds(monitor.latest_status)
|
||||
):
|
||||
raise RuntimeError("MOTION-COMM-303:calibration status stopped")
|
||||
except KeyboardInterrupt as error:
|
||||
latest_status["state"] = "ABORTED"
|
||||
latest_status["reason"] = "operator_abort"
|
||||
_request_safe_abort(monitor)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_SAFETY
|
||||
except BaseException as error:
|
||||
latest_status["state"] = "PAUSED"
|
||||
latest_status["reason"] = str(error)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_QUALITY
|
||||
finally:
|
||||
if process is not None:
|
||||
_stop_stack(process)
|
||||
monitor.destroy_node()
|
||||
log_stream.flush()
|
||||
os.fsync(log_stream.fileno())
|
||||
log_stream.close()
|
||||
|
||||
if exit_code != EXIT_PASS:
|
||||
_, block = build_failure_report(
|
||||
config,
|
||||
session,
|
||||
latest_status,
|
||||
reason=str(latest_status.get("reason", "unknown_failure")),
|
||||
)
|
||||
print(block, flush=True)
|
||||
return latest_status, exit_code
|
||||
|
||||
|
||||
def _startup_failure_block(path: Path, error: BaseException) -> str:
|
||||
return "\n".join(
|
||||
[
|
||||
"========== 请复制以下内容给开发者 ==========",
|
||||
"结果:FAIL",
|
||||
"错误代码:CFG-PRODUCT-001",
|
||||
"失败阶段:启动静态预检",
|
||||
f"问题:{error}",
|
||||
f"产品配置:{path}",
|
||||
"自动处理:未启动相机、SDK或机械手运动",
|
||||
"建议:复制本诊断块给开发者,不要手工修改哈希绕过检查。",
|
||||
"========== 复制结束 ==========",
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
||||
"""Return the newest compatible failed attempt, never a passed session.
|
||||
|
||||
Do not trust only ``latest_attempt``. A process interrupted during the
|
||||
device-only startup gate may have already moved that pointer while still
|
||||
containing no ``session_start`` checkpoint. In that case walk backwards
|
||||
to the preceding usable failed session instead of throwing away hours of
|
||||
completed tasks.
|
||||
"""
|
||||
root = config.session_root
|
||||
try:
|
||||
resolved_root = root.resolve(strict=True)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
candidates: list[Path] = []
|
||||
pointer = config.session_root / "latest_attempt"
|
||||
if pointer.exists():
|
||||
try:
|
||||
candidates.append(pointer.resolve(strict=True))
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
candidates.extend(
|
||||
sorted(
|
||||
(
|
||||
path
|
||||
for path in root.iterdir()
|
||||
if path.is_dir() and not path.name.startswith("latest_")
|
||||
),
|
||||
key=lambda path: path.name,
|
||||
reverse=True,
|
||||
)
|
||||
)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
passed_pointer = config.session_root / "latest_passed"
|
||||
passed: Path | None = None
|
||||
if passed_pointer.exists():
|
||||
try:
|
||||
passed = passed_pointer.resolve(strict=True)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
seen: set[Path] = set()
|
||||
for unresolved in candidates:
|
||||
try:
|
||||
candidate = unresolved.resolve(strict=True)
|
||||
except OSError:
|
||||
continue
|
||||
if candidate in seen:
|
||||
continue
|
||||
seen.add(candidate)
|
||||
if candidate.parent != resolved_root or not candidate.is_dir():
|
||||
continue
|
||||
# A failed attempt older than the current formal release is stale and
|
||||
# must not seed a new independent calibration.
|
||||
if passed is not None and candidate.name <= passed.name:
|
||||
continue
|
||||
raw_path = candidate / "raw_samples.jsonl"
|
||||
if not raw_path.is_file():
|
||||
continue
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
summary: dict[str, Any] | None = None
|
||||
if summary_path.is_file():
|
||||
try:
|
||||
loaded = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
|
||||
continue
|
||||
summary = loaded
|
||||
hashes = summary.get("hashes", {})
|
||||
if not isinstance(hashes, Mapping):
|
||||
continue
|
||||
if (
|
||||
str(hashes.get("source_urdf_sha256", ""))
|
||||
!= config.source_urdf_sha256
|
||||
or str(hashes.get("camera_extrinsics_sha256", ""))
|
||||
!= config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
start: dict[str, Any] | None = None
|
||||
try:
|
||||
with raw_path.open("r", encoding="utf-8") as stream:
|
||||
for line in stream:
|
||||
if not line.strip():
|
||||
continue
|
||||
value = json.loads(line)
|
||||
if (
|
||||
isinstance(value, dict)
|
||||
and value.get("kind") == "session_start"
|
||||
):
|
||||
start = value
|
||||
break
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if (
|
||||
start is None
|
||||
or start.get("hand_type") != config.side
|
||||
or start.get("tag_layout") != config.tag_layout
|
||||
or start.get("source_urdf_sha256")
|
||||
!= config.source_urdf_sha256
|
||||
):
|
||||
continue
|
||||
if summary is None:
|
||||
# Ctrl+C can terminate the ROS launch tree before the wrapper gets
|
||||
# a chance to create calibration_summary_zh.json. The immutable
|
||||
# checkpoint itself is enough to resume only after independently
|
||||
# proving that its external geometry still matches the product.
|
||||
try:
|
||||
checkpoint_extrinsics = Path(
|
||||
str(start["camera_extrinsics_file"])
|
||||
).expanduser().resolve(strict=True)
|
||||
if sha256_file(checkpoint_extrinsics) != (
|
||||
config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
except (KeyError, OSError, ValueError):
|
||||
continue
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
def _resolve_partial_base_session(
|
||||
config: ProductConfig, base_session: str | Path | None
|
||||
) -> Path:
|
||||
"""Validate the complete passed session that donates non-target tasks."""
|
||||
if base_session is None or not str(base_session).strip():
|
||||
raise ValueError(
|
||||
"partial scope requires --base-session pointing to a passed "
|
||||
"complete G20 right session"
|
||||
)
|
||||
candidate = Path(base_session).expanduser().resolve(strict=True)
|
||||
root = config.session_root.resolve()
|
||||
if candidate.parent != root or not candidate.is_dir():
|
||||
raise ValueError(
|
||||
"base session must resolve to a direct session directory under "
|
||||
f"{root}"
|
||||
)
|
||||
raw_path = candidate / "raw_samples.jsonl"
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
payload_path = (
|
||||
candidate
|
||||
/ f"g20_right_{config.serial_number}_calibration.json"
|
||||
)
|
||||
for required in (raw_path, summary_path, payload_path):
|
||||
if not required.is_file():
|
||||
raise ValueError(f"base session is missing required artifact: {required}")
|
||||
try:
|
||||
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except json.JSONDecodeError as error:
|
||||
raise ValueError("base session summary is invalid JSON") from error
|
||||
if (
|
||||
not isinstance(summary, dict)
|
||||
or summary.get("result") != "PASS"
|
||||
or not bool(summary.get("quality", {}).get("passed"))
|
||||
):
|
||||
raise ValueError("base session is not a formally passed session")
|
||||
hashes = summary.get("hashes", {})
|
||||
expected_hashes = {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
}
|
||||
if not isinstance(hashes, Mapping) or any(
|
||||
str(hashes.get(name, "")) != expected
|
||||
for name, expected in expected_hashes.items()
|
||||
):
|
||||
raise ValueError(
|
||||
"base session source URDF, camera extrinsics or calibration "
|
||||
"configuration differs from the current product"
|
||||
)
|
||||
return candidate
|
||||
|
||||
|
||||
def run(
|
||||
config_path: str | Path,
|
||||
*,
|
||||
workspace: str | Path | None = None,
|
||||
preflight_only: bool = False,
|
||||
allow_resume: bool = True,
|
||||
scope: str = "full",
|
||||
base_session: str | Path | None = None,
|
||||
) -> int:
|
||||
path = Path(config_path).expanduser().resolve()
|
||||
try:
|
||||
# Resolve every file and camera identity before allowing a hardware
|
||||
# process to start. A second load enables the real CAN existence gate.
|
||||
config = load_product_config(path, workspace=workspace, check_can=False)
|
||||
load_product_config(path, workspace=workspace, check_can=True)
|
||||
selected_scope = str(scope).strip().lower()
|
||||
if selected_scope not in {"full", "thumb", "fingers"}:
|
||||
raise ValueError("scope must be one of: full, thumb, fingers")
|
||||
if selected_scope == "full" and base_session is not None:
|
||||
raise ValueError(
|
||||
"--base-session is valid only with --scope thumb/fingers"
|
||||
)
|
||||
partial_base = None
|
||||
if selected_scope == "fingers" or base_session is not None:
|
||||
partial_base = _resolve_partial_base_session(config, base_session)
|
||||
except BaseException as error:
|
||||
print(_startup_failure_block(path, error), flush=True)
|
||||
return EXIT_QUALITY
|
||||
if preflight_only:
|
||||
print("PASS:产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。")
|
||||
return EXIT_PASS
|
||||
|
||||
config.session_root.mkdir(parents=True, exist_ok=True)
|
||||
resume_candidate = (
|
||||
partial_base
|
||||
if selected_scope != "full"
|
||||
else (_automatic_resume_candidate(config) if allow_resume else None)
|
||||
)
|
||||
if resume_candidate is not None:
|
||||
if selected_scope == "thumb":
|
||||
print(
|
||||
"拇指专项标定:四指任务继承自已通过会话 "
|
||||
f"{resume_candidate.name};4项拇指任务将全部重新采集,"
|
||||
"四指零位保持不变。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "fingers":
|
||||
print(
|
||||
"四指专项标定:拇指任务和4个拇指零位继承自已通过会话 "
|
||||
f"{resume_candidate.name};12项四指任务将全部重新采集。",
|
||||
flush=True,
|
||||
)
|
||||
else:
|
||||
print(
|
||||
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
|
||||
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "thumb":
|
||||
print(
|
||||
"独立拇指标定:不导入四指会话;仅采集4项拇指任务,"
|
||||
"四指URDF零位保持原始CAD值。",
|
||||
flush=True,
|
||||
)
|
||||
maximum_sessions = config.required_independent_passes
|
||||
for pass_index in range(maximum_sessions):
|
||||
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
|
||||
if maximum_sessions > 1:
|
||||
print(f"正式标定复验:第 {pass_index + 1}/{maximum_sessions} 次", flush=True)
|
||||
status, code = _run_hardware_session(
|
||||
config,
|
||||
session,
|
||||
resume_from=(
|
||||
resume_candidate
|
||||
if selected_scope != "full" or pass_index == 0
|
||||
else None
|
||||
),
|
||||
recalibration_scope=selected_scope,
|
||||
)
|
||||
if code != EXIT_PASS:
|
||||
return code
|
||||
# Keep the exact node-side completion contract durable before the
|
||||
# independent publication layer starts. If publication itself fails,
|
||||
# developers can re-run artifact checks without repeating motion or
|
||||
# inventing lost combination-validation metrics.
|
||||
atomic_write_json(session / "node_status.json", status)
|
||||
try:
|
||||
summary, release_ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
except BaseException as error:
|
||||
_write_trace(session / "calibration.log", error)
|
||||
status = dict(status)
|
||||
status["state"] = "PAUSED"
|
||||
status["reason"] = f"PUB-ARTIFACT-601:{error}"
|
||||
_, block = build_failure_report(config, session, status, reason=status["reason"])
|
||||
print(block, flush=True)
|
||||
return EXIT_QUALITY
|
||||
if release_ready:
|
||||
result_pointer = (
|
||||
config.session_root / "latest_thumb_passed"
|
||||
if selected_scope == "thumb" and partial_base is None
|
||||
else config.session_root / "latest_passed"
|
||||
)
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"PASS:{config.model} {config.side} 标定、URDF修正和复验全部通过。",
|
||||
f"正式结果:{result_pointer}",
|
||||
f"JSON:{session / summary['artifacts']['json']}",
|
||||
f"URDF:{summary['artifacts']['urdf']}",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
return EXIT_PASS
|
||||
print("本次会话质量PASS;正在自动执行第二次独立完整复验。", flush=True)
|
||||
return EXIT_QUALITY
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定")
|
||||
parser.add_argument("--config", default=str(_default_product_config()))
|
||||
parser.add_argument("--workspace", default=None)
|
||||
parser.add_argument("--preflight-only", action="store_true")
|
||||
parser.add_argument(
|
||||
"--scope",
|
||||
choices=("full", "thumb", "fingers"),
|
||||
default="full",
|
||||
help=(
|
||||
"full重新标定全手;thumb仅重采4项拇指任务;"
|
||||
"fingers复用已认证拇指并仅重采12项四指任务"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--base-session",
|
||||
default=None,
|
||||
help=(
|
||||
"可选:thumb模式将结果合并到该完整会话;"
|
||||
"fingers模式必须提供该基础会话"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-resume",
|
||||
action="store_true",
|
||||
help="忽略失败会话,从第一个关节开始全新采集",
|
||||
)
|
||||
arguments = parser.parse_args(args)
|
||||
configure_fastdds_large_image_transport()
|
||||
ros_log_dir = Path(
|
||||
os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs")
|
||||
)
|
||||
ros_log_dir.mkdir(parents=True, exist_ok=True)
|
||||
rclpy.init()
|
||||
try:
|
||||
code = run(
|
||||
arguments.config,
|
||||
workspace=arguments.workspace,
|
||||
preflight_only=arguments.preflight_only,
|
||||
allow_resume=not arguments.no_resume,
|
||||
scope=arguments.scope,
|
||||
base_session=arguments.base_session,
|
||||
)
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user