187 lines
7.1 KiB
Python
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()
|