根据json修正urdf
This commit is contained in:
@@ -52,6 +52,13 @@ ros2 run linkerhand_calibration calibrate_hand --config \
|
||||
--offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl
|
||||
```
|
||||
|
||||
拟合通过后，程序先原子写入并重新校验
|
||||
`l6_right_<序列号>_urdf_correction_input.json`，验证串号、Profile 和源 URDF
|
||||
SHA-256 后，才把其中的原精度零偏、行程和 DIP 耦合参数交给现有 URDF 写回器。
|
||||
面向运行时的 schema-v6 `*_partial_calibration.json` 及修正 URDF 的字段、数值和
|
||||
格式保持兼容；单独的 correction-input JSON 是可审计的 URDF 生成依据，不是运行桥
|
||||
的输入文件。
|
||||
|
||||
运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、
|
||||
roll、index、middle、ring、pinky；旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路
|
||||
兼容别名读取，新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。
|
||||
@@ -612,15 +619,21 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_calibration.json
|
||||
g20_left_G20_LEFT_001_calibration_urdf_correction_input.json
|
||||
linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
|
||||
calibration_output/G20_RIGHT_001/<时间戳>/
|
||||
g20_right_G20_RIGHT_001_calibration.json
|
||||
g20_right_G20_RIGHT_001_calibration_urdf_correction_input.json
|
||||
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
```
|
||||
|
||||
`*_urdf_correction_input.json` 会在修正 URDF 之前落盘并重新读取，且绑定源 URDF
|
||||
哈希、型号、侧别、layout 和序列号。公开 schema-v4 运行 JSON 仍保持原格式，原有
|
||||
G20 运行桥、金标准哈希和 URDF 写回数值不因该交接层改变。
|
||||
|
||||
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
|
||||
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
||||
指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
|
||||
|
||||
@@ -90,6 +90,10 @@ from ...sample_schema import (
|
||||
)
|
||||
from ...storage import append_jsonl, append_jsonl_many, atomic_write_json
|
||||
from .reporting_zh import render_three_camera_status_text_zh
|
||||
from .urdf_input import (
|
||||
build_g20_urdf_input_payload,
|
||||
load_g20_urdf_input,
|
||||
)
|
||||
from .zero_solver import (
|
||||
LEFT_ZERO_PROFILE,
|
||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
||||
@@ -11787,6 +11791,27 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
name: urdf_offsets[name] for name in frozen_names
|
||||
},
|
||||
)
|
||||
urdf_input_path = self.final_path.with_name(
|
||||
f"{self.final_path.stem}_urdf_correction_input.json"
|
||||
)
|
||||
atomic_write_json(
|
||||
urdf_input_path,
|
||||
build_g20_urdf_input_payload(
|
||||
side=self.hand_type,
|
||||
layout_id=self.profile.layout_id,
|
||||
serial_number=self.serial_number,
|
||||
source_urdf=self.source_urdf_path,
|
||||
offsets_rad=urdf_offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
|
||||
),
|
||||
)
|
||||
urdf_offsets, endpoint_zero_offsets = load_g20_urdf_input(
|
||||
urdf_input_path,
|
||||
source_urdf=self.source_urdf_path,
|
||||
side=self.hand_type,
|
||||
layout_id=self.profile.layout_id,
|
||||
serial_number=self.serial_number,
|
||||
)
|
||||
self.corrected_urdf_path = write_zero_corrected_urdf(
|
||||
source_urdf=self.source_urdf_path,
|
||||
output_directory=self.corrected_urdf_output_dir,
|
||||
|
||||
@@ -42,6 +42,10 @@ from ...product import get_product_calibration_contract
|
||||
from ...sample_schema import fitting_sample_record, fitting_sample_records
|
||||
from ...storage import atomic_write_json
|
||||
from .publication import clamp_compact_payload_to_urdf_limits
|
||||
from .urdf_input import (
|
||||
build_g20_urdf_input_payload,
|
||||
load_g20_urdf_input,
|
||||
)
|
||||
from .zero_solver import (
|
||||
JointAxisMeasurement,
|
||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
|
||||
@@ -1717,11 +1721,19 @@ def replay_session(
|
||||
f"{output_suffix}.urdf"
|
||||
)
|
||||
final_urdf = session / expected_urdf_name
|
||||
urdf_input_path = session / (
|
||||
f"g20_{side}_{hand_serial}_calibration{output_suffix}"
|
||||
"_urdf_correction_input.json"
|
||||
)
|
||||
report_path = session / (
|
||||
f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json"
|
||||
)
|
||||
if write_outputs:
|
||||
existing = [path for path in (final_json, final_urdf, report_path) if path.exists()]
|
||||
existing = [
|
||||
path
|
||||
for path in (final_json, final_urdf, urdf_input_path, report_path)
|
||||
if path.exists()
|
||||
]
|
||||
if existing:
|
||||
raise ValueError(
|
||||
"refusing to overwrite replay outputs: "
|
||||
@@ -1742,6 +1754,29 @@ def replay_session(
|
||||
else published_zero_offsets
|
||||
)
|
||||
with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary:
|
||||
correction_json = (
|
||||
urdf_input_path
|
||||
if write_outputs
|
||||
else Path(temporary) / urdf_input_path.name
|
||||
)
|
||||
atomic_write_json(
|
||||
correction_json,
|
||||
build_g20_urdf_input_payload(
|
||||
side=side,
|
||||
layout_id=layout_id,
|
||||
serial_number=hand_serial,
|
||||
source_urdf=source_urdf,
|
||||
offsets_rad=urdf_offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
|
||||
),
|
||||
)
|
||||
urdf_offsets, endpoint_zero_offsets = load_g20_urdf_input(
|
||||
correction_json,
|
||||
source_urdf=source_urdf,
|
||||
side=side,
|
||||
layout_id=layout_id,
|
||||
serial_number=hand_serial,
|
||||
)
|
||||
candidate = write_zero_corrected_urdf(
|
||||
source_urdf=source_urdf,
|
||||
output_directory=temporary,
|
||||
|
||||
@@ -0,0 +1,133 @@
|
||||
"""Exact JSON handoff between G20 fitting and the existing URDF writer."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
SCHEMA_VERSION = 1
|
||||
ARTIFACT_TYPE = "linkerhand_g20_urdf_correction_input"
|
||||
|
||||
|
||||
def _sha256_file(path: str | Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with Path(path).expanduser().resolve().open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def build_g20_urdf_input_payload(
|
||||
*,
|
||||
side: str,
|
||||
layout_id: str,
|
||||
serial_number: str,
|
||||
source_urdf: str | Path,
|
||||
offsets_rad: Mapping[str, float],
|
||||
endpoint_anchored_offsets_rad: Mapping[str, float],
|
||||
) -> dict[str, Any]:
|
||||
"""Build the exact correction parameters that will be read from disk."""
|
||||
payload: dict[str, Any] = {
|
||||
"schema_version": SCHEMA_VERSION,
|
||||
"artifact_type": ARTIFACT_TYPE,
|
||||
"profile_id": f"G20/{str(side).lower()}/{layout_id}",
|
||||
"model": "G20",
|
||||
"side": str(side).lower(),
|
||||
"layout_id": str(layout_id),
|
||||
"serial_number": str(serial_number),
|
||||
"source_urdf_sha256": _sha256_file(source_urdf),
|
||||
"offsets_rad": {
|
||||
str(name): float(value) for name, value in sorted(offsets_rad.items())
|
||||
},
|
||||
"endpoint_anchored_offsets_rad": {
|
||||
str(name): float(value)
|
||||
for name, value in sorted(endpoint_anchored_offsets_rad.items())
|
||||
},
|
||||
}
|
||||
validate_g20_urdf_input_payload(payload)
|
||||
return payload
|
||||
|
||||
|
||||
def validate_g20_urdf_input_payload(payload: Mapping[str, Any]) -> None:
|
||||
required = {
|
||||
"schema_version", "artifact_type", "profile_id", "model", "side",
|
||||
"layout_id", "serial_number", "source_urdf_sha256", "offsets_rad",
|
||||
"endpoint_anchored_offsets_rad",
|
||||
}
|
||||
if set(payload) != required:
|
||||
raise ValueError("G20 URDF correction input has unexpected fields")
|
||||
if (
|
||||
payload.get("schema_version") != SCHEMA_VERSION
|
||||
or payload.get("artifact_type") != ARTIFACT_TYPE
|
||||
or payload.get("model") != "G20"
|
||||
or payload.get("side") not in {"left", "right"}
|
||||
):
|
||||
raise ValueError("G20 URDF correction input identity is invalid")
|
||||
expected_profile = (
|
||||
f"G20/{payload['side']}/{payload['layout_id']}"
|
||||
)
|
||||
if payload.get("profile_id") != expected_profile:
|
||||
raise ValueError("G20 URDF correction input profile is invalid")
|
||||
source_hash = str(payload.get("source_urdf_sha256", ""))
|
||||
if len(source_hash) != 64 or any(
|
||||
character not in "0123456789abcdef" for character in source_hash
|
||||
):
|
||||
raise ValueError("G20 URDF correction input source hash is invalid")
|
||||
offsets = payload.get("offsets_rad")
|
||||
endpoints = payload.get("endpoint_anchored_offsets_rad")
|
||||
if not isinstance(offsets, Mapping) or not offsets:
|
||||
raise ValueError("G20 URDF correction input offsets are missing")
|
||||
if not isinstance(endpoints, Mapping) or not set(endpoints) <= set(offsets):
|
||||
raise ValueError("G20 URDF correction input endpoint offsets are invalid")
|
||||
if any(not math.isfinite(float(value)) for value in offsets.values()):
|
||||
raise ValueError("G20 URDF correction input contains a non-finite offset")
|
||||
if any(not math.isfinite(float(value)) for value in endpoints.values()):
|
||||
raise ValueError("G20 URDF correction input has a non-finite endpoint")
|
||||
|
||||
|
||||
def load_g20_urdf_input(
|
||||
path: str | Path,
|
||||
*,
|
||||
source_urdf: str | Path,
|
||||
side: str,
|
||||
layout_id: str,
|
||||
serial_number: str,
|
||||
) -> tuple[dict[str, float], dict[str, float]]:
|
||||
"""Read, validate and authenticate correction parameters from JSON."""
|
||||
source = Path(path).expanduser().resolve()
|
||||
try:
|
||||
payload = json.loads(source.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as error:
|
||||
raise ValueError(f"could not read G20 URDF correction input: {source}") from error
|
||||
if not isinstance(payload, Mapping):
|
||||
raise ValueError("G20 URDF correction input must be a JSON object")
|
||||
validate_g20_urdf_input_payload(payload)
|
||||
if (
|
||||
payload["side"] != str(side).lower()
|
||||
or payload["layout_id"] != str(layout_id)
|
||||
or payload["serial_number"] != str(serial_number)
|
||||
):
|
||||
raise ValueError("G20 URDF correction input does not match the session")
|
||||
if payload["source_urdf_sha256"] != _sha256_file(source_urdf):
|
||||
raise ValueError("G20 source URDF changed after calibration JSON was written")
|
||||
return (
|
||||
{
|
||||
str(name): float(value)
|
||||
for name, value in payload["offsets_rad"].items()
|
||||
},
|
||||
{
|
||||
str(name): float(value)
|
||||
for name, value in payload["endpoint_anchored_offsets_rad"].items()
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"build_g20_urdf_input_payload",
|
||||
"load_g20_urdf_input",
|
||||
"validate_g20_urdf_input_payload",
|
||||
]
|
||||
@@ -12,7 +12,8 @@ import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .fitting import L6FitResult
|
||||
from ..g20.profile import JointCurveFit
|
||||
from .fitting import L6FitResult, MimicFit
|
||||
from .profile import (
|
||||
ACTIVE_JOINTS,
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
@@ -31,6 +32,8 @@ from .profile import (
|
||||
|
||||
|
||||
ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS)
|
||||
L6_URDF_INPUT_SCHEMA_VERSION = 1
|
||||
L6_URDF_INPUT_ARTIFACT_TYPE = "linkerhand_l6_urdf_correction_input"
|
||||
|
||||
|
||||
def _sha256_file(path: str | Path) -> str:
|
||||
@@ -344,6 +347,215 @@ def build_l6_runtime_payload(
|
||||
return payload
|
||||
|
||||
|
||||
def build_l6_urdf_input_payload(
|
||||
*,
|
||||
serial_number: str,
|
||||
source_urdf: str | Path,
|
||||
result: L6FitResult,
|
||||
) -> dict[str, Any]:
|
||||
"""Serialize the exact, minimal L6 fit consumed by the URDF writer.
|
||||
|
||||
The public schema-v6 artifact deliberately rounds runtime lookup tables.
|
||||
Feeding those rounded values back into the URDF writer would alter
|
||||
established URDF bytes. This correction-input JSON preserves Python's
|
||||
round-trip float representation without changing either public schema or
|
||||
correction mathematics.
|
||||
"""
|
||||
measured = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
|
||||
if set(result.curves) != measured:
|
||||
raise ValueError("L6 URDF input has the wrong measured curve set")
|
||||
if set(result.zero_offsets_rad) != CALIBRATED_ACTIVE_JOINTS:
|
||||
raise ValueError("L6 URDF input has the wrong zero-offset set")
|
||||
if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS:
|
||||
raise ValueError("L6 URDF input has the wrong travel set")
|
||||
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
|
||||
raise ValueError("L6 URDF input has the wrong mimic-fit set")
|
||||
payload: dict[str, Any] = {
|
||||
"schema_version": L6_URDF_INPUT_SCHEMA_VERSION,
|
||||
"artifact_type": L6_URDF_INPUT_ARTIFACT_TYPE,
|
||||
"profile_id": KEY.profile_id,
|
||||
"model": "L6",
|
||||
"side": "right",
|
||||
"serial_number": str(serial_number),
|
||||
"source_urdf_sha256": _sha256_file(source_urdf),
|
||||
"zero_offsets_rad": {
|
||||
name: float(result.zero_offsets_rad[name])
|
||||
for name in sorted(CALIBRATED_ACTIVE_JOINTS)
|
||||
},
|
||||
"travels_rad": {
|
||||
name: float(result.travels_rad[name])
|
||||
for name in sorted(CALIBRATED_ACTIVE_JOINTS)
|
||||
},
|
||||
"measured_curves": {
|
||||
name: {
|
||||
"angle_rad": [float(value) for value in result.curves[name].angle_rad],
|
||||
"decreasing_rad": [
|
||||
float(value) for value in result.curves[name].decreasing_rad
|
||||
],
|
||||
"increasing_rad": [
|
||||
float(value) for value in result.curves[name].increasing_rad
|
||||
],
|
||||
}
|
||||
for name in sorted(measured)
|
||||
},
|
||||
"mimic_fits": {
|
||||
name: {
|
||||
"source_joint": fit.source_joint,
|
||||
"target_joint": fit.target_joint,
|
||||
"model": fit.model,
|
||||
"coefficients": [float(value) for value in fit.coefficients],
|
||||
"urdf_mimic_multiplier": float(fit.urdf_mimic_multiplier),
|
||||
"urdf_mimic_policy": fit.urdf_mimic_policy,
|
||||
}
|
||||
for name in sorted(MEASURED_PASSIVE_JOINTS)
|
||||
for fit in (result.mimic_fits[name],)
|
||||
},
|
||||
}
|
||||
validate_l6_urdf_input_payload(payload)
|
||||
return payload
|
||||
|
||||
|
||||
def validate_l6_urdf_input_payload(payload: Mapping[str, Any]) -> None:
|
||||
required = {
|
||||
"schema_version", "artifact_type", "profile_id", "model", "side",
|
||||
"serial_number", "source_urdf_sha256", "zero_offsets_rad",
|
||||
"travels_rad", "measured_curves", "mimic_fits",
|
||||
}
|
||||
if set(payload) != required:
|
||||
raise ValueError("L6 URDF correction input has unexpected fields")
|
||||
if (
|
||||
payload.get("schema_version") != L6_URDF_INPUT_SCHEMA_VERSION
|
||||
or payload.get("artifact_type") != L6_URDF_INPUT_ARTIFACT_TYPE
|
||||
or payload.get("profile_id") != KEY.profile_id
|
||||
or payload.get("model") != "L6"
|
||||
or payload.get("side") != "right"
|
||||
):
|
||||
raise ValueError("L6 URDF correction input identity is invalid")
|
||||
source_hash = str(payload.get("source_urdf_sha256", ""))
|
||||
if len(source_hash) != 64 or any(
|
||||
char not in "0123456789abcdef" for char in source_hash
|
||||
):
|
||||
raise ValueError("L6 URDF correction input source hash is invalid")
|
||||
active = set(CALIBRATED_ACTIVE_JOINTS)
|
||||
measured = active | set(MEASURED_PASSIVE_JOINTS)
|
||||
zeros = payload.get("zero_offsets_rad")
|
||||
travels = payload.get("travels_rad")
|
||||
curves = payload.get("measured_curves")
|
||||
mimics = payload.get("mimic_fits")
|
||||
if not isinstance(zeros, Mapping) or set(zeros) != active:
|
||||
raise ValueError("L6 URDF correction input zero offsets are incomplete")
|
||||
if not isinstance(travels, Mapping) or set(travels) != active:
|
||||
raise ValueError("L6 URDF correction input travels are incomplete")
|
||||
if not isinstance(curves, Mapping) or set(curves) != measured:
|
||||
raise ValueError("L6 URDF correction input curves are incomplete")
|
||||
if not isinstance(mimics, Mapping) or set(mimics) != MEASURED_PASSIVE_JOINTS:
|
||||
raise ValueError("L6 URDF correction input mimic fits are incomplete")
|
||||
if any(not math.isfinite(float(value)) for value in zeros.values()):
|
||||
raise ValueError("L6 URDF correction input has a non-finite zero offset")
|
||||
if any(
|
||||
not math.isfinite(float(value)) or float(value) <= 0.0
|
||||
for value in travels.values()
|
||||
):
|
||||
raise ValueError("L6 URDF correction input has an invalid travel")
|
||||
for name, item in curves.items():
|
||||
if not isinstance(item, Mapping):
|
||||
raise ValueError(f"L6 URDF correction curve is invalid: {name}")
|
||||
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||
values = np.asarray(item.get(field), dtype=float)
|
||||
if values.shape != (256,) or not np.all(np.isfinite(values)):
|
||||
raise ValueError(
|
||||
f"L6 URDF correction curve is invalid: {name}.{field}"
|
||||
)
|
||||
for name, item in mimics.items():
|
||||
if not isinstance(item, Mapping):
|
||||
raise ValueError(f"L6 URDF correction mimic fit is invalid: {name}")
|
||||
if (
|
||||
item.get("target_joint") != name
|
||||
or item.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name]
|
||||
or item.get("model") != COUPLING_MODEL_BY_JOINT[name]
|
||||
):
|
||||
raise ValueError(f"L6 URDF correction mimic topology is invalid: {name}")
|
||||
coefficients = np.asarray(item.get("coefficients"), dtype=float)
|
||||
expected_count = 2 if item.get("model") == "quadratic_runtime" else 1
|
||||
if coefficients.shape != (expected_count,) or not np.all(
|
||||
np.isfinite(coefficients)
|
||||
):
|
||||
raise ValueError(f"L6 URDF correction coefficients are invalid: {name}")
|
||||
multiplier = float(item.get("urdf_mimic_multiplier", "nan"))
|
||||
if not math.isfinite(multiplier) or multiplier <= 0.0:
|
||||
raise ValueError(f"L6 URDF correction multiplier is invalid: {name}")
|
||||
|
||||
|
||||
def load_l6_urdf_input(
|
||||
path: str | Path,
|
||||
*,
|
||||
source_urdf: str | Path,
|
||||
serial_number: str,
|
||||
) -> L6FitResult:
|
||||
"""Load and authenticate the exact L6 fit used to materialize a URDF."""
|
||||
source = Path(path).expanduser().resolve()
|
||||
try:
|
||||
payload = json.loads(source.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as error:
|
||||
raise ValueError(f"could not read L6 URDF correction input: {source}") from error
|
||||
if not isinstance(payload, Mapping):
|
||||
raise ValueError("L6 URDF correction input must be a JSON object")
|
||||
validate_l6_urdf_input_payload(payload)
|
||||
if str(payload["serial_number"]) != str(serial_number):
|
||||
raise ValueError("L6 URDF correction input serial number differs")
|
||||
if str(payload["source_urdf_sha256"]) != _sha256_file(source_urdf):
|
||||
raise ValueError("L6 source URDF changed after calibration JSON was written")
|
||||
curves = {
|
||||
str(name): JointCurveFit(
|
||||
angle_rad=tuple(float(value) for value in item["angle_rad"]),
|
||||
decreasing_rad=tuple(
|
||||
float(value) for value in item["decreasing_rad"]
|
||||
),
|
||||
increasing_rad=tuple(
|
||||
float(value) for value in item["increasing_rad"]
|
||||
),
|
||||
circle={},
|
||||
maximum_monotonic_correction_rad=0.0,
|
||||
maximum_hysteresis_rad=0.0,
|
||||
quality={},
|
||||
)
|
||||
for name, item in payload["measured_curves"].items()
|
||||
}
|
||||
mimic_fits = {
|
||||
str(name): MimicFit(
|
||||
source_joint=str(item["source_joint"]),
|
||||
target_joint=str(item["target_joint"]),
|
||||
model=str(item["model"]),
|
||||
coefficients=tuple(float(value) for value in item["coefficients"]),
|
||||
urdf_mimic_multiplier=float(item["urdf_mimic_multiplier"]),
|
||||
urdf_mimic_policy=str(item["urdf_mimic_policy"]),
|
||||
cycle_coefficients=(),
|
||||
maximum_cycle_range=0.0,
|
||||
maximum_cycle_prediction_range_rad=0.0,
|
||||
residual_rms_rad=0.0,
|
||||
residual_p95_rad=0.0,
|
||||
residual_max_rad=0.0,
|
||||
)
|
||||
for name, item in payload["mimic_fits"].items()
|
||||
}
|
||||
return L6FitResult(
|
||||
curves=curves,
|
||||
zero_offsets_rad={
|
||||
str(name): float(value)
|
||||
for name, value in payload["zero_offsets_rad"].items()
|
||||
},
|
||||
travels_rad={
|
||||
str(name): float(value)
|
||||
for name, value in payload["travels_rad"].items()
|
||||
},
|
||||
mimic_fits=mimic_fits,
|
||||
holdout_errors_rad={},
|
||||
zero_method_by_joint={},
|
||||
zero_fallback_reason_by_joint={},
|
||||
thumb_zero_result=None,
|
||||
)
|
||||
|
||||
|
||||
def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
required_top = {
|
||||
"schema_version", "profile_id", "layout_id", "model", "side",
|
||||
@@ -518,7 +730,10 @@ __all__ = [
|
||||
"ALL_REVOLUTE_JOINTS",
|
||||
"artifact_hashes",
|
||||
"atomic_write_json",
|
||||
"build_l6_urdf_input_payload",
|
||||
"build_l6_runtime_payload",
|
||||
"load_l6_urdf_input",
|
||||
"publish_partial_session",
|
||||
"validate_l6_urdf_input_payload",
|
||||
"validate_l6_runtime_payload",
|
||||
]
|
||||
|
||||
@@ -12,6 +12,8 @@ from .artifacts import (
|
||||
artifact_hashes,
|
||||
atomic_write_json,
|
||||
build_l6_runtime_payload,
|
||||
build_l6_urdf_input_payload,
|
||||
load_l6_urdf_input,
|
||||
publish_partial_session,
|
||||
)
|
||||
from .fitting import L6FitResult, fit_l6_session
|
||||
@@ -156,12 +158,28 @@ def finalize_l6_session(
|
||||
protected_inputs=protected_inputs,
|
||||
passed=True,
|
||||
)
|
||||
urdf_input_path = (
|
||||
directory / f"l6_right_{serial_number}_urdf_correction_input.json"
|
||||
)
|
||||
atomic_write_json(
|
||||
urdf_input_path,
|
||||
build_l6_urdf_input_payload(
|
||||
serial_number=serial_number,
|
||||
source_urdf=source_urdf,
|
||||
result=result,
|
||||
),
|
||||
)
|
||||
urdf_result = load_l6_urdf_input(
|
||||
urdf_input_path,
|
||||
source_urdf=source_urdf,
|
||||
serial_number=serial_number,
|
||||
)
|
||||
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
correction = write_l6_corrected_urdf(
|
||||
source_urdf=source_urdf,
|
||||
output_directory=directory,
|
||||
serial_number=serial_number,
|
||||
result=result,
|
||||
result=urdf_result,
|
||||
timestamp=stamp,
|
||||
)
|
||||
json_path = directory / f"l6_right_{serial_number}_partial_calibration.json"
|
||||
|
||||
@@ -15,7 +15,10 @@ from linkerhand_calibration.calibrated_joint_state_bridge import (
|
||||
)
|
||||
from linkerhand_calibration.core import validate_profile
|
||||
from linkerhand_calibration.models.l6.artifacts import (
|
||||
atomic_write_json,
|
||||
build_l6_urdf_input_payload,
|
||||
build_l6_runtime_payload,
|
||||
load_l6_urdf_input,
|
||||
validate_l6_runtime_payload,
|
||||
)
|
||||
from linkerhand_calibration.models.l6.fitting import fit_l6_session
|
||||
@@ -796,6 +799,41 @@ def test_l6_online_and_offline_finalization_are_identical(tmp_path: Path) -> Non
|
||||
assert first_urdf.path.read_bytes() == second_urdf.path.read_bytes()
|
||||
|
||||
|
||||
def test_l6_json_handoff_preserves_existing_urdf_bytes(tmp_path: Path) -> None:
|
||||
records = _synthetic_records()
|
||||
result = fit_l6_session(SOURCE, accepted_records_by_joint(records))
|
||||
direct = write_l6_corrected_urdf(
|
||||
source_urdf=SOURCE,
|
||||
output_directory=tmp_path / "direct",
|
||||
serial_number="L6_JSON_HANDOFF",
|
||||
result=result,
|
||||
timestamp="20260902_140000",
|
||||
)
|
||||
input_path = tmp_path / "l6_urdf_correction_input.json"
|
||||
atomic_write_json(
|
||||
input_path,
|
||||
build_l6_urdf_input_payload(
|
||||
serial_number="L6_JSON_HANDOFF",
|
||||
source_urdf=SOURCE,
|
||||
result=result,
|
||||
),
|
||||
)
|
||||
reloaded = load_l6_urdf_input(
|
||||
input_path,
|
||||
source_urdf=SOURCE,
|
||||
serial_number="L6_JSON_HANDOFF",
|
||||
)
|
||||
via_json = write_l6_corrected_urdf(
|
||||
source_urdf=SOURCE,
|
||||
output_directory=tmp_path / "via_json",
|
||||
serial_number="L6_JSON_HANDOFF",
|
||||
result=reloaded,
|
||||
timestamp="20260902_140000",
|
||||
)
|
||||
|
||||
assert direct.path.read_bytes() == via_json.path.read_bytes()
|
||||
|
||||
|
||||
def test_l6_legacy_relative_only_session_cannot_publish_thumb_zero(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
|
||||
@@ -133,12 +133,19 @@ def test_finalize_publishes_the_frozen_validated_endpoint_state(
|
||||
"build_compact_payload",
|
||||
lambda **kwargs: {"passed": kwargs["passed"]},
|
||||
)
|
||||
def fake_atomic_write_json(path, payload):
|
||||
if str(path).endswith("_urdf_correction_input.json"):
|
||||
path.write_text(
|
||||
json.dumps(payload, ensure_ascii=False, indent=2) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
return
|
||||
captured.update({"final_path": path, "payload": payload})
|
||||
|
||||
monkeypatch.setattr(
|
||||
three_camera_node,
|
||||
"atomic_write_json",
|
||||
lambda path, payload: captured.update(
|
||||
{"final_path": path, "payload": payload}
|
||||
),
|
||||
fake_atomic_write_json,
|
||||
)
|
||||
node = SimpleNamespace(
|
||||
zero_result=zero_result,
|
||||
|
||||
@@ -44,6 +44,11 @@ from linkerhand_calibration.full_hand import (
|
||||
JointCurveFit,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from linkerhand_calibration.models.g20.urdf_input import (
|
||||
build_g20_urdf_input_payload,
|
||||
load_g20_urdf_input,
|
||||
)
|
||||
from linkerhand_calibration.storage import atomic_write_json
|
||||
|
||||
|
||||
PACKAGE_ROOT = Path(__file__).resolve().parents[1]
|
||||
@@ -552,6 +557,46 @@ def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Pa
|
||||
)
|
||||
|
||||
|
||||
def test_g20_json_handoff_preserves_existing_urdf_bytes(tmp_path: Path) -> None:
|
||||
offsets = {"thumb_cmc_yaw": math.radians(7.3)}
|
||||
direct = write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path / "direct",
|
||||
serial_number="G20_LEFT_JSON_HANDOFF",
|
||||
offsets_rad=offsets,
|
||||
timestamp="20260902_140001",
|
||||
)
|
||||
input_path = tmp_path / "g20_urdf_correction_input.json"
|
||||
atomic_write_json(
|
||||
input_path,
|
||||
build_g20_urdf_input_payload(
|
||||
side="left",
|
||||
layout_id="legacy_11",
|
||||
serial_number="G20_LEFT_JSON_HANDOFF",
|
||||
source_urdf=SOURCE_URDF,
|
||||
offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad={},
|
||||
),
|
||||
)
|
||||
loaded_offsets, loaded_endpoints = load_g20_urdf_input(
|
||||
input_path,
|
||||
source_urdf=SOURCE_URDF,
|
||||
side="left",
|
||||
layout_id="legacy_11",
|
||||
serial_number="G20_LEFT_JSON_HANDOFF",
|
||||
)
|
||||
via_json = write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path / "via_json",
|
||||
serial_number="G20_LEFT_JSON_HANDOFF",
|
||||
offsets_rad=loaded_offsets,
|
||||
endpoint_anchored_offsets_rad=loaded_endpoints,
|
||||
timestamp="20260902_140001",
|
||||
)
|
||||
|
||||
assert direct.read_bytes() == via_json.read_bytes()
|
||||
|
||||
|
||||
def test_urdf_writer_changes_only_the_16_active_zero_origins(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
|
||||
Reference in New Issue
Block a user