#!/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()