Files
SkillFactory/scripts/migrate_leg_description.py
T
2026-10-10 16:34:40 +08:00

187 lines
7.1 KiB
Python

#!/usr/bin/env python3
"""Export a validated legacy leg bundle as PartSpec v2; this does not create a new runtime adapter."""
from __future__ import annotations
import argparse
import json
import math
from pathlib import Path
def _quaternion(rpy):
r, p, y = (angle / 2 for angle in rpy)
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
return [
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy,
]
def part_from_leg_bundle(bundle):
"""Keep all bodies/fixed branches and the independent actuator declaration order."""
if bundle.get("schema_version") != 2 or bundle.get("kind") != "single_leg":
raise ValueError("Expected legacy single_leg asset manifest v2")
spec = bundle["spec"]
records = {joint["name"]: joint for joint in bundle["joints"]}
controls = {joint["name"]: joint for joint in spec["joints"]}
if bundle["joint_order"] != list(controls) or set(records) != set(controls):
raise ValueError("Legacy controlled-joint order mismatch")
joints = []
for node in bundle["tree"]:
fixed = node["type"] == "fixed"
limits = records[node["name"]]["limits"] if not fixed else None
joints.append(
{
"id": node["name"],
"source_name": node["name"],
"type": node["type"],
"parent": node["parent"],
"child": node["child"],
"origin": {"xyz": node["xyz"], "quat_xyzw": _quaternion(node["rpy"])},
"actuation": "fixed" if fixed else "active",
"position_unit": None if fixed else "rad",
"axis": None if fixed else node["axis"],
"limits_origin": "source",
"limits": None
if fixed
else {
"position": [limits["lower"], limits["upper"]],
"velocity": limits["velocity"],
"effort": limits["effort"],
},
"coupling": None,
}
)
dt = spec["physics_dt"] * spec["decimation"]
part = {
"schema_version": 2,
"kind": "robot_part",
"part_id": spec["part_id"],
"part_type": "leg",
"description_role": "asset_reference",
"source": {
"format": "urdf",
"root": spec["source_root"],
"path": spec["urdf"],
"package_name": spec["package_name"],
"license_file": spec["license_file"],
"license_id": spec["license_id"],
"revision": bundle["source"]["commit"],
"sha256": bundle["source"]["sha256"],
},
"topology": {
"structure": "tree",
"root_body": bundle["anchor_body"],
"bodies": [bundle["anchor_body"], *bundle["links"]],
"joints": joints,
"constraints": [],
},
"frames": [
{"id": "root", "body": bundle["anchor_body"], "xyz": [0, 0, 0], "quat_xyzw": [0, 0, 0, 1]},
{"id": "foot_link", "body": bundle["foot"]["body"], "xyz": [0, 0, 0], "quat_xyzw": [0, 0, 0, 1]},
],
"root_frame": "root",
"attachment": {
"mode": "fixed",
"frame_id": "root",
"source_mount_joint": spec["mount_joint"],
"auxiliary_root_joints": spec["auxiliary_root_joints"],
"source_parent_transform": {
"body": bundle["mount"]["source_parent"],
"xyz": bundle["mount"]["source_parent_T_anchor"]["xyz"],
"quat_xyzw": _quaternion(bundle["mount"]["source_parent_T_anchor"]["rpy"]),
},
},
"actuators": [
{
"id": "drive_" + control["name"],
"joint_id": control["name"],
"neutral_position": control["q_default"],
"commands": {
"position_pd": {
"unit": "rad",
"scale": control["action_scale"],
"offset": control["q_default"],
"gains": {"kp": control["kp"], "kd": control["kd"]},
}
},
}
for control in spec["joints"]
],
"end_effectors": [
{
"id": "foot",
"role": "support",
"frame_id": "foot_link",
"contact": {
"source_collision_index": spec["foot_collision_index"],
"gap_method": bundle["foot"]["type"],
},
}
],
"sensors": [
{
"id": "joint_state",
"kind": "joint_state",
"outputs": ["joint.position", "joint.velocity"],
"frame_id": "root",
"target": {"entity": "joint", "ids": bundle["joint_order"]},
"update_period_s": spec["physics_dt"],
},
{
"id": "foot_contact",
"kind": "contact_normal",
"outputs": ["contact.normal_force"],
"frame_id": "foot_link",
"target": {"entity": "end_effector", "ids": ["foot"]},
"update_period_s": spec["physics_dt"],
},
],
"timing": {"physics_dt_s": spec["physics_dt"], "control_dt_s": dt},
"safety": {
"soft_limit_factor": spec["soft_limit_factor"],
"on_invalid_command": "reject",
"on_timeout": "hold",
"command_timeout_s": 5 * dt,
},
"scales": {"length_m": bundle["length_scale_m"], "known_mass_kg": bundle["known_mass_scale_kg"]},
"extensions": {
"skillfactory.legacy": {
"profile": "single_leg_v1",
"derived_urdf_sha256": bundle["derived_urdf_sha256"],
"watchdog_policy": "v2 requirement only; not implemented by the legacy runtime",
}
},
}
return part
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--bundle", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
args = parser.parse_args()
from SkillFactory.legs.spec import load_bundle
from validate_descriptions import validate_hardware
output = args.output.resolve()
if output.exists() or output.is_relative_to(args.bundle.resolve()):
parser.error("Output must be a new file outside the legacy bundle")
part = part_from_leg_bundle(load_bundle(args.bundle))
output.parent.mkdir(parents=True, exist_ok=True)
temporary = output.with_suffix(output.suffix + ".tmp")
try:
temporary.write_text(json.dumps(part, indent=2, allow_nan=False) + "\n", encoding="utf-8")
validate_hardware(temporary)
temporary.replace(output)
finally:
temporary.unlink(missing_ok=True)
print(f"PartSpec v2 exported: {output}; legacy simulation/training remains unchanged")
if __name__ == "__main__":
main()