大拇指零位正确性稳定性修改
This commit is contained in:
@@ -8,6 +8,26 @@
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
|
||||
```
|
||||
|
||||
只重新采集大拇指的4项任务、四指沿用一份已经正式通过的完整会话时,使用:
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
--scope thumb \
|
||||
--base-session calibration_output/G20_RIGHT_001/latest_passed
|
||||
```
|
||||
|
||||
采样文件中的运动域是显式且不可混用的:
|
||||
`requested_command_u8` 表示下发命令,`feedback_u8` 表示电机反馈。
|
||||
在线拟合和离线重放通过同一个数据契约投影到曲线索引;新会话不会把含糊的
|
||||
`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
|
||||
`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`,完成复核后才允许机械手运动。
|
||||
|
||||
`--base-session`必须解析到同一序列号目录下的完整PASS会话;启动前会校验源CAD
|
||||
URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF,不在旧校准
|
||||
URDF上叠加。该模式只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务,其余任务的原始记录导入后仍按
|
||||
当前硬门限重新验证。
|
||||
|
||||
开发阶段若上一次会话失败,同一命令会自动校验硬件/几何哈希,并恢复已经
|
||||
完整提交的关节任务;失败中的当前任务始终丢弃重做,位于它后面但已经完整通过的
|
||||
独立任务仍会复用,不再因“连续前缀”限制整段重采。导入的任务会立即用与
|
||||
|
||||
@@ -24,7 +24,7 @@ artifacts:
|
||||
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
|
||||
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: 59ab7510ad0a2912ca636876039427c4c25906cbbfa84e96300d46b86e929334
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: 8fa16814d7a3462a00aae4eadc273e66bc167bedd5908b0fed40ce6d505b2aeb
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||
|
||||
@@ -119,6 +119,8 @@ class HandCalibrationProfile:
|
||||
axis_validation_sources: Mapping[str, str] | None = None
|
||||
palm_axis_observers: tuple[PalmAxisObserver, ...] = ()
|
||||
minimum_palm_orientation_sources: int = 0
|
||||
palm_orientation_minimum_arc_rad: float = math.radians(15.0)
|
||||
palm_orientation_maximum_command_distance_u8: int = 255
|
||||
# Hardware and algorithm differences belong to the product profile, not
|
||||
# to the acquisition state machine. Keeping these fields here lets a new
|
||||
# model (or the mirrored hand) reuse the same recorder/fitter by supplying
|
||||
@@ -662,8 +664,32 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
||||
layout_id=G20_RIGHT_19_LAYOUT,
|
||||
measurement_specs=measurement_specs,
|
||||
axis_validation_sources=axis_validation_sources,
|
||||
palm_axis_observers=(),
|
||||
minimum_palm_orientation_sources=0,
|
||||
# Observe the zero-adjacent roll and pitch axes from Tag 9 while the
|
||||
# existing front sweeps run. Their same-view angle identifies the
|
||||
# intervening yaw zero without adding a motion task or reference URDF.
|
||||
palm_axis_observers=(
|
||||
PalmAxisObserver(
|
||||
source_name="thumb_cmc_pitch_top_axis",
|
||||
task_name="thumb_cmc_pitch_front",
|
||||
view="top",
|
||||
parent_role="top_base",
|
||||
child_role="thumb_yaw",
|
||||
model_joint="thumb_cmc_pitch",
|
||||
motor_index=joint_specs["thumb_cmc_pitch"].motor_index,
|
||||
),
|
||||
PalmAxisObserver(
|
||||
source_name="thumb_cmc_roll_top_axis",
|
||||
task_name="thumb_cmc_roll_front",
|
||||
view="top",
|
||||
parent_role="top_base",
|
||||
child_role="thumb_yaw",
|
||||
model_joint="thumb_cmc_roll",
|
||||
motor_index=joint_specs["thumb_cmc_roll"].motor_index,
|
||||
),
|
||||
),
|
||||
minimum_palm_orientation_sources=2,
|
||||
palm_orientation_minimum_arc_rad=math.radians(10.0),
|
||||
palm_orientation_maximum_command_distance_u8=64,
|
||||
capabilities=frozenset(
|
||||
{
|
||||
"precheck_sweeps",
|
||||
@@ -674,6 +700,7 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
||||
"stable_cross_view_cone_bias",
|
||||
"measured_passive_dips",
|
||||
"urdf_zero_publication",
|
||||
"palm_axis_side_channel_v2",
|
||||
}
|
||||
),
|
||||
)
|
||||
|
||||
+65
-38
@@ -37,9 +37,11 @@ from .full_hand import (
|
||||
validate_compact_payload,
|
||||
)
|
||||
from .product import get_product_calibration_contract
|
||||
from .sample_schema import fitting_sample_record, fitting_sample_records
|
||||
from .storage import atomic_write_json
|
||||
from .urdf_zero import (
|
||||
JointAxisMeasurement,
|
||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
anchor_right_19_mechanical_endpoint_curves,
|
||||
@@ -55,6 +57,8 @@ from .urdf_zero import (
|
||||
derive_right_19_mechanical_endpoint_offsets,
|
||||
joint_curve_holdout_errors,
|
||||
refit_axis_line_group_with_shared_radius,
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
||||
select_cross_view_roll_direction_source,
|
||||
solve_urdf_zero_offsets,
|
||||
with_depth_free_axis_projection,
|
||||
@@ -92,25 +96,12 @@ def _steady_records_in_feedback_domain(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Mirror the live mechanical-backlash domain for offline replay."""
|
||||
result: list[dict[str, Any]] = []
|
||||
for source in records:
|
||||
record = dict(source)
|
||||
requested = int(
|
||||
record.get("requested_command_u8", record.get("command_u8", -1))
|
||||
)
|
||||
record["command_u8"] = (
|
||||
requested
|
||||
if requested in {0, 255}
|
||||
else int(
|
||||
np.clip(
|
||||
np.rint(float(record.get("feedback_u8", requested))),
|
||||
0,
|
||||
255,
|
||||
)
|
||||
)
|
||||
)
|
||||
result.append(record)
|
||||
return result
|
||||
return fitting_sample_records(
|
||||
records,
|
||||
domain="feedback",
|
||||
allow_legacy_command=True,
|
||||
snap_requested_endpoints=True,
|
||||
)
|
||||
|
||||
|
||||
def _latest_attempt_records(
|
||||
@@ -195,19 +186,20 @@ def _load_raw_session(
|
||||
for line in raw_path.read_text(encoding="utf-8").splitlines()
|
||||
if line.strip()
|
||||
]
|
||||
# New sessions persist both domains explicitly. The fitting primitives
|
||||
# retain their historical command_u8 input name internally, so normalise
|
||||
# only after loading; raw_samples.jsonl itself never contains an ambiguous
|
||||
# command_u8 field.
|
||||
# Both online fitting and replay use the same canonical-to-fit projection.
|
||||
# raw_samples.jsonl itself never contains the ambiguous compatibility key.
|
||||
for row in rows:
|
||||
if "command_u8" in row:
|
||||
continue
|
||||
if row.get("kind") in {
|
||||
"sample", "baseline_hold_sample", "palm_axis_sample"
|
||||
"sample",
|
||||
"baseline_hold_sample",
|
||||
"steady_command_sample",
|
||||
"palm_axis_sample",
|
||||
}:
|
||||
row["command_u8"] = int(round(float(row["feedback_u8"])))
|
||||
elif row.get("kind") == "steady_command_sample":
|
||||
row["command_u8"] = int(row["requested_command_u8"])
|
||||
projected = fitting_sample_record(
|
||||
row, allow_legacy_command=True
|
||||
)
|
||||
row.clear()
|
||||
row.update(projected)
|
||||
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")
|
||||
@@ -1247,8 +1239,20 @@ def replay_session(
|
||||
)
|
||||
command_fits = anchor_right_19_mechanical_endpoint_curves(
|
||||
command_fits,
|
||||
command_records_by_joint,
|
||||
{
|
||||
name: (
|
||||
[
|
||||
record
|
||||
for record in curve_records_by_joint[name]
|
||||
if int(record["cycle"]) in training_cycle_set
|
||||
]
|
||||
if name == "thumb_cmc_roll"
|
||||
else command_records_by_joint[name]
|
||||
)
|
||||
for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS
|
||||
},
|
||||
maximum_direction_difference_rad=command_gap_limit,
|
||||
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
|
||||
)
|
||||
cross_view_roll_metrics: dict[str, dict[str, float]] = {}
|
||||
for name, validation_name in (
|
||||
@@ -1304,8 +1308,8 @@ def replay_session(
|
||||
minimum_sources=int(
|
||||
profile.minimum_palm_orientation_sources
|
||||
),
|
||||
minimum_arc_rad=math.radians(
|
||||
float(parameters["trajectory_minimum_arc_deg"])
|
||||
minimum_arc_rad=(
|
||||
profile.palm_orientation_minimum_arc_rad
|
||||
),
|
||||
maximum_rotation_orthogonal_rms_rad=math.radians(
|
||||
float(
|
||||
@@ -1314,6 +1318,9 @@ def replay_session(
|
||||
]
|
||||
)
|
||||
),
|
||||
maximum_command_distance_u8=(
|
||||
profile.palm_orientation_maximum_command_distance_u8
|
||||
),
|
||||
)
|
||||
)
|
||||
failures = _quality_failures(
|
||||
@@ -1435,7 +1442,7 @@ def replay_session(
|
||||
motor_by_joint = {
|
||||
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
|
||||
}
|
||||
endpoint_zero_offsets = (
|
||||
measured_endpoint_zero_offsets = (
|
||||
derive_right_19_mechanical_endpoint_offsets(
|
||||
source_urdf,
|
||||
command_fits,
|
||||
@@ -1450,6 +1457,16 @@ def replay_session(
|
||||
if layout_id == G20_RIGHT_19_LAYOUT
|
||||
else {}
|
||||
)
|
||||
endpoint_zero_offsets = {
|
||||
name: value
|
||||
for name, value in measured_endpoint_zero_offsets.items()
|
||||
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
}
|
||||
post_solve_endpoint_offsets = {
|
||||
name: value
|
||||
for name, value in measured_endpoint_zero_offsets.items()
|
||||
if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS
|
||||
}
|
||||
command_fits = clamp_runtime_fits_to_urdf_limits(
|
||||
source_urdf,
|
||||
command_fits,
|
||||
@@ -1515,6 +1532,10 @@ def replay_session(
|
||||
**zero_profile.fixed_direct_zero_offsets_rad,
|
||||
**endpoint_zero_offsets,
|
||||
},
|
||||
"static_output_zero_offsets_rad": {
|
||||
**zero_profile.static_output_zero_offsets_rad,
|
||||
**post_solve_endpoint_offsets,
|
||||
},
|
||||
}
|
||||
holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
|
||||
if not holdout_zero.passed:
|
||||
@@ -1611,12 +1632,15 @@ def replay_session(
|
||||
# differ slightly at settled checkpoints; using them here creates
|
||||
# a fictitious residual zero after a structurally correct write.
|
||||
curves=training_fits,
|
||||
static_output_zero_offsets_rad={
|
||||
name: 0.0
|
||||
for name in zero_profile.static_output_zero_offsets_rad
|
||||
},
|
||||
**{
|
||||
**solve_arguments,
|
||||
"static_output_zero_offsets_rad": {
|
||||
name: 0.0
|
||||
for name in (
|
||||
set(zero_profile.static_output_zero_offsets_rad)
|
||||
| set(post_solve_endpoint_offsets)
|
||||
)
|
||||
},
|
||||
"source_urdf": candidate,
|
||||
"fixed_direct_zero_offsets_rad": {
|
||||
name: 0.0
|
||||
@@ -1762,7 +1786,10 @@ def replay_session(
|
||||
},
|
||||
"static_output_zero_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in zero_profile.static_output_zero_offsets_rad.items()
|
||||
for name, value in {
|
||||
**zero_profile.static_output_zero_offsets_rad,
|
||||
**post_solve_endpoint_offsets,
|
||||
}.items()
|
||||
},
|
||||
"direct_offsets_deg": {
|
||||
name: math.degrees(value)
|
||||
|
||||
@@ -153,6 +153,7 @@ def _launch_command(
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
recalibration_scope: str = "full",
|
||||
) -> list[str]:
|
||||
values = {
|
||||
"model": config.model,
|
||||
@@ -173,6 +174,7 @@ def _launch_command(
|
||||
"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(
|
||||
@@ -235,6 +237,7 @@ def _run_hardware_session(
|
||||
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()
|
||||
@@ -254,7 +257,12 @@ def _run_hardware_session(
|
||||
exit_code = EXIT_QUALITY
|
||||
try:
|
||||
process = subprocess.Popen(
|
||||
_launch_command(config, session, resume_from=resume_from),
|
||||
_launch_command(
|
||||
config,
|
||||
session,
|
||||
resume_from=resume_from,
|
||||
recalibration_scope=recalibration_scope,
|
||||
),
|
||||
cwd=config.workspace,
|
||||
stdout=log_stream,
|
||||
stderr=subprocess.STDOUT,
|
||||
@@ -488,12 +496,66 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
||||
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(
|
||||
"--scope thumb 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:
|
||||
@@ -501,6 +563,16 @@ def run(
|
||||
# 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"}:
|
||||
raise ValueError("scope must be one of: full, thumb")
|
||||
if selected_scope == "full" and base_session is not None:
|
||||
raise ValueError("--base-session is valid only with --scope thumb")
|
||||
partial_base = (
|
||||
None
|
||||
if selected_scope == "full"
|
||||
else _resolve_partial_base_session(config, base_session)
|
||||
)
|
||||
except BaseException as error:
|
||||
print(_startup_failure_block(path, error), flush=True)
|
||||
return EXIT_QUALITY
|
||||
@@ -510,14 +582,23 @@ def run(
|
||||
|
||||
config.session_root.mkdir(parents=True, exist_ok=True)
|
||||
resume_candidate = (
|
||||
_automatic_resume_candidate(config) if allow_resume else None
|
||||
partial_base
|
||||
if selected_scope == "thumb"
|
||||
else (_automatic_resume_candidate(config) if allow_resume else None)
|
||||
)
|
||||
if resume_candidate is not None:
|
||||
print(
|
||||
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
|
||||
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
|
||||
flush=True,
|
||||
)
|
||||
if selected_scope == "thumb":
|
||||
print(
|
||||
"拇指专项标定:四指任务继承自已通过会话 "
|
||||
f"{resume_candidate.name};4项拇指任务将全部重新采集。",
|
||||
flush=True,
|
||||
)
|
||||
else:
|
||||
print(
|
||||
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
|
||||
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
|
||||
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")
|
||||
@@ -531,7 +612,12 @@ def run(
|
||||
status, code = _run_hardware_session(
|
||||
config,
|
||||
session,
|
||||
resume_from=(resume_candidate if pass_index == 0 else None),
|
||||
resume_from=(
|
||||
resume_candidate
|
||||
if selected_scope == "thumb" or pass_index == 0
|
||||
else None
|
||||
),
|
||||
recalibration_scope=selected_scope,
|
||||
)
|
||||
if code != EXIT_PASS:
|
||||
return code
|
||||
@@ -569,6 +655,17 @@ def main(args: list[str] | None = None) -> None:
|
||||
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"),
|
||||
default="full",
|
||||
help="full重新标定全手;thumb仅重采4项拇指任务",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--base-session",
|
||||
default=None,
|
||||
help="thumb模式继承四指数据的已通过完整会话目录",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-resume",
|
||||
action="store_true",
|
||||
@@ -587,6 +684,8 @@ def main(args: list[str] | None = None) -> None:
|
||||
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():
|
||||
|
||||
+18
-1
@@ -161,8 +161,18 @@ def render_progress_zh(
|
||||
+ "(正面+侧面同步)"
|
||||
)
|
||||
if not joint:
|
||||
import_progress = status.get("base_import", {})
|
||||
import_fraction = (
|
||||
float(import_progress.get("fraction", 0.0))
|
||||
if isinstance(import_progress, Mapping)
|
||||
else 0.0
|
||||
)
|
||||
joint = str(active.get("label_zh", "")) or (
|
||||
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
|
||||
f"导入基础会话 {100.0 * import_fraction:.1f}%"
|
||||
if state in {"IMPORTING_BASE", "REVALIDATING_INHERITED"}
|
||||
else (
|
||||
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
|
||||
)
|
||||
)
|
||||
direction = {
|
||||
"decreasing": "递减",
|
||||
@@ -337,6 +347,13 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
|
||||
)
|
||||
if value.startswith("CFG-"):
|
||||
return value, "产品配置或文件预检失败", "不要移动相机;复制本诊断块给开发者。"
|
||||
if value.startswith("DATA-CONTRACT-"):
|
||||
return (
|
||||
value.split(":", 1)[0],
|
||||
"标定样本的请求命令与反馈值数据契约不完整",
|
||||
"机械手无需重新采集;保留原始会话并用离线重放验证,"
|
||||
"复制契约错误给开发者。",
|
||||
)
|
||||
if value.startswith("CAM-STATUS-202"):
|
||||
return (
|
||||
"CAM-STATUS-202",
|
||||
|
||||
@@ -579,6 +579,26 @@ def finalize_session_artifacts(
|
||||
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"}:
|
||||
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 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"
|
||||
)
|
||||
offsets = active_offsets(payload)
|
||||
endpoint_offsets = {
|
||||
name: offsets[name]
|
||||
@@ -610,6 +630,13 @@ def finalize_session_artifacts(
|
||||
"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"
|
||||
else str(resume.get("source_session"))
|
||||
),
|
||||
"freshly_calibrated_task_keys": list(recalibration_tasks),
|
||||
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
|
||||
"quality": quality,
|
||||
"runtime_limit_clipped_bins": clipped_runtime_joints,
|
||||
|
||||
@@ -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
|
||||
+9
@@ -9,6 +9,8 @@ from typing import Any, Mapping, Sequence
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"IMPORTING_BASE": "正在读取基础标定会话",
|
||||
"REVALIDATING_INHERITED": "正在复核继承的四指数据",
|
||||
"RETURN_BASELINE": "正在恢复目标姿态",
|
||||
"PREPARE_SWEEP": "正在到达扫描起点",
|
||||
"SWEEP": "正在采集轨迹",
|
||||
@@ -457,6 +459,13 @@ def three_camera_reason_zh(
|
||||
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,
|
||||
|
||||
+258
-91
@@ -80,11 +80,20 @@ from .pnp import (
|
||||
SquareTagPoseTracker,
|
||||
)
|
||||
from .product import get_product_calibration_contract
|
||||
from .sample_schema import (
|
||||
SampleDataContractError,
|
||||
canonical_sample_record,
|
||||
fitting_sample_record,
|
||||
fitting_sample_records,
|
||||
validate_sample_records,
|
||||
)
|
||||
from .storage import append_jsonl, append_jsonl_many, atomic_write_json
|
||||
from .three_camera_diagnostics import render_three_camera_status_text_zh
|
||||
from .urdf_zero import (
|
||||
LEFT_ZERO_PROFILE,
|
||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
||||
JointAxisMeasurement,
|
||||
PalmOrientationMeasurement,
|
||||
UrdfKinematicModel,
|
||||
@@ -114,6 +123,8 @@ from .urdf_zero import (
|
||||
|
||||
STATE_PREFLIGHT = "PREFLIGHT"
|
||||
STATE_WAIT_START = "WAIT_START"
|
||||
STATE_IMPORTING_BASE = "IMPORTING_BASE"
|
||||
STATE_REVALIDATING_INHERITED = "REVALIDATING_INHERITED"
|
||||
STATE_RETURN_BASELINE = "RETURN_BASELINE"
|
||||
STATE_PREPARE_SWEEP = "PREPARE_SWEEP"
|
||||
STATE_SWEEP = "SWEEP"
|
||||
@@ -306,7 +317,7 @@ def _palm_axis_resume_policy(
|
||||
session_start: Mapping[str, Any],
|
||||
) -> tuple[bool, tuple[str, ...]]:
|
||||
"""Validate optional palm-axis checkpoint data when a model uses it."""
|
||||
capability = "palm_axis_side_channel_v1"
|
||||
capability = "palm_axis_side_channel_v2"
|
||||
previous = {str(value) for value in session_start.get("capabilities", [])}
|
||||
if not profile.palm_axis_observers and capability not in profile.capabilities:
|
||||
return True, ()
|
||||
@@ -319,9 +330,11 @@ def _palm_axis_resume_policy(
|
||||
):
|
||||
raise ValueError("resume palm-axis observer schema differs")
|
||||
return True, ()
|
||||
return False, tuple(
|
||||
observer.task_name for observer in profile.palm_axis_observers
|
||||
)
|
||||
# A new side-channel version changes the zero solver's observation model.
|
||||
# Do not combine its fresh CMC axes with any trajectory captured under the
|
||||
# previous PnP transaction semantics. This full invalidation happens only
|
||||
# once; same-version checkpoints keep their normal task-level resume.
|
||||
return False, tuple(spec.key for spec in profile.sweep_specs)
|
||||
|
||||
|
||||
def _node_profile(node: Any) -> HandCalibrationProfile:
|
||||
@@ -776,6 +789,39 @@ RESUMABLE_SAMPLE_KINDS = frozenset(
|
||||
{"sample", "baseline_hold_sample", "steady_command_sample"}
|
||||
)
|
||||
|
||||
RECALIBRATION_SCOPES = frozenset({"full", "thumb"})
|
||||
|
||||
|
||||
def recalibration_task_keys(
|
||||
profile: HandCalibrationProfile, scope: str
|
||||
) -> tuple[str, ...]:
|
||||
"""Return tasks that must be freshly acquired for a partial session."""
|
||||
selected = str(scope).strip().lower()
|
||||
if selected not in RECALIBRATION_SCOPES:
|
||||
raise ValueError(
|
||||
"recalibration_scope must be one of: "
|
||||
+ ", ".join(sorted(RECALIBRATION_SCOPES))
|
||||
)
|
||||
if selected == "full":
|
||||
return ()
|
||||
if (
|
||||
profile.side != "right"
|
||||
or profile.layout_id != G20_RIGHT_19_LAYOUT
|
||||
):
|
||||
raise ValueError(
|
||||
"thumb recalibration is supported only for the G20 right 19-Tag product"
|
||||
)
|
||||
tasks = tuple(
|
||||
spec.key
|
||||
for spec in profile.sweep_specs
|
||||
if any(str(name).startswith("thumb_") for name in spec.joints)
|
||||
)
|
||||
if len(tasks) != 4:
|
||||
raise ValueError(
|
||||
"G20 right thumb recalibration must resolve exactly four tasks"
|
||||
)
|
||||
return tasks
|
||||
|
||||
|
||||
def _normalise_legacy_split_roll_resume_rows(
|
||||
profile: HandCalibrationProfile,
|
||||
@@ -1140,6 +1186,13 @@ def _file_sha256(path: str | Path) -> str:
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _publish_import_status(node: Any) -> None:
|
||||
"""Publish synchronous import progress when running inside a ROS node."""
|
||||
publisher = getattr(node, "_publish_status", None)
|
||||
if callable(publisher):
|
||||
publisher(time.monotonic())
|
||||
|
||||
|
||||
def _steady_records_in_feedback_domain(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
) -> list[dict[str, Any]]:
|
||||
@@ -1151,25 +1204,12 @@ def _steady_records_in_feedback_domain(
|
||||
already passed their motor-specific reach gate, so they are snapped to
|
||||
the exact endpoints required by the curve fitter.
|
||||
"""
|
||||
result: list[dict[str, Any]] = []
|
||||
for source in records:
|
||||
record = dict(source)
|
||||
requested = int(
|
||||
record.get("requested_command_u8", record.get("command_u8", -1))
|
||||
)
|
||||
if requested in {0, 255}:
|
||||
feedback_index = requested
|
||||
else:
|
||||
feedback_index = int(
|
||||
np.clip(
|
||||
np.rint(float(record.get("feedback_u8", requested))),
|
||||
0,
|
||||
255,
|
||||
)
|
||||
)
|
||||
record["command_u8"] = feedback_index
|
||||
result.append(record)
|
||||
return result
|
||||
return fitting_sample_records(
|
||||
records,
|
||||
domain="feedback",
|
||||
allow_legacy_command=True,
|
||||
snap_requested_endpoints=True,
|
||||
)
|
||||
|
||||
|
||||
def _classify_cross_view_roll_hysteresis(
|
||||
@@ -1822,6 +1862,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
|
||||
self.state = STATE_PREFLIGHT
|
||||
self.reason = "waiting_for_devices_and_sdk"
|
||||
self.sample_schema_version = 1
|
||||
self.base_import_progress: dict[str, Any] = {}
|
||||
self.paused_reason = ""
|
||||
self.started = False
|
||||
# Startup has two deliberately separate gates. Camera/SDK health is
|
||||
@@ -1999,6 +2041,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.declare_parameter("serial_number", "UNSET")
|
||||
self.declare_parameter("session_dir", "calibration_output/session")
|
||||
self.declare_parameter("resume_raw_samples_path", "")
|
||||
self.declare_parameter("recalibration_scope", "full")
|
||||
self.declare_parameter("camera_extrinsics_file", "")
|
||||
self.declare_parameter("source_urdf_path", "")
|
||||
self.declare_parameter("source_urdf_expected_sha256", "")
|
||||
@@ -2206,6 +2249,20 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
if not resume_value
|
||||
else Path(resume_value).expanduser().resolve()
|
||||
)
|
||||
self.recalibration_scope = str(
|
||||
value("recalibration_scope")
|
||||
).strip().lower()
|
||||
self.recalibration_task_keys = recalibration_task_keys(
|
||||
self.profile, self.recalibration_scope
|
||||
)
|
||||
if (
|
||||
self.recalibration_scope != "full"
|
||||
and self.resume_raw_samples_path is None
|
||||
):
|
||||
raise ValueError(
|
||||
"partial recalibration requires resume_raw_samples_path from "
|
||||
"a passed complete session"
|
||||
)
|
||||
self.resumed_task_keys: tuple[str, ...] = ()
|
||||
self.resume_source_session = ""
|
||||
self.camera_extrinsics_file = Path(
|
||||
@@ -3951,7 +4008,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
* Rotation.from_matrix(child_matrix[:3, :3])
|
||||
)
|
||||
feedback = float(state_u8[observer.motor_index])
|
||||
record = {
|
||||
durable = canonical_sample_record({
|
||||
"kind": "palm_axis_sample",
|
||||
"attempt": self.sweep_attempts.get(
|
||||
_sweep_storage_key(item.spec), 1
|
||||
@@ -3963,7 +4020,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"motor_index": int(observer.motor_index),
|
||||
"cycle": int(item.cycle),
|
||||
"direction": item.direction,
|
||||
"command_u8": int(np.clip(np.rint(feedback), 0, 255)),
|
||||
"requested_command_u8": int(item.target_u8),
|
||||
"feedback_u8": feedback,
|
||||
"relative_quaternion_xyzw": [
|
||||
float(value) for value in relative_rotation.as_quat()
|
||||
],
|
||||
@@ -3981,7 +4039,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
6,
|
||||
),
|
||||
"image_stamp_ns": int(stamp_ns),
|
||||
}
|
||||
})
|
||||
record = fitting_sample_record(durable)
|
||||
self.palm_axis_records_by_source.setdefault(
|
||||
observer.source_name, []
|
||||
).append(record)
|
||||
@@ -4012,18 +4071,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
]
|
||||
append_jsonl_many(
|
||||
self.raw_path,
|
||||
(
|
||||
{
|
||||
**{
|
||||
key: value
|
||||
for key, value in record.items()
|
||||
if key != "command_u8"
|
||||
},
|
||||
"requested_command_u8": int(item.target_u8),
|
||||
"feedback_u8": int(record["command_u8"]),
|
||||
}
|
||||
for record in rows
|
||||
),
|
||||
(canonical_sample_record(record) for record in rows),
|
||||
)
|
||||
|
||||
def _accept_frame(self, observation: FrameObservation) -> None:
|
||||
@@ -4432,18 +4480,32 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
if source.resolve() == self.raw_path.resolve():
|
||||
raise RuntimeError("resume raw samples must come from an older session")
|
||||
rows: list[dict[str, Any]] = []
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
for line_number, line in enumerate(stream, start=1):
|
||||
if not line.strip():
|
||||
source_size = max(1, int(source.stat().st_size))
|
||||
bytes_read = 0
|
||||
with source.open("rb") as stream:
|
||||
for line_number, raw_line in enumerate(stream, start=1):
|
||||
bytes_read += len(raw_line)
|
||||
if not raw_line.strip():
|
||||
continue
|
||||
try:
|
||||
line = raw_line.decode("utf-8")
|
||||
value = json.loads(line)
|
||||
except json.JSONDecodeError as error:
|
||||
except (UnicodeDecodeError, json.JSONDecodeError) as error:
|
||||
raise RuntimeError(
|
||||
f"resume raw JSON is invalid at line {line_number}"
|
||||
) from error
|
||||
if isinstance(value, dict):
|
||||
rows.append(value)
|
||||
if line_number % 1000 == 0:
|
||||
self.base_import_progress = {
|
||||
"phase": "reading",
|
||||
"records_read": len(rows),
|
||||
"bytes_read": bytes_read,
|
||||
"total_bytes": source_size,
|
||||
"fraction": min(1.0, bytes_read / source_size),
|
||||
}
|
||||
self.reason = "reading_base_session_records"
|
||||
_publish_import_status(self)
|
||||
starts = [row for row in rows if row.get("kind") == "session_start"]
|
||||
if len(starts) != 1:
|
||||
raise RuntimeError("resume raw must contain exactly one session_start")
|
||||
@@ -4494,6 +4556,10 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
) from error
|
||||
invalidated_task_set = set(size_invalidated_tasks)
|
||||
invalidated_task_set.update(palm_axis_invalidated_tasks)
|
||||
scope_invalidated_tasks = tuple(
|
||||
getattr(self, "recalibration_task_keys", ()) or ()
|
||||
)
|
||||
invalidated_task_set.update(scope_invalidated_tasks)
|
||||
if invalidated_task_set:
|
||||
rows = [
|
||||
row
|
||||
@@ -4508,6 +4574,22 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
minimum_sweep_bins=self.minimum_sweep_bins,
|
||||
allow_sparse=True,
|
||||
)
|
||||
validate_sample_records(
|
||||
row
|
||||
for row in reusable
|
||||
if str(row.get("kind", "")) in RESUMABLE_SAMPLE_KINDS
|
||||
or row.get("kind") == "palm_axis_sample"
|
||||
)
|
||||
self.state = STATE_REVALIDATING_INHERITED
|
||||
self.reason = "revalidating_inherited_tasks"
|
||||
self.base_import_progress = {
|
||||
"phase": "revalidating",
|
||||
"records_read": len(rows),
|
||||
"completed_tasks": 0,
|
||||
"total_tasks": len(completed),
|
||||
"fraction": 0.0,
|
||||
}
|
||||
_publish_import_status(self)
|
||||
for durable in reusable:
|
||||
kind = str(durable["kind"])
|
||||
if kind == "synchronised_frame":
|
||||
@@ -4519,10 +4601,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"resume checkpoint contains unknown palm-axis source "
|
||||
+ source_name
|
||||
)
|
||||
record = dict(durable)
|
||||
record["command_u8"] = int(
|
||||
round(float(record["feedback_u8"]))
|
||||
)
|
||||
record = fitting_sample_record(durable)
|
||||
self.palm_axis_records_by_source[source_name].append(record)
|
||||
continue
|
||||
joint_name = str(durable["joint"])
|
||||
@@ -4530,15 +4609,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
raise RuntimeError(
|
||||
f"resume checkpoint contains unknown joint {joint_name}"
|
||||
)
|
||||
record = dict(durable)
|
||||
if kind in {"sample", "baseline_hold_sample"}:
|
||||
record["command_u8"] = int(
|
||||
round(float(record["feedback_u8"]))
|
||||
)
|
||||
elif kind == "steady_command_sample":
|
||||
record["command_u8"] = int(record["requested_command_u8"])
|
||||
else:
|
||||
if kind not in {
|
||||
"sample", "baseline_hold_sample", "steady_command_sample"
|
||||
}:
|
||||
continue
|
||||
record = fitting_sample_record(durable)
|
||||
if kind == "sample":
|
||||
self.records_by_joint[joint_name].append(record)
|
||||
elif kind == "baseline_hold_sample":
|
||||
@@ -4612,6 +4687,12 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"palm_axis_invalidated_task_keys": list(
|
||||
palm_axis_invalidated_tasks
|
||||
),
|
||||
"recalibration_scope": str(
|
||||
getattr(self, "recalibration_scope", "full")
|
||||
),
|
||||
"scope_invalidated_task_keys": list(
|
||||
scope_invalidated_tasks
|
||||
),
|
||||
"imported_record_count": len(reusable),
|
||||
"imported_attempt_floor_by_task": attempt_floor_by_task,
|
||||
"source_raw_samples_sha256": _file_sha256(source),
|
||||
@@ -4635,7 +4716,18 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"""
|
||||
accepted: list[str] = []
|
||||
dropped: list[dict[str, Any]] = []
|
||||
for task_key in completed:
|
||||
for task_index, task_key in enumerate(completed, start=1):
|
||||
if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED:
|
||||
total = max(1, len(completed))
|
||||
self.base_import_progress.update(
|
||||
{
|
||||
"current_task": task_key,
|
||||
"completed_tasks": task_index - 1,
|
||||
"total_tasks": len(completed),
|
||||
"fraction": (task_index - 1) / total,
|
||||
}
|
||||
)
|
||||
_publish_import_status(self)
|
||||
spec = next(
|
||||
(
|
||||
item.spec
|
||||
@@ -4669,6 +4761,16 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
continue
|
||||
break
|
||||
accepted.append(task_key)
|
||||
if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED:
|
||||
self.base_import_progress.update(
|
||||
{
|
||||
"current_task": "",
|
||||
"completed_tasks": len(completed),
|
||||
"total_tasks": len(completed),
|
||||
"fraction": 1.0,
|
||||
}
|
||||
)
|
||||
_publish_import_status(self)
|
||||
if dropped:
|
||||
accepted_set = set(accepted)
|
||||
for task_key in completed:
|
||||
@@ -4805,6 +4907,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"model": self.model,
|
||||
"hand_type": self.hand_type,
|
||||
"tag_layout": self.profile.layout_id,
|
||||
"sample_schema_version": self.sample_schema_version,
|
||||
"recalibration_scope": self.recalibration_scope,
|
||||
"recalibration_task_keys": list(
|
||||
self.recalibration_task_keys
|
||||
),
|
||||
"command_names": list(_command_names(self)),
|
||||
"capabilities": sorted(self.profile.capabilities),
|
||||
"reference_finger": self.profile.reference_finger,
|
||||
@@ -4865,6 +4972,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
},
|
||||
)
|
||||
try:
|
||||
if self.resume_raw_samples_path is not None:
|
||||
self.state = STATE_IMPORTING_BASE
|
||||
self.reason = "reading_base_session_records"
|
||||
self.base_import_progress = {
|
||||
"phase": "reading",
|
||||
"records_read": 0,
|
||||
"bytes_read": 0,
|
||||
"total_bytes": int(
|
||||
self.resume_raw_samples_path.stat().st_size
|
||||
),
|
||||
"fraction": 0.0,
|
||||
}
|
||||
self._publish_status(time.monotonic())
|
||||
restored_tasks = self._restore_durable_task_checkpoint()
|
||||
except Exception as error:
|
||||
self.started = False
|
||||
@@ -6376,11 +6496,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
),
|
||||
axis=0,
|
||||
)
|
||||
record = {
|
||||
# command_u8 is retained only in memory for the established
|
||||
# pure fitting API. The durable record below uses explicit
|
||||
# requested_command_u8 and feedback_u8 names.
|
||||
"command_u8": int(requested_command_u8),
|
||||
durable = canonical_sample_record({
|
||||
"kind": "steady_command_sample",
|
||||
"attempt": self.sweep_attempts.get(
|
||||
_sweep_storage_key(item.spec), 1
|
||||
@@ -6439,9 +6555,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
),
|
||||
"state_u8": [float(value) for value in state],
|
||||
"valid_frames": len(selected),
|
||||
}
|
||||
})
|
||||
record = fitting_sample_record(durable)
|
||||
self.command_records_by_joint[joint_name].append(record)
|
||||
durable = {key: value for key, value in record.items() if key != "command_u8"}
|
||||
append_jsonl(self.raw_path, durable)
|
||||
|
||||
def _publish_next_checkpoint(self, now: float) -> bool:
|
||||
@@ -8592,7 +8708,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
),
|
||||
axis=0,
|
||||
)
|
||||
record = {
|
||||
durable = canonical_sample_record({
|
||||
"kind": "baseline_hold_sample",
|
||||
"attempt": attempt,
|
||||
"task_name": item.spec.key,
|
||||
@@ -8601,7 +8717,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"motor_index": motor,
|
||||
"cycle": item.cycle,
|
||||
"direction": item.direction,
|
||||
"command_u8": command,
|
||||
"requested_command_u8": command,
|
||||
"feedback_u8": round(float(state[motor]), 6),
|
||||
"image_stamp_ns": int(
|
||||
np.median([frame.stamp_ns for frame in selected])
|
||||
),
|
||||
"relative_quaternion_xyzw": [
|
||||
float(value) for value in quaternion
|
||||
],
|
||||
@@ -8651,18 +8771,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
),
|
||||
"valid_frames": len(selected),
|
||||
"hold_seconds": float(self.baseline_hold_seconds),
|
||||
}
|
||||
})
|
||||
record = fitting_sample_record(durable)
|
||||
self.baseline_records_by_joint[joint_name].append(record)
|
||||
if item.cycle == 0:
|
||||
self.command_records_by_joint[joint_name].append(record)
|
||||
durable = {
|
||||
key: value for key, value in record.items() if key != "command_u8"
|
||||
}
|
||||
durable["requested_command_u8"] = command
|
||||
durable["feedback_u8"] = round(float(state[motor]), 6)
|
||||
durable["image_stamp_ns"] = int(
|
||||
np.median([frame.stamp_ns for frame in selected])
|
||||
)
|
||||
append_jsonl(self.raw_path, durable)
|
||||
|
||||
def _baseline_hysteresis_records(
|
||||
@@ -9005,7 +9118,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
np.asarray([frame.state_u8 for frame in frames], dtype=float),
|
||||
axis=0,
|
||||
)
|
||||
record = {
|
||||
durable = canonical_sample_record({
|
||||
"kind": "sample",
|
||||
"attempt": self.sweep_attempts.get(
|
||||
_sweep_storage_key(item.spec), 1
|
||||
@@ -9016,7 +9129,15 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"motor_index": motor,
|
||||
"cycle": item.cycle,
|
||||
"direction": item.direction,
|
||||
"command_u8": int(command),
|
||||
"requested_command_u8": int(
|
||||
self.sweep_checkpoint_target_u8
|
||||
if self.sweep_checkpoint_target_u8 is not None
|
||||
else item.target_u8
|
||||
),
|
||||
"feedback_u8": int(command),
|
||||
"image_stamp_ns": int(
|
||||
np.median([frame.stamp_ns for frame in frames])
|
||||
),
|
||||
"relative_translation_xyz_m": [
|
||||
float(value) for value in vector
|
||||
],
|
||||
@@ -9055,20 +9176,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
6,
|
||||
),
|
||||
"valid_frames": len(frames),
|
||||
}
|
||||
})
|
||||
record = fitting_sample_record(durable)
|
||||
self.records_by_joint[joint_name].append(record)
|
||||
durable = {
|
||||
key: value for key, value in record.items() if key != "command_u8"
|
||||
}
|
||||
durable["requested_command_u8"] = int(
|
||||
self.sweep_checkpoint_target_u8
|
||||
if self.sweep_checkpoint_target_u8 is not None
|
||||
else item.target_u8
|
||||
)
|
||||
durable["feedback_u8"] = int(command)
|
||||
durable["image_stamp_ns"] = int(
|
||||
np.median([frame.stamp_ns for frame in frames])
|
||||
)
|
||||
append_jsonl(self.raw_path, durable)
|
||||
|
||||
G20ThreeCameraCalibrationNode._persist_palm_axis_samples(self, item)
|
||||
@@ -9196,6 +9306,23 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
fitting_started_at = time.monotonic()
|
||||
self.last_status_publish = fitting_started_at
|
||||
self._publish_status(fitting_started_at)
|
||||
if int(getattr(self, "sample_schema_version", 0)) >= 1:
|
||||
for store_name, store in (
|
||||
("trajectory", self.records_by_joint),
|
||||
("baseline", self.baseline_records_by_joint),
|
||||
("steady", self.command_records_by_joint),
|
||||
("palm_axis", self.palm_axis_records_by_source),
|
||||
):
|
||||
try:
|
||||
validate_sample_records(
|
||||
record
|
||||
for records in store.values()
|
||||
for record in records
|
||||
)
|
||||
except SampleDataContractError as error:
|
||||
raise SampleDataContractError(
|
||||
f"{error};store={store_name}"
|
||||
) from error
|
||||
# Provisional checks have a warning band so collection can continue,
|
||||
# but the final fit always re-applies the unmodified hard thresholds.
|
||||
for spec in self.profile.sweep_specs:
|
||||
@@ -9452,10 +9579,16 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
minimum_sources=int(
|
||||
self.profile.minimum_palm_orientation_sources
|
||||
),
|
||||
minimum_arc_rad=self.trajectory_minimum_arc_rad,
|
||||
minimum_arc_rad=(
|
||||
self.profile.palm_orientation_minimum_arc_rad
|
||||
),
|
||||
maximum_rotation_orthogonal_rms_rad=(
|
||||
self.active_maximum_rotation_orthogonal_rms_rad
|
||||
),
|
||||
maximum_command_distance_u8=(
|
||||
self.profile
|
||||
.palm_orientation_maximum_command_distance_u8
|
||||
),
|
||||
)
|
||||
)
|
||||
except ValueError as error:
|
||||
@@ -9499,12 +9632,24 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
if self.profile.layout_id == G20_RIGHT_19_LAYOUT:
|
||||
command_fits = anchor_right_19_mechanical_endpoint_curves(
|
||||
command_fits,
|
||||
self.command_records_by_joint,
|
||||
{
|
||||
name: (
|
||||
[
|
||||
record
|
||||
for record in self.records_by_joint[name]
|
||||
if int(record["cycle"]) in training_cycle_set
|
||||
]
|
||||
if name == "thumb_cmc_roll"
|
||||
else self.command_records_by_joint[name]
|
||||
)
|
||||
for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS
|
||||
},
|
||||
maximum_direction_difference_rad=(
|
||||
self.command_maximum_direction_gap_rad
|
||||
),
|
||||
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
|
||||
)
|
||||
endpoint_zero_offsets = (
|
||||
measured_endpoint_zero_offsets = (
|
||||
derive_right_19_mechanical_endpoint_offsets(
|
||||
self.source_urdf_path,
|
||||
command_fits,
|
||||
@@ -9513,8 +9658,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
),
|
||||
)
|
||||
)
|
||||
endpoint_zero_offsets = {
|
||||
name: value
|
||||
for name, value in measured_endpoint_zero_offsets.items()
|
||||
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
}
|
||||
post_solve_endpoint_offsets = {
|
||||
name: value
|
||||
for name, value in measured_endpoint_zero_offsets.items()
|
||||
if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS
|
||||
}
|
||||
else:
|
||||
endpoint_zero_offsets = {}
|
||||
post_solve_endpoint_offsets = {}
|
||||
holdout_zero_result = solve_urdf_zero_offsets(
|
||||
source_urdf=self.source_urdf_path,
|
||||
measurements=axes,
|
||||
@@ -9559,6 +9715,10 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
**self.zero_profile.fixed_direct_zero_offsets_rad,
|
||||
**endpoint_zero_offsets,
|
||||
},
|
||||
static_output_zero_offsets_rad={
|
||||
**self.zero_profile.static_output_zero_offsets_rad,
|
||||
**post_solve_endpoint_offsets,
|
||||
},
|
||||
)
|
||||
append_jsonl(
|
||||
self.raw_path,
|
||||
@@ -11680,10 +11840,17 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"resume": {
|
||||
"used": bool(self.resumed_task_keys),
|
||||
"source_session": self.resume_source_session,
|
||||
"recalibration_scope": self.recalibration_scope,
|
||||
"recalibration_task_keys": list(
|
||||
self.recalibration_task_keys
|
||||
),
|
||||
"completed_task_count": len(self.resumed_task_keys),
|
||||
"total_task_count": len(self.profile.sweep_specs),
|
||||
"completed_task_keys": list(self.resumed_task_keys),
|
||||
},
|
||||
"base_import": dict(
|
||||
getattr(self, "base_import_progress", {})
|
||||
),
|
||||
"quality": (
|
||||
{} if self.completed_payload is None else self.completed_payload["quality"]
|
||||
),
|
||||
|
||||
@@ -28,6 +28,7 @@ from .full_hand import (
|
||||
JointCurveFit,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from .sample_schema import explicit_domain_value
|
||||
from .trajectory import (
|
||||
_fit_circle_with_axis,
|
||||
_fit_joint_curve,
|
||||
@@ -55,6 +56,10 @@ class ZeroCalibrationProfile:
|
||||
axis_parent_joint: Mapping[str, str]
|
||||
phase_parent_joint: Mapping[str, str]
|
||||
offset_observer_joint: Mapping[str, str]
|
||||
# Some serial offsets can be observed from the angle between two axes
|
||||
# captured by the same camera. The mapping is geometric topology only;
|
||||
# it never contains a model- or serial-specific zero value.
|
||||
same_view_axis_pair_by_offset: Mapping[str, tuple[str, str]]
|
||||
fixed_direct_zero_offsets_rad: Mapping[str, float]
|
||||
static_output_zero_offsets_rad: Mapping[str, float]
|
||||
|
||||
@@ -135,6 +140,7 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile:
|
||||
axis_parent_joint=axis_parent,
|
||||
phase_parent_joint=phase_parent,
|
||||
offset_observer_joint=observer,
|
||||
same_view_axis_pair_by_offset={},
|
||||
fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets,
|
||||
static_output_zero_offsets_rad={},
|
||||
)
|
||||
@@ -243,6 +249,9 @@ def _build_right_19_zero_profile(
|
||||
axis_parent_joint=axis_parent,
|
||||
phase_parent_joint=phase_parent,
|
||||
offset_observer_joint=observer,
|
||||
same_view_axis_pair_by_offset={
|
||||
"thumb_cmc_yaw": ("thumb_cmc_roll", "thumb_cmc_pitch")
|
||||
},
|
||||
# Endpoint-observable offsets are supplied by the caller. CMC remains
|
||||
# visually solved and no serial-specific zero is hidden in the shared
|
||||
# profile.
|
||||
@@ -262,6 +271,18 @@ RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset(
|
||||
}
|
||||
)
|
||||
|
||||
# Thumb CMC roll has a directly observed actuator-to-actuator travel, but its
|
||||
# electrical stop is not used as a runtime URDF limit. Keep it out of
|
||||
# RIGHT_19_MECHANICAL_ENDPOINT_JOINTS (whose offsets also move upper limits),
|
||||
# and use its endpoint estimate only as the final static roll origin. This
|
||||
# makes roll independent of the fitted palm-frame phase and of all yaw logic.
|
||||
RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS = frozenset({"thumb_cmc_roll"})
|
||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS = (
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
| RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS
|
||||
)
|
||||
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS = RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS
|
||||
|
||||
|
||||
def anchor_right_19_mechanical_endpoint_curves(
|
||||
curves: Mapping[str, JointCurveFit],
|
||||
@@ -269,6 +290,7 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
*,
|
||||
maximum_direction_difference_rad: float = math.radians(1.0),
|
||||
maximum_curve_correction_rad: float = math.radians(3.0),
|
||||
feedback_endpoint_joints: frozenset[str] = frozenset(),
|
||||
) -> dict[str, JointCurveFit]:
|
||||
"""Set mechanical-endpoint curve scale from direct SO(3) travel.
|
||||
|
||||
@@ -279,10 +301,11 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
both poses by a rigid hand/camera transform or either fixed Tag mounting
|
||||
rotation cannot change it.
|
||||
|
||||
Use both sweep directions as independent endpoint measurements, reject
|
||||
disagreement, and apply their robust mean as one scale correction to the
|
||||
already validated curve. Curve shape, hysteresis, axis diagnostics and
|
||||
all existing quality gates remain unchanged.
|
||||
Use every supplied cycle and both sweep directions as independent endpoint
|
||||
measurements, reject disagreement, and apply their robust median as one
|
||||
scale correction to the already validated curve. Dense CMC-roll records
|
||||
are selected in feedback coordinates; settled mechanical-contact records
|
||||
remain selected in requested-command coordinates.
|
||||
"""
|
||||
if (
|
||||
not math.isfinite(maximum_direction_difference_rad)
|
||||
@@ -296,41 +319,74 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
raise ValueError("endpoint curve correction limit must be positive")
|
||||
|
||||
result = dict(curves)
|
||||
for name in sorted(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS):
|
||||
unknown_feedback_joints = sorted(
|
||||
set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS)
|
||||
)
|
||||
if unknown_feedback_joints:
|
||||
raise ValueError(
|
||||
"feedback endpoint joints are not endpoint measurements: "
|
||||
+ ",".join(unknown_feedback_joints)
|
||||
)
|
||||
|
||||
for name in sorted(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS):
|
||||
fit = curves.get(name)
|
||||
records = list(records_by_joint.get(name, ()))
|
||||
if fit is None:
|
||||
raise ValueError(f"missing measured endpoint curve for {name}")
|
||||
|
||||
direction_travel: dict[str, float] = {}
|
||||
all_travel: list[float] = []
|
||||
for direction in ("decreasing", "increasing"):
|
||||
endpoint_rotations: dict[int, Rotation] = {}
|
||||
for command in (0, 255):
|
||||
quaternions = [
|
||||
_relative_rotation(record)
|
||||
cycles = sorted(
|
||||
{
|
||||
int(record.get("cycle", 0))
|
||||
for record in records
|
||||
if str(record.get("direction")) == direction
|
||||
and int(
|
||||
record.get(
|
||||
"requested_command_u8",
|
||||
record.get("command_u8", -1),
|
||||
)
|
||||
)
|
||||
== command
|
||||
]
|
||||
if not quaternions:
|
||||
raise ValueError(
|
||||
f"{name} {direction} is missing settled endpoint {command}"
|
||||
)
|
||||
endpoint_rotations[command] = Rotation.from_quat(
|
||||
robust_rotation_summary(quaternions)[0]
|
||||
)
|
||||
direction_travel[direction] = float(
|
||||
(
|
||||
endpoint_rotations[255].inv()
|
||||
* endpoint_rotations[0]
|
||||
).magnitude()
|
||||
}
|
||||
)
|
||||
branch_travel: list[float] = []
|
||||
for cycle in cycles:
|
||||
endpoint_rotations: dict[int, Rotation] = {}
|
||||
for command in (0, 255):
|
||||
quaternions = []
|
||||
for record in records:
|
||||
if (
|
||||
str(record.get("direction")) != direction
|
||||
or int(record.get("cycle", 0)) != cycle
|
||||
):
|
||||
continue
|
||||
endpoint_value = (
|
||||
explicit_domain_value(record, "feedback")
|
||||
if name in feedback_endpoint_joints
|
||||
else record.get(
|
||||
"requested_command_u8",
|
||||
record.get("command_u8", -1),
|
||||
)
|
||||
)
|
||||
if endpoint_value is None:
|
||||
continue
|
||||
if int(round(float(endpoint_value))) == command:
|
||||
quaternions.append(_relative_rotation(record))
|
||||
if not quaternions:
|
||||
raise ValueError(
|
||||
f"{name} {direction} cycle {cycle} is missing "
|
||||
f"settled endpoint {command}"
|
||||
)
|
||||
endpoint_rotations[command] = Rotation.from_quat(
|
||||
robust_rotation_summary(quaternions)[0]
|
||||
)
|
||||
branch_travel.append(
|
||||
float(
|
||||
(
|
||||
endpoint_rotations[255].inv()
|
||||
* endpoint_rotations[0]
|
||||
).magnitude()
|
||||
)
|
||||
)
|
||||
if not branch_travel:
|
||||
raise ValueError(f"{name} is missing {direction} endpoint travel")
|
||||
direction_travel[direction] = float(np.median(branch_travel))
|
||||
all_travel.extend(branch_travel)
|
||||
|
||||
travel_values = np.asarray(list(direction_travel.values()), dtype=float)
|
||||
direction_difference = float(np.ptp(travel_values))
|
||||
@@ -339,7 +395,13 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
f"{name} settled endpoint directions disagree: "
|
||||
f"{math.degrees(direction_difference):.3f}deg"
|
||||
)
|
||||
direct_travel = float(np.median(travel_values))
|
||||
cycle_range = float(np.ptp(np.asarray(all_travel, dtype=float)))
|
||||
if cycle_range > maximum_direction_difference_rad:
|
||||
raise ValueError(
|
||||
f"{name} settled endpoint cycles disagree: "
|
||||
f"{math.degrees(cycle_range):.3f}deg"
|
||||
)
|
||||
direct_travel = float(np.median(np.asarray(all_travel, dtype=float)))
|
||||
curve = np.asarray(fit.angle_rad, dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"{name} endpoint curve must contain 256 finite bins")
|
||||
@@ -364,6 +426,8 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
"mechanical_endpoint_direction_difference_rad": (
|
||||
direction_difference
|
||||
),
|
||||
"mechanical_endpoint_cycle_range_rad": cycle_range,
|
||||
"mechanical_endpoint_sample_count": len(all_travel),
|
||||
"mechanical_endpoint_raw_curve_travel_rad": projected_travel,
|
||||
"mechanical_endpoint_curve_scale": scale,
|
||||
}
|
||||
@@ -400,10 +464,10 @@ def derive_right_19_mechanical_endpoint_offsets(
|
||||
The source URDF upper limit describes those same physical endpoints.
|
||||
Therefore ``origin_offset + measured_travel == CAD_upper``.
|
||||
|
||||
The coupled thumb-CMC axes are deliberately excluded. Physical
|
||||
inspection of G20_RIGHT_001 proved that their electrical actuator
|
||||
endpoints do not equal the source-CAD upper coordinates. Treating them
|
||||
as equal produced repeatable but physically wrong zero offsets.
|
||||
Thumb CMC pitch/yaw remain excluded because their endpoint-to-CAD contract
|
||||
is not independently established. CMC roll is included as a post-solve
|
||||
origin only: its measured travel determines roll without allowing the
|
||||
palm common-direction fit to write that joint.
|
||||
|
||||
The returned corrections are measured, generally non-zero encoder zeros;
|
||||
they do not retain the CAD origin. Using full relative rotation travel
|
||||
@@ -415,7 +479,7 @@ def derive_right_19_mechanical_endpoint_offsets(
|
||||
root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot()
|
||||
joints = {str(node.get("name")): node for node in root.findall("joint")}
|
||||
result: dict[str, float] = {}
|
||||
for name in sorted(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS):
|
||||
for name in sorted(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS):
|
||||
fit = curves.get(name)
|
||||
joint = joints.get(name)
|
||||
limit = None if joint is None else joint.find("limit")
|
||||
@@ -1079,14 +1143,15 @@ def fit_partial_palm_orientation_measurement(
|
||||
zero_command_u8: int,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5),
|
||||
maximum_command_distance_u8: int = 255,
|
||||
) -> PalmOrientationMeasurement:
|
||||
"""Fit a physical axis direction without requiring full command travel."""
|
||||
samples = [
|
||||
"""Fit a physical axis within a configured window around encoder zero."""
|
||||
cycle_samples = [
|
||||
dict(record)
|
||||
for record in records
|
||||
if int(record.get("cycle", -1)) == int(cycle)
|
||||
]
|
||||
if len(samples) < 12:
|
||||
if len(cycle_samples) < 12:
|
||||
raise ValueError(
|
||||
f"{source_joint} cycle {cycle + 1} has too few visible samples"
|
||||
)
|
||||
@@ -1096,6 +1161,22 @@ def fit_partial_palm_orientation_measurement(
|
||||
raise ValueError(
|
||||
"palm orientation rotation residual limit must be positive"
|
||||
)
|
||||
command_distance = int(maximum_command_distance_u8)
|
||||
if command_distance < 1 or command_distance > 255:
|
||||
raise ValueError(
|
||||
"palm orientation command distance must be in [1, 255]"
|
||||
)
|
||||
zero = int(zero_command_u8)
|
||||
samples = [
|
||||
record
|
||||
for record in cycle_samples
|
||||
if abs(int(record["command_u8"]) - zero) <= command_distance
|
||||
]
|
||||
if len(samples) < 12:
|
||||
raise ValueError(
|
||||
f"{source_joint} cycle {cycle + 1} has too few zero-adjacent "
|
||||
"visible samples"
|
||||
)
|
||||
reference = Rotation.from_quat(
|
||||
_baseline_reference(samples, int(zero_command_u8))
|
||||
)
|
||||
@@ -1150,8 +1231,8 @@ def fit_partial_palm_orientation_measurement(
|
||||
)
|
||||
if orthogonal_rms > float(maximum_rotation_orthogonal_rms_rad):
|
||||
raise ValueError(
|
||||
f"{source_joint} cycle {cycle + 1} rotation residual "
|
||||
f"{math.degrees(orthogonal_rms):.3f}deg exceeds "
|
||||
f"{source_joint} cycle {cycle + 1} zero-adjacent rotation "
|
||||
f"residual {math.degrees(orthogonal_rms):.3f}deg exceeds "
|
||||
f"{math.degrees(maximum_rotation_orthogonal_rms_rad):.3f}deg"
|
||||
)
|
||||
zero_records = _near_zero_records(samples, int(zero_command_u8))
|
||||
@@ -1198,8 +1279,9 @@ def fit_partial_palm_orientation_measurements(
|
||||
minimum_sources: int,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5),
|
||||
maximum_command_distance_u8: int = 255,
|
||||
) -> tuple[tuple[PalmOrientationMeasurement, ...], Mapping[str, str]]:
|
||||
"""Fit every usable optional source and require redundant coverage."""
|
||||
"""Fit every usable optional source and require configured coverage."""
|
||||
source_map = {str(name): str(model) for name, model in sources.items()}
|
||||
minimum = int(minimum_sources)
|
||||
if not source_map:
|
||||
@@ -1236,6 +1318,9 @@ def fit_partial_palm_orientation_measurements(
|
||||
maximum_rotation_orthogonal_rms_rad=(
|
||||
maximum_rotation_orthogonal_rms_rad
|
||||
),
|
||||
maximum_command_distance_u8=(
|
||||
maximum_command_distance_u8
|
||||
),
|
||||
)
|
||||
except Exception as error:
|
||||
rejected[f"{source_joint}:cycle{cycle + 1}"] = str(error)
|
||||
@@ -2280,6 +2365,17 @@ def solve_urdf_zero_offsets(
|
||||
for item in orientation_measurements
|
||||
if int(item.cycle) == int(validation_cycle)
|
||||
)
|
||||
orientation_by_model_cycle: dict[
|
||||
tuple[str, int], PalmOrientationMeasurement
|
||||
] = {}
|
||||
for item in orientation_measurements:
|
||||
key = (str(item.model_joint), int(item.cycle))
|
||||
if key in orientation_by_model_cycle:
|
||||
raise ValueError(
|
||||
"palm orientation has multiple sources for "
|
||||
f"{key[0]} cycle {key[1] + 1}"
|
||||
)
|
||||
orientation_by_model_cycle[key] = item
|
||||
if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad:
|
||||
raise ValueError("finger maximum offset must be positive and no larger than thumb")
|
||||
joint_limits = {
|
||||
@@ -2368,6 +2464,7 @@ def solve_urdf_zero_offsets(
|
||||
joint_angles=angles,
|
||||
)
|
||||
return axis / np.linalg.norm(axis)
|
||||
|
||||
fixed_offsets = {
|
||||
str(name): float(value)
|
||||
for name, value in (
|
||||
@@ -2898,6 +2995,64 @@ def solve_urdf_zero_offsets(
|
||||
)
|
||||
)
|
||||
|
||||
def same_view_axis_pair_errors(
|
||||
offset_joint: str,
|
||||
selected: Sequence[JointAxisMeasurement],
|
||||
offsets: Mapping[str, float],
|
||||
) -> tuple[float, ...]:
|
||||
"""Return the camera-frame-invariant residual for a serial-axis pair.
|
||||
|
||||
This observation intentionally has two mirror roots. The optimiser
|
||||
disambiguates them with the original cross-view estimate, but the
|
||||
pair residual alone determines the final numerical zero.
|
||||
"""
|
||||
pair = profile.same_view_axis_pair_by_offset.get(offset_joint)
|
||||
if pair is None:
|
||||
return ()
|
||||
anchor_joint, observer_joint = pair
|
||||
|
||||
def angle(left: np.ndarray, right: np.ndarray) -> float:
|
||||
cosine = float(left @ right) / float(
|
||||
np.linalg.norm(left) * np.linalg.norm(right)
|
||||
)
|
||||
return math.acos(abs(float(np.clip(cosine, -1.0, 1.0))))
|
||||
|
||||
errors: list[float] = []
|
||||
for cycle in sorted({int(item.cycle) for item in selected}):
|
||||
anchor = orientation_by_model_cycle.get((anchor_joint, cycle))
|
||||
observer = orientation_by_model_cycle.get(
|
||||
(observer_joint, cycle)
|
||||
)
|
||||
if anchor is None or observer is None:
|
||||
return ()
|
||||
angles = _angles_from_state(
|
||||
observer.condition_state_u8,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=profile.inherited_zero_joints,
|
||||
)
|
||||
predicted_anchor, _ = model.axis_line(
|
||||
anchor_joint,
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
predicted_observer, _ = model.axis_line(
|
||||
observer_joint,
|
||||
zero_offsets=offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
observed_anchor = np.asarray(
|
||||
anchor.axis_common_xyz, dtype=float
|
||||
)
|
||||
observed_observer = np.asarray(
|
||||
observer.axis_common_xyz, dtype=float
|
||||
)
|
||||
errors.append(
|
||||
angle(predicted_anchor, predicted_observer)
|
||||
- angle(observed_anchor, observed_observer)
|
||||
)
|
||||
return tuple(errors)
|
||||
|
||||
def angular_error_samples(
|
||||
offsets: Mapping[str, float],
|
||||
selected: Sequence[JointAxisMeasurement],
|
||||
@@ -2909,6 +3064,12 @@ def solve_urdf_zero_offsets(
|
||||
name: [] for name in profile.direct_zero_joints
|
||||
}
|
||||
for offset_joint, observer_joint in profile.offset_observer_joint.items():
|
||||
pair_errors = same_view_axis_pair_errors(
|
||||
offset_joint, selected, offsets
|
||||
)
|
||||
if pair_errors:
|
||||
errors[offset_joint].extend(pair_errors)
|
||||
continue
|
||||
observer_items = [
|
||||
item for item in selected if item.joint == observer_joint
|
||||
]
|
||||
@@ -3061,11 +3222,18 @@ def solve_urdf_zero_offsets(
|
||||
}
|
||||
)
|
||||
result.update(fixed_offsets)
|
||||
for name, limit in zip(
|
||||
profile.direct_zero_joints, diagnostic_offset_limits
|
||||
for name, diagnostic_limit, configured_limit in zip(
|
||||
profile.direct_zero_joints,
|
||||
diagnostic_offset_limits,
|
||||
offset_limits,
|
||||
):
|
||||
if name in fixed_offsets:
|
||||
continue
|
||||
has_axis_pair = name in profile.same_view_axis_pair_by_offset
|
||||
limit = float(
|
||||
configured_limit if has_axis_pair else diagnostic_limit
|
||||
)
|
||||
|
||||
def residual(value: np.ndarray) -> np.ndarray:
|
||||
candidate = dict(result)
|
||||
candidate[name] = float(value[0])
|
||||
@@ -3432,25 +3600,37 @@ def solve_urdf_zero_offsets(
|
||||
improvement_by_joint: dict[str, float] = {}
|
||||
improvement_confidence_lower: dict[str, float] = {}
|
||||
improvement_passed = True
|
||||
palm_orientation_validation_errors = tuple(
|
||||
math.acos(
|
||||
abs(
|
||||
float(
|
||||
np.clip(
|
||||
base_rotation.apply(
|
||||
predicted_palm_orientation_local(
|
||||
item, zero_offsets
|
||||
if profile.same_view_axis_pair_by_offset:
|
||||
# Validate the same camera/Tag-mount-invariant axis-pair angle used by
|
||||
# the configured offset. This keeps the side channel out of every
|
||||
# other joint, including the already stable thumb CMC roll solve.
|
||||
palm_orientation_validation_errors = tuple(
|
||||
abs(float(error))
|
||||
for offset_joint in profile.same_view_axis_pair_by_offset
|
||||
for error in same_view_axis_pair_errors(
|
||||
offset_joint, validation, applied_training
|
||||
)
|
||||
)
|
||||
else:
|
||||
palm_orientation_validation_errors = tuple(
|
||||
math.acos(
|
||||
abs(
|
||||
float(
|
||||
np.clip(
|
||||
base_rotation.apply(
|
||||
predicted_palm_orientation_local(
|
||||
item, zero_offsets
|
||||
)
|
||||
)
|
||||
@ np.asarray(item.axis_common_xyz, dtype=float),
|
||||
-1.0,
|
||||
1.0,
|
||||
)
|
||||
@ np.asarray(item.axis_common_xyz, dtype=float),
|
||||
-1.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
)
|
||||
for item in orientation_validation
|
||||
)
|
||||
for item in orientation_validation
|
||||
)
|
||||
palm_orientation_validation_limit = (
|
||||
maximum_validation_error_rad
|
||||
if maximum_validation_error_rad is not None
|
||||
|
||||
@@ -287,6 +287,9 @@ def _launch_stack(context):
|
||||
"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
|
||||
@@ -483,6 +486,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
|
||||
DeclareLaunchArgument("recalibration_scope", default_value="full"),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(
|
||||
|
||||
@@ -99,8 +99,36 @@ def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None:
|
||||
"pinky_mcp_roll",
|
||||
"pinky_mcp_roll_side",
|
||||
)
|
||||
assert profile.palm_axis_observers == ()
|
||||
assert profile.minimum_palm_orientation_sources == 0
|
||||
assert {
|
||||
(
|
||||
observer.source_name,
|
||||
observer.task_name,
|
||||
observer.view,
|
||||
observer.model_joint,
|
||||
observer.motor_index,
|
||||
)
|
||||
for observer in profile.palm_axis_observers
|
||||
} == {
|
||||
(
|
||||
"thumb_cmc_pitch_top_axis",
|
||||
"thumb_cmc_pitch_front",
|
||||
"top",
|
||||
"thumb_cmc_pitch",
|
||||
0,
|
||||
),
|
||||
(
|
||||
"thumb_cmc_roll_top_axis",
|
||||
"thumb_cmc_roll_front",
|
||||
"top",
|
||||
"thumb_cmc_roll",
|
||||
5,
|
||||
),
|
||||
}
|
||||
assert profile.minimum_palm_orientation_sources == 2
|
||||
assert profile.palm_orientation_minimum_arc_rad == pytest.approx(
|
||||
math.radians(10.0)
|
||||
)
|
||||
assert profile.palm_orientation_maximum_command_distance_u8 == 64
|
||||
assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip")
|
||||
assert {
|
||||
name: spec.source_joint
|
||||
|
||||
@@ -31,6 +31,8 @@ from g20_thumb_apriltag_calibration.operator_report import (
|
||||
from g20_thumb_apriltag_calibration.one_command import (
|
||||
_automatic_resume_candidate,
|
||||
_calibration_node_exited_before_status,
|
||||
_launch_command,
|
||||
_resolve_partial_base_session,
|
||||
_status_timeout_seconds,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.product import load_product_config
|
||||
@@ -56,6 +58,7 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
|
||||
_model_link_in_observer_base,
|
||||
_palm_axis_observer_schema,
|
||||
_palm_axis_resume_policy,
|
||||
recalibration_task_keys,
|
||||
_steady_checkpoint_commands,
|
||||
_unresolved_fit_failure_tasks,
|
||||
combination_target_coverage,
|
||||
@@ -247,6 +250,9 @@ def _complete_resume_task_rows(profile, spec) -> list[dict]:
|
||||
"joint": joint,
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"requested_command_u8": (
|
||||
0 if direction == "decreasing" else 255
|
||||
),
|
||||
"feedback_u8": command,
|
||||
}
|
||||
for command in (*range(32), 255)
|
||||
@@ -339,18 +345,18 @@ def test_resume_preserves_palm_axis_side_channel_without_requiring_it() -> None:
|
||||
assert completed_without_side_channel == (spec.key,)
|
||||
|
||||
|
||||
def test_checkpoint_needs_no_palm_axis_reacquisition_when_disabled() -> None:
|
||||
def test_checkpoint_reacquires_new_thumb_yaw_side_channel_once() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
old_capabilities = set(profile.capabilities) - {
|
||||
"palm_axis_side_channel_v1"
|
||||
"palm_axis_side_channel_v2"
|
||||
}
|
||||
|
||||
compatible, invalidated = _palm_axis_resume_policy(
|
||||
profile, {"capabilities": sorted(old_capabilities)}
|
||||
)
|
||||
|
||||
assert compatible is True
|
||||
assert invalidated == ()
|
||||
assert compatible is False
|
||||
assert set(invalidated) == {spec.key for spec in profile.sweep_specs}
|
||||
|
||||
compatible, invalidated = _palm_axis_resume_policy(
|
||||
profile,
|
||||
@@ -759,6 +765,70 @@ def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> N
|
||||
assert _automatic_resume_candidate(config) is None
|
||||
|
||||
|
||||
def test_thumb_scope_resolves_passed_base_and_launches_partial_mode(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path)
|
||||
session = config.session_root / "20260830_120000"
|
||||
session.mkdir(parents=True)
|
||||
(session / "raw_samples.jsonl").write_text("{}\n", encoding="utf-8")
|
||||
(session / f"g20_right_{config.serial_number}_calibration.json").write_text(
|
||||
"{}\n", encoding="utf-8"
|
||||
)
|
||||
atomic_write_json(
|
||||
session / "calibration_summary_zh.json",
|
||||
{
|
||||
"result": "PASS",
|
||||
"quality": {"passed": True},
|
||||
"hashes": {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
},
|
||||
},
|
||||
)
|
||||
atomic_session_pointer(config.session_root, "latest_passed", session)
|
||||
|
||||
resolved = _resolve_partial_base_session(
|
||||
config, config.session_root / "latest_passed"
|
||||
)
|
||||
command = _launch_command(
|
||||
config,
|
||||
config.session_root / "20260830_130000",
|
||||
resume_from=resolved,
|
||||
recalibration_scope="thumb",
|
||||
)
|
||||
|
||||
assert resolved == session.resolve()
|
||||
assert "recalibration_scope:=thumb" in command
|
||||
assert f"resume_raw_samples_path:={session / 'raw_samples.jsonl'}" in command
|
||||
|
||||
|
||||
def test_thumb_scope_rejects_base_with_different_geometry(tmp_path: Path) -> None:
|
||||
config = _config(tmp_path)
|
||||
session = config.session_root / "20260830_120000"
|
||||
session.mkdir(parents=True)
|
||||
(session / "raw_samples.jsonl").touch()
|
||||
(session / f"g20_right_{config.serial_number}_calibration.json").write_text(
|
||||
"{}\n", encoding="utf-8"
|
||||
)
|
||||
atomic_write_json(
|
||||
session / "calibration_summary_zh.json",
|
||||
{
|
||||
"result": "PASS",
|
||||
"quality": {"passed": True},
|
||||
"hashes": {
|
||||
"source_urdf_sha256": "0" * 64,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
with pytest.raises(ValueError, match="differs from the current product"):
|
||||
_resolve_partial_base_session(config, session)
|
||||
|
||||
|
||||
def test_automatic_resume_accepts_ctrl_c_checkpoint_without_summary(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
@@ -847,6 +917,8 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
||||
},
|
||||
"baseline_command_u8": baseline,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"capabilities": sorted(profile.capabilities),
|
||||
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
||||
},
|
||||
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
|
||||
]
|
||||
@@ -882,6 +954,10 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
||||
records_by_joint={name: [] for name in profile.record_joints},
|
||||
baseline_records_by_joint={name: [] for name in profile.record_joints},
|
||||
command_records_by_joint={name: [] for name in profile.record_joints},
|
||||
palm_axis_records_by_source={
|
||||
observer.source_name: []
|
||||
for observer in profile.palm_axis_observers
|
||||
},
|
||||
sweep_index=0,
|
||||
sweep_items=sweep_items,
|
||||
resumed_task_keys=(),
|
||||
@@ -906,6 +982,99 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
||||
assert any(row["kind"] == "sample" for row in restored)
|
||||
|
||||
|
||||
def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks(
|
||||
tmp_path: Path, monkeypatch
|
||||
) -> None:
|
||||
monkeypatch.setattr(
|
||||
G20ThreeCameraCalibrationNode,
|
||||
"_revalidate_imported_tasks",
|
||||
lambda self, completed, **_kwargs: (tuple(completed), []),
|
||||
)
|
||||
config = _config(tmp_path)
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127] * 4
|
||||
thumb_tasks = recalibration_task_keys(profile, "thumb")
|
||||
thumb_spec = next(spec for spec in profile.sweep_specs if spec.key in thumb_tasks)
|
||||
finger_spec = next(
|
||||
spec
|
||||
for spec in profile.sweep_specs
|
||||
if spec.key == "pinky_pitch_side"
|
||||
)
|
||||
source_raw = tmp_path / "passed" / "raw_samples.jsonl"
|
||||
source_raw.parent.mkdir()
|
||||
rows = [
|
||||
{
|
||||
"kind": "session_start",
|
||||
"model": "G20",
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
"view_tags": {
|
||||
view: dict(tags) for view, tags in profile.view_tags.items()
|
||||
},
|
||||
"baseline_command_u8": baseline,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"capabilities": sorted(profile.capabilities),
|
||||
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
||||
},
|
||||
*_complete_resume_task_rows(profile, thumb_spec),
|
||||
*_complete_resume_task_rows(profile, finger_spec),
|
||||
]
|
||||
source_raw.write_text(
|
||||
"".join(json.dumps(row) + "\n" for row in rows),
|
||||
encoding="utf-8",
|
||||
)
|
||||
current_raw = tmp_path / "new" / "raw_samples.jsonl"
|
||||
current_raw.parent.mkdir()
|
||||
current_raw.touch()
|
||||
sweep_items = [
|
||||
SweepItem(spec, cycle, direction)
|
||||
for spec in profile.sweep_specs
|
||||
for cycle in range(4)
|
||||
for direction in ("decreasing", "increasing")
|
||||
]
|
||||
node = SimpleNamespace(
|
||||
resume_raw_samples_path=source_raw,
|
||||
raw_path=current_raw,
|
||||
model="G20",
|
||||
hand_type="right",
|
||||
profile=profile,
|
||||
baseline_command=tuple(baseline),
|
||||
source_urdf_path=config.source_urdf,
|
||||
repetitions=4,
|
||||
minimum_sweep_bins=32,
|
||||
records_by_joint={name: [] for name in profile.record_joints},
|
||||
baseline_records_by_joint={name: [] for name in profile.record_joints},
|
||||
command_records_by_joint={name: [] for name in profile.record_joints},
|
||||
palm_axis_records_by_source={
|
||||
observer.source_name: [] for observer in profile.palm_axis_observers
|
||||
},
|
||||
sweep_index=0,
|
||||
sweep_items=sweep_items,
|
||||
resumed_task_keys=(),
|
||||
resume_source_session="",
|
||||
recalibration_scope="thumb",
|
||||
recalibration_task_keys=thumb_tasks,
|
||||
)
|
||||
|
||||
count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint(node)
|
||||
|
||||
assert count == 1
|
||||
assert node.resumed_task_keys == (finger_spec.key,)
|
||||
assert node.sweep_index == 0
|
||||
assert all(
|
||||
not node.records_by_joint[name]
|
||||
for name in thumb_spec.joints
|
||||
)
|
||||
assert all(node.records_by_joint[name] for name in finger_spec.joints)
|
||||
imported = [
|
||||
json.loads(line)
|
||||
for line in current_raw.read_text(encoding="utf-8").splitlines()
|
||||
][0]
|
||||
assert imported["recalibration_scope"] == "thumb"
|
||||
assert imported["scope_invalidated_task_keys"] == list(thumb_tasks)
|
||||
|
||||
|
||||
def test_runtime_json_is_minimal_v4_with_21_independent_midpoint_curves() -> None:
|
||||
payload = _payload()
|
||||
assert payload["schema_version"] == 4
|
||||
@@ -955,6 +1124,33 @@ def test_publication_protects_passive_xml_and_requires_two_matching_sessions(
|
||||
assert len(commands["poses"]) == 8
|
||||
|
||||
|
||||
def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
session = _make_passed_session(config, "20260830_140000")
|
||||
status = _passed_node_status()
|
||||
tasks = list(
|
||||
recalibration_task_keys(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
"thumb",
|
||||
)
|
||||
)
|
||||
status["resume"] = {
|
||||
"used": True,
|
||||
"source_session": "20260828_205005",
|
||||
"recalibration_scope": "thumb",
|
||||
"recalibration_task_keys": tasks,
|
||||
}
|
||||
|
||||
summary, ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
|
||||
assert ready is True
|
||||
assert summary["calibration_scope"] == "thumb"
|
||||
assert summary["inherited_base_session"] == "20260828_205005"
|
||||
assert summary["freshly_calibrated_task_keys"] == tasks
|
||||
|
||||
|
||||
def test_publication_numerically_binds_json_offsets_and_urdf_limits(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
@@ -1351,6 +1547,16 @@ def test_missing_startup_status_has_specific_camera_status_code() -> None:
|
||||
assert "尚未开始运动" in suggestion
|
||||
|
||||
|
||||
def test_sample_domain_contract_has_specific_non_publication_code() -> None:
|
||||
code, problem, suggestion = classify_error(
|
||||
"DATA-CONTRACT-701:sample is missing explicit feedback_u8", {}
|
||||
)
|
||||
|
||||
assert code == "DATA-CONTRACT-701"
|
||||
assert "数据契约" in problem
|
||||
assert "无需重新采集" in suggestion
|
||||
|
||||
|
||||
def test_startup_watchdog_detects_only_calibration_child_exit(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
@@ -1795,6 +2001,8 @@ def test_node_drops_imported_task_failing_hard_gates(
|
||||
},
|
||||
"baseline_command_u8": baseline,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"capabilities": sorted(profile.capabilities),
|
||||
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
||||
},
|
||||
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
|
||||
]
|
||||
@@ -1825,6 +2033,10 @@ def test_node_drops_imported_task_failing_hard_gates(
|
||||
name: [] for name in profile.record_joints
|
||||
},
|
||||
command_records_by_joint={name: [] for name in profile.record_joints},
|
||||
palm_axis_records_by_source={
|
||||
observer.source_name: []
|
||||
for observer in profile.palm_axis_observers
|
||||
},
|
||||
sweep_index=0,
|
||||
sweep_items=sweep_items,
|
||||
resumed_task_keys=(),
|
||||
|
||||
@@ -0,0 +1,69 @@
|
||||
"""Tests for the explicit calibration sample command domains."""
|
||||
|
||||
from g20_thumb_apriltag_calibration.sample_schema import (
|
||||
SampleDataContractError,
|
||||
canonical_sample_record,
|
||||
fitting_sample_record,
|
||||
fitting_sample_records,
|
||||
)
|
||||
|
||||
import pytest
|
||||
|
||||
|
||||
def _sample(kind: str = 'sample') -> dict[str, object]:
|
||||
return {
|
||||
'kind': kind,
|
||||
'joint': 'thumb_cmc_roll',
|
||||
'cycle': 0,
|
||||
'direction': 'decreasing',
|
||||
'requested_command_u8': 0,
|
||||
'feedback_u8': 17.6,
|
||||
}
|
||||
|
||||
|
||||
def test_online_and_serialized_replay_use_identical_fit_projection() -> None:
|
||||
"""Serialization must not change the curve fitter's input record."""
|
||||
durable = canonical_sample_record(_sample())
|
||||
online = fitting_sample_record(durable)
|
||||
serialized = canonical_sample_record(online)
|
||||
replay = fitting_sample_record(serialized)
|
||||
|
||||
assert 'command_u8' not in durable
|
||||
assert serialized == durable
|
||||
assert replay == online
|
||||
assert online['command_u8'] == 18
|
||||
|
||||
|
||||
def test_default_domain_is_requested_only_for_steady_checkpoints() -> None:
|
||||
"""Settled checkpoints and dense trajectories use declared domains."""
|
||||
steady = _sample('steady_command_sample')
|
||||
|
||||
assert fitting_sample_record(steady)['command_u8'] == 0
|
||||
assert fitting_sample_record(steady, domain='feedback')['command_u8'] == 18
|
||||
|
||||
|
||||
def test_feedback_projection_can_snap_proven_requested_endpoints() -> None:
|
||||
"""A settled endpoint remains exact after feedback-domain projection."""
|
||||
records = fitting_sample_records(
|
||||
[_sample()],
|
||||
domain='feedback',
|
||||
snap_requested_endpoints=True,
|
||||
)
|
||||
|
||||
assert records[0]['command_u8'] == 0
|
||||
assert records[0]['feedback_u8'] == pytest.approx(17.6)
|
||||
|
||||
|
||||
def test_new_sample_rejects_missing_explicit_feedback_domain() -> None:
|
||||
"""A new observation cannot rely on the compatibility command key."""
|
||||
ambiguous = {
|
||||
'kind': 'sample',
|
||||
'requested_command_u8': 0,
|
||||
'command_u8': 0,
|
||||
}
|
||||
|
||||
with pytest.raises(
|
||||
SampleDataContractError,
|
||||
match='DATA-CONTRACT-701.*feedback_u8',
|
||||
):
|
||||
canonical_sample_record(ambiguous)
|
||||
@@ -268,22 +268,25 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
|
||||
assert "运动采样:" not in text
|
||||
|
||||
|
||||
def test_palm_orientation_failure_explains_direction_coverage() -> None:
|
||||
def test_palm_orientation_failure_explains_thumb_top_coverage() -> None:
|
||||
explanation, suggestion = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"palm_orientation_quality_failed",
|
||||
{
|
||||
"failures": [
|
||||
{
|
||||
"reason": "palm orientation cycle 2 has 2/3 usable sources"
|
||||
"reason": (
|
||||
"palm orientation cycle 2 has 1/2 usable sources: "
|
||||
"thumb_cmc_roll_top_axis:cycle2=too few samples"
|
||||
)
|
||||
}
|
||||
]
|
||||
},
|
||||
)
|
||||
|
||||
assert "至少三根手指" in explanation
|
||||
assert "2/3 usable sources" in explanation
|
||||
assert "Tag 10–13" in suggestion
|
||||
assert "顶部Tag 8/9" in explanation
|
||||
assert "1/2 usable sources" in explanation
|
||||
assert "CMC pitch和roll" in suggestion
|
||||
|
||||
|
||||
def test_cross_view_side_line_rms_names_side_source() -> None:
|
||||
|
||||
@@ -535,6 +535,38 @@ def test_right_roll_requires_both_views_task_local_tags() -> None:
|
||||
) == ("side_base", "pinky_pip")
|
||||
|
||||
|
||||
def test_thumb_yaw_top_axis_observer_does_not_change_front_sweep_contract() -> None:
|
||||
spec = next(
|
||||
item
|
||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
||||
if item.key == "thumb_cmc_pitch_front"
|
||||
)
|
||||
observer = _palm_axis_observer_for_sweep(
|
||||
RIGHT_19_HAND_PROFILE, spec, "top"
|
||||
)
|
||||
assert observer is not None
|
||||
assert observer.model_joint == "thumb_cmc_pitch"
|
||||
assert _sweep_views(RIGHT_19_HAND_PROFILE, spec) == ("front",)
|
||||
|
||||
node = SimpleNamespace(
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
views={
|
||||
view: SimpleNamespace(
|
||||
roles=tuple(RIGHT_19_HAND_PROFILE.view_tags[view]),
|
||||
preflight_roles=(f"{view}_base",),
|
||||
)
|
||||
for view in ("front", "side", "top")
|
||||
},
|
||||
active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING),
|
||||
active_validation=None,
|
||||
retry_sweep_spec=None,
|
||||
active_combination_validation=None,
|
||||
)
|
||||
assert G20ThreeCameraCalibrationNode._required_roles_for_view(
|
||||
node, "top"
|
||||
) == ("top_base",)
|
||||
|
||||
|
||||
def test_pitch_task_does_not_add_a_front_direction_observer() -> None:
|
||||
spec = next(
|
||||
item
|
||||
@@ -4422,6 +4454,10 @@ def _import_revalidation_node(records_by_joint: dict) -> SimpleNamespace:
|
||||
]
|
||||
for name, records in records_by_joint.items()
|
||||
}
|
||||
node.palm_axis_records_by_source = {
|
||||
observer.source_name: []
|
||||
for observer in RIGHT_19_HAND_PROFILE.palm_axis_observers
|
||||
}
|
||||
node.command_maximum_direction_gap_rad = math.radians(2.0)
|
||||
node.sweep_items = [
|
||||
SweepItem(spec, 0, DIRECTION_DECREASING)
|
||||
|
||||
@@ -617,10 +617,12 @@ def _solve_synthetic_offsets(
|
||||
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,
|
||||
palm_orientation_frame_bias_degrees: float = 0.0,
|
||||
maximum_systematic_axis_cone_bias_degrees: float | None = None,
|
||||
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
|
||||
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
||||
validation_offset_bias_degrees: dict[str, float] | None = None,
|
||||
static_output_offsets_degrees: dict[str, float] | None = None,
|
||||
base_euler_xyz_rad: tuple[float, float, float] = (0.5, -0.4, 0.8),
|
||||
base_translation_xyz_m: tuple[float, float, float] = (0.31, -0.19, 0.72),
|
||||
):
|
||||
@@ -782,12 +784,20 @@ def _solve_synthetic_offsets(
|
||||
zero_offsets=cycle_offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
axis_common = base_rotation.apply(axis)
|
||||
if palm_orientation_frame_bias_degrees:
|
||||
bias_axis = np.asarray([0.3, -0.2, 0.4], dtype=float)
|
||||
bias_axis /= np.linalg.norm(bias_axis)
|
||||
axis_common = Rotation.from_rotvec(
|
||||
math.radians(palm_orientation_frame_bias_degrees)
|
||||
* bias_axis
|
||||
).apply(axis_common)
|
||||
palm_orientation_measurements.append(
|
||||
PalmOrientationMeasurement(
|
||||
source_joint=source_joint,
|
||||
model_joint=model_joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(base_rotation.apply(axis)),
|
||||
axis_common_xyz=tuple(axis_common),
|
||||
condition_state_u8=tuple(state),
|
||||
observed_arc_rad=math.radians(45.0),
|
||||
rotation_orthogonal_rms_rad=math.radians(0.1),
|
||||
@@ -814,6 +824,10 @@ def _solve_synthetic_offsets(
|
||||
maximum_systematic_axis_cone_bias_degrees
|
||||
)
|
||||
),
|
||||
static_output_zero_offsets_rad={
|
||||
name: math.radians(value)
|
||||
for name, value in (static_output_offsets_degrees or {}).items()
|
||||
},
|
||||
)
|
||||
return zero, result
|
||||
|
||||
@@ -870,6 +884,35 @@ def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supp
|
||||
assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints)
|
||||
|
||||
|
||||
def test_right_19_independent_roll_output_cannot_change_yaw_solution() -> None:
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0, 0.7,
|
||||
0.8, -0.7, -0.8,
|
||||
-0.5, 0.6, 0.9,
|
||||
1.1, -1.0, -0.6,
|
||||
]
|
||||
_, visual = _solve_synthetic_offsets(
|
||||
"right", injected, layout_id="g20_right_19"
|
||||
)
|
||||
_, independent = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
static_output_offsets_degrees={"thumb_cmc_roll": 3.4},
|
||||
)
|
||||
|
||||
assert math.degrees(
|
||||
independent.direct_offsets_rad["thumb_cmc_roll"]
|
||||
) == pytest.approx(3.4)
|
||||
assert independent.direct_offsets_rad["thumb_cmc_yaw"] == pytest.approx(
|
||||
visual.direct_offsets_rad["thumb_cmc_yaw"], abs=1.0e-10
|
||||
)
|
||||
assert independent.direct_offsets_rad["thumb_cmc_pitch"] == pytest.approx(
|
||||
visual.direct_offsets_rad["thumb_cmc_pitch"], abs=1.0e-10
|
||||
)
|
||||
|
||||
|
||||
def _partial_orientation_records(
|
||||
*,
|
||||
tag_mount: Rotation,
|
||||
@@ -995,6 +1038,66 @@ def test_partial_palm_direction_is_not_weighted_by_dwell_frame_count() -> None:
|
||||
assert difference < math.radians(0.25)
|
||||
|
||||
|
||||
def test_partial_palm_direction_ignores_far_stroke_pnp_bias() -> None:
|
||||
records = _partial_orientation_records(
|
||||
tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]),
|
||||
common_rotation=Rotation.identity(),
|
||||
maximum_angle_deg=30.0,
|
||||
)
|
||||
expected = fit_partial_palm_orientation_measurement(
|
||||
"thumb_cmc_roll_top_axis",
|
||||
"thumb_cmc_roll",
|
||||
records,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
minimum_arc_rad=math.radians(5.0),
|
||||
maximum_command_distance_u8=64,
|
||||
)
|
||||
biased: list[dict[str, object]] = []
|
||||
for source in records:
|
||||
record = dict(source)
|
||||
if abs(int(record["command_u8"]) - 255) > 64:
|
||||
rotation = Rotation.from_quat(
|
||||
record["relative_quaternion_xyzw"]
|
||||
)
|
||||
record["relative_quaternion_xyzw"] = list(
|
||||
(
|
||||
Rotation.from_rotvec([math.radians(12.0), 0.0, 0.0])
|
||||
* rotation
|
||||
).as_quat()
|
||||
)
|
||||
biased.append(record)
|
||||
|
||||
fitted = fit_partial_palm_orientation_measurement(
|
||||
"thumb_cmc_roll_top_axis",
|
||||
"thumb_cmc_roll",
|
||||
biased,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
minimum_arc_rad=math.radians(5.0),
|
||||
maximum_command_distance_u8=64,
|
||||
)
|
||||
|
||||
assert abs(
|
||||
float(np.dot(expected.axis_common_xyz, fitted.axis_common_xyz))
|
||||
) == pytest.approx(1.0, abs=1.0e-10)
|
||||
|
||||
|
||||
def test_right_19_top_axis_pair_holdout_ignores_shared_frame_bias() -> None:
|
||||
injected = [0.0] * 16
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
palm_orientation_frame_bias_degrees=8.0,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_yaw"]
|
||||
) == pytest.approx(0.0, abs=0.05)
|
||||
|
||||
|
||||
def test_partial_palm_direction_rejects_too_short_visible_arc() -> None:
|
||||
records = _partial_orientation_records(
|
||||
tag_mount=Rotation.identity(),
|
||||
@@ -1066,7 +1169,7 @@ def test_right_19_offsets_are_invariant_to_rigid_hand_repositioning() -> None:
|
||||
)
|
||||
|
||||
|
||||
def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None:
|
||||
def test_right_19_roll_and_verified_contact_zeros_use_independent_endpoints() -> None:
|
||||
hand = get_hand_calibration_profile("right", "g20_right_19")
|
||||
curves = {
|
||||
name: _synthetic_curve(255, math.radians(50.0))
|
||||
@@ -1095,6 +1198,7 @@ def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None
|
||||
)
|
||||
|
||||
assert set(offsets) == {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
*(
|
||||
f"{finger}_{suffix}"
|
||||
@@ -1105,9 +1209,10 @@ def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None
|
||||
assert math.degrees(offsets["thumb_mcp"]) == pytest.approx(
|
||||
math.degrees(1.25) - 70.0
|
||||
)
|
||||
assert not {
|
||||
"thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"
|
||||
}.intersection(offsets)
|
||||
assert math.degrees(offsets["thumb_cmc_roll"]) == pytest.approx(
|
||||
math.degrees(1.39) - 76.0
|
||||
)
|
||||
assert not {"thumb_cmc_yaw", "thumb_cmc_pitch"}.intersection(offsets)
|
||||
assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx(
|
||||
math.degrees(1.22) - 71.0
|
||||
)
|
||||
@@ -1141,8 +1246,10 @@ def _settled_endpoint_records(
|
||||
for command, rotation in ((0, endpoint), (255, fixed_mounting)):
|
||||
result.append(
|
||||
{
|
||||
"cycle": 0,
|
||||
"direction": direction,
|
||||
"requested_command_u8": command,
|
||||
"feedback_u8": float(command),
|
||||
"relative_quaternion_xyzw": rotation.as_quat().tolist(),
|
||||
}
|
||||
)
|
||||
@@ -1152,6 +1259,7 @@ def _settled_endpoint_records(
|
||||
def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None:
|
||||
hand = get_hand_calibration_profile("right", "g20_right_19")
|
||||
names = {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
*(
|
||||
f"{finger}_{suffix}"
|
||||
@@ -1169,7 +1277,11 @@ def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None:
|
||||
name: _settled_endpoint_records(direct_travel) for name in names
|
||||
}
|
||||
|
||||
anchored = anchor_right_19_mechanical_endpoint_curves(curves, records)
|
||||
anchored = anchor_right_19_mechanical_endpoint_curves(
|
||||
curves,
|
||||
records,
|
||||
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
|
||||
)
|
||||
|
||||
assert curves["middle_pip"].angle_rad[0] == pytest.approx(
|
||||
projected_travel
|
||||
@@ -1186,6 +1298,7 @@ def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None:
|
||||
def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None:
|
||||
hand = get_hand_calibration_profile("right", "g20_right_19")
|
||||
names = {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
*(
|
||||
f"{finger}_{suffix}"
|
||||
@@ -1207,7 +1320,11 @@ def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None:
|
||||
)
|
||||
|
||||
with pytest.raises(ValueError, match="endpoint directions disagree"):
|
||||
anchor_right_19_mechanical_endpoint_curves(curves, records)
|
||||
anchor_right_19_mechanical_endpoint_curves(
|
||||
curves,
|
||||
records,
|
||||
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
|
||||
)
|
||||
|
||||
|
||||
def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
|
||||
@@ -1565,6 +1682,31 @@ def test_right_19_audits_stable_cross_view_cone_bias_without_retry() -> None:
|
||||
] == "stable_cross_view_or_planar_pnp_bias"
|
||||
|
||||
|
||||
def test_right_19_thumb_yaw_uses_top_axis_pair_not_front_cone_bias() -> None:
|
||||
zero = get_zero_calibration_profile("right", "g20_right_19")
|
||||
injected = [0.0] * len(zero.direct_zero_joints)
|
||||
injected[zero.direct_zero_joints.index("thumb_cmc_roll")] = 3.611
|
||||
injected[zero.direct_zero_joints.index("thumb_cmc_yaw")] = -1.319
|
||||
zero, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
inject_observer_cone_bias_degrees=3.803,
|
||||
maximum_systematic_axis_cone_bias_degrees=15.0,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_roll"]
|
||||
) == pytest.approx(3.611, abs=0.05)
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_yaw"]
|
||||
) == pytest.approx(-1.319, abs=0.05)
|
||||
assert math.degrees(
|
||||
result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"]
|
||||
) == pytest.approx(3.803, abs=0.01)
|
||||
|
||||
|
||||
def test_right_19_still_rejects_gross_cross_view_cone_mismatch() -> None:
|
||||
injected = [0.0] * 16
|
||||
_, result = _solve_synthetic_offsets(
|
||||
|
||||
@@ -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],
|
||||
|
||||
Reference in New Issue
Block a user