312 lines
12 KiB
Python
312 lines
12 KiB
Python
#!/usr/bin/env python3
|
|
"""Generate explicitly synthetic multi-part design examples, not manufacturer assets or runnable policies."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import copy
|
|
import json
|
|
from pathlib import Path
|
|
|
|
|
|
def joint(name, parent, child, joint_type="revolute", coupling=None):
|
|
linear = joint_type == "prismatic"
|
|
return {
|
|
"id": name,
|
|
"source_name": name,
|
|
"type": joint_type,
|
|
"parent": parent,
|
|
"child": child,
|
|
"origin": {"xyz": [0, 0, 0.1], "quat_xyzw": [0, 0, 0, 1]},
|
|
"actuation": "coupled" if coupling else "active",
|
|
"position_unit": "m" if linear else "rad",
|
|
"axis": [1, 0, 0] if linear else [0, 1, 0],
|
|
"limits_origin": "declared",
|
|
"limits": {
|
|
"position": None if joint_type == "continuous" else ([0, 0.04] if linear else [-1.5, 1.5]),
|
|
"velocity": 1 if linear else 10,
|
|
"effort": 10,
|
|
},
|
|
"coupling": coupling,
|
|
}
|
|
|
|
|
|
def part(part_type, count, mode="position_pd", joint_type="revolute"):
|
|
part_id = f"example_{part_type}"
|
|
joints = [joint(f"joint_{i}", "base" if i == 0 else f"link_{i - 1}", f"link_{i}", joint_type) for i in range(count)]
|
|
actuators = []
|
|
for item in joints:
|
|
linear = joint_type == "prismatic"
|
|
neutral = 0.02 if linear else 0.0
|
|
unit = ("m" if linear else "rad") if mode == "position_pd" else "rad/s"
|
|
actuators.append(
|
|
{
|
|
"id": "drive_" + item["id"],
|
|
"joint_id": item["id"],
|
|
"neutral_position": neutral,
|
|
"commands": {
|
|
mode: {
|
|
"unit": unit,
|
|
"scale": 0.01 if linear else 0.25,
|
|
"offset": neutral if mode == "position_pd" else 0,
|
|
"gains": {"kp": 25, "kd": 0.5} if mode == "position_pd" else None,
|
|
}
|
|
},
|
|
}
|
|
)
|
|
root = {"id": "root", "body": "base", "xyz": [0, 0, 0], "quat_xyzw": [0, 0, 0, 1]}
|
|
sensors = []
|
|
if joints:
|
|
sensors.append(
|
|
{
|
|
"id": "joint_state",
|
|
"kind": "joint_state",
|
|
"outputs": ["joint.position", "joint.velocity"],
|
|
"frame_id": "root",
|
|
"target": {"entity": "joint", "ids": [item["id"] for item in joints]},
|
|
"update_period_s": 0.002,
|
|
}
|
|
)
|
|
return {
|
|
"schema_version": 2,
|
|
"kind": "robot_part",
|
|
"part_id": part_id,
|
|
"part_type": part_type,
|
|
"description_role": "design_template",
|
|
"source": {
|
|
"format": "design",
|
|
"root": None,
|
|
"path": None,
|
|
"package_name": None,
|
|
"license_file": None,
|
|
"license_id": "LicenseRef-Design-Template",
|
|
"revision": None,
|
|
"sha256": None,
|
|
},
|
|
"topology": {
|
|
"structure": "tree",
|
|
"root_body": "base",
|
|
"bodies": ["base", *[item["child"] for item in joints]],
|
|
"joints": joints,
|
|
"constraints": [],
|
|
},
|
|
"frames": [root],
|
|
"root_frame": "root",
|
|
"attachment": {"mode": "fixed", "frame_id": "root", "source_mount_joint": None, "auxiliary_root_joints": []},
|
|
"actuators": actuators,
|
|
"end_effectors": [],
|
|
"sensors": sensors,
|
|
"timing": {"physics_dt_s": 0.002, "control_dt_s": 0.01},
|
|
"safety": {
|
|
"soft_limit_factor": 0.9,
|
|
"on_invalid_command": "reject",
|
|
"on_timeout": "disable",
|
|
"command_timeout_s": 0.05,
|
|
},
|
|
"scales": {"length_m": None, "known_mass_kg": None},
|
|
"extensions": {"skillfactory.example": {"synthetic": True, "not_manufacturer_data": True}},
|
|
}
|
|
|
|
|
|
def effector(data, name, body, role):
|
|
data["frames"].append({"id": name + "_frame", "body": body, "xyz": [0, 0, 0], "quat_xyzw": [0, 0, 0, 1]})
|
|
data["end_effectors"].append({"id": name, "role": role, "frame_id": name + "_frame", "contact": None})
|
|
|
|
|
|
def sensor(data, name, kind, outputs):
|
|
data["sensors"].append(
|
|
{
|
|
"id": name,
|
|
"kind": kind,
|
|
"outputs": outputs,
|
|
"frame_id": "root",
|
|
"target": {"entity": "part", "ids": [data["part_id"]]},
|
|
"update_period_s": 0.02,
|
|
}
|
|
)
|
|
|
|
|
|
def feature(name, quantity, unit="1", normalization="identity"):
|
|
return {"name": name, "quantity": quantity, "unit": unit, "normalization": normalization}
|
|
|
|
|
|
def port(name, entity, kind, shape, features=(), selector=None, required=True, mask=None, command=None):
|
|
return {
|
|
"name": name,
|
|
"direction": "output" if kind == "command" else "input",
|
|
"kind": kind,
|
|
"entity": entity,
|
|
"selector": selector or {"type": "all", "values": []},
|
|
"required": required,
|
|
"dtype": "bool" if kind == "mask" else ("uint8" if kind == "image" else "float32"),
|
|
"shape": shape,
|
|
"frame": "part_root" if kind == "goal" else "none",
|
|
"representation": "features" if features else "tensor",
|
|
"features": list(features),
|
|
"mask": mask,
|
|
"command": command,
|
|
}
|
|
|
|
|
|
def software(name, bindings, part_types, joint_types, mode=None):
|
|
return {
|
|
"schema_version": 2,
|
|
"kind": "part_software",
|
|
"software_id": name,
|
|
"software_version": "0.0.2",
|
|
"status": "design",
|
|
"binding_mode": "alternatives",
|
|
"hardware_bindings": [{"part_id": item, "spec_path": f"configs/parts/{item}.json"} for item in bindings],
|
|
"requirements": {
|
|
"part_types": part_types,
|
|
"joint_types": joint_types,
|
|
"topologies": ["tree"],
|
|
"attachment_modes": ["fixed", "external", "floating"],
|
|
"supports_coupling": True,
|
|
"required_control_modes": [mode] if mode else [],
|
|
"required_sensor_kinds": ["joint_state"] if mode else ["camera"],
|
|
"required_sensor_outputs": ["joint.position", "joint.velocity"] if mode else ["image.rgb8"],
|
|
"required_effector_roles": [],
|
|
"capacities": {"Jmax": 32, "Amax": 32, "Emax": 8, "Smax": 8},
|
|
},
|
|
"interface": {
|
|
"contract_id": name,
|
|
"contract_version": 1,
|
|
"quaternion_order": "xyzw",
|
|
"ports": [],
|
|
"reset": "indexed_isolated" if mode else "not_applicable",
|
|
"action_history": "last_applied" if mode else "none",
|
|
},
|
|
"runtime": {
|
|
"adapter_id": None,
|
|
"implementation": "unimplemented",
|
|
"dependencies": {},
|
|
"devices": ["cpu", "cuda"],
|
|
"timing": {"policy": "per_part", "control_dt_s": None},
|
|
},
|
|
"artifact": {"format": "none", "checkpoint": None, "trained_skill": False, "frozen": False},
|
|
}
|
|
|
|
|
|
def control_ports(mode, include_goal=False):
|
|
ports = [
|
|
port("joint_mask", "joint", "mask", ["B", "Jmax"]),
|
|
port(
|
|
"joint_state",
|
|
"joint",
|
|
"state",
|
|
["B", "Jmax", 2],
|
|
[
|
|
feature("position", "joint.position", "joint_position"),
|
|
feature("velocity", "joint.velocity", "joint_velocity"),
|
|
],
|
|
mask="joint_mask",
|
|
),
|
|
port("actuator_mask", "actuator", "mask", ["B", "Amax"]),
|
|
port(
|
|
"command",
|
|
"actuator",
|
|
"command",
|
|
["B", "Amax", 1],
|
|
[feature("command", "actuator.command", normalization="command_scale_offset")],
|
|
mask="actuator_mask",
|
|
command={"mode": mode, "encoding": "normalized", "clip": [-1, 1]},
|
|
),
|
|
]
|
|
if include_goal:
|
|
ports.extend(
|
|
[
|
|
port("effector_mask", "end_effector", "mask", ["B", "Emax"], required=False),
|
|
port(
|
|
"target_position",
|
|
"end_effector",
|
|
"goal",
|
|
["B", "Emax", 3],
|
|
[feature(axis, "goal.position." + axis, "m") for axis in ("x", "y", "z")],
|
|
required=False,
|
|
mask="effector_mask",
|
|
),
|
|
]
|
|
)
|
|
return ports
|
|
|
|
|
|
def build_examples():
|
|
arm, torso, head = part("arm", 6), part("torso", 2), part("head", 2)
|
|
effector(arm, "tool", "link_5", "tool")
|
|
effector(head, "gaze", "link_1", "gaze")
|
|
sensor(head, "camera", "camera", ["image.rgb8"])
|
|
sensor(head, "imu", "imu", ["imu.acceleration", "imu.angular_velocity"])
|
|
hand = part("hand", 4)
|
|
# Two independent fingers, each with a coupled distal joint: J=4, A=2, E=2.
|
|
hand["topology"]["joints"][2]["parent"] = "base"
|
|
for distal, driver in ((1, 0), (3, 2)):
|
|
item = hand["topology"]["joints"][distal]
|
|
item["actuation"] = "coupled"
|
|
item["coupling"] = {"joint_id": f"joint_{driver}", "multiplier": 0.5, "offset": 0}
|
|
hand["actuators"] = [item for item in hand["actuators"] if item["joint_id"] in ("joint_0", "joint_2")]
|
|
effector(hand, "finger_a", "link_1", "fingertip")
|
|
effector(hand, "finger_b", "link_3", "fingertip")
|
|
gripper = part("gripper", 2, joint_type="prismatic")
|
|
gripper["topology"]["joints"][1].update(
|
|
parent="base", actuation="coupled", coupling={"joint_id": "joint_0", "multiplier": -1, "offset": 0.04}
|
|
)
|
|
gripper["actuators"] = gripper["actuators"][:1]
|
|
effector(gripper, "jaw_a", "link_0", "grasp")
|
|
effector(gripper, "jaw_b", "link_1", "grasp")
|
|
base = part("mobile_base", 4, mode="velocity", joint_type="continuous")
|
|
base["attachment"]["mode"] = "floating"
|
|
for item in base["topology"]["joints"]:
|
|
item["parent"] = "base"
|
|
sensor(base, "imu", "imu", ["imu.acceleration", "imu.angular_velocity"])
|
|
pod = part("sensor", 0)
|
|
pod["attachment"]["mode"] = "external"
|
|
sensor(pod, "camera", "camera", ["image.rgb8"])
|
|
sensor(pod, "imu", "imu", ["imu.acceleration", "imu.angular_velocity"])
|
|
parts = [arm, hand, gripper, torso, head, base, pod]
|
|
outputs = {f"configs/parts/{data['part_id']}.json": data for data in parts}
|
|
pd = software(
|
|
"part_position_control",
|
|
["go2_fl", *[data["part_id"] for data in parts[:5]]],
|
|
["leg", "arm", "hand", "gripper", "torso", "head"],
|
|
["revolute", "prismatic"],
|
|
"position_pd",
|
|
)
|
|
pd["interface"]["ports"] = control_ports("position_pd", include_goal=True)
|
|
velocity = software("wheel_velocity_control", ["example_mobile_base"], ["mobile_base"], ["continuous"], "velocity")
|
|
velocity["interface"]["ports"] = control_ports("velocity")
|
|
vision = software("part_rgb_perception", ["example_head", "example_sensor"], ["head", "sensor"], ["revolute"])
|
|
# Capacities cover hardware, even when this profile doesn't command head motors or consume gaze.
|
|
vision["requirements"]["capacities"].update(Amax=2, Emax=1)
|
|
selector = {"type": "sensor_output", "values": ["image.rgb8"]}
|
|
vision["interface"]["ports"] = [
|
|
port("camera_mask", "sensor", "mask", ["B", "Smax"], selector=copy.deepcopy(selector)),
|
|
port("rgb_image", "sensor", "image", ["B", "Smax", 480, 640, 3], selector=selector, mask="camera_mask"),
|
|
]
|
|
for data in (pd, velocity, vision):
|
|
outputs[f"configs/software/{data['software_id']}.json"] = data
|
|
return outputs
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument(
|
|
"--output-root",
|
|
type=Path,
|
|
required=True,
|
|
help="Repository-shaped destination; existing files are never overwritten",
|
|
)
|
|
args = parser.parse_args()
|
|
outputs = build_examples()
|
|
paths = [(args.output_root / name, data) for name, data in outputs.items()]
|
|
if any(path.exists() for path, _ in paths):
|
|
parser.error("Refusing to overwrite existing descriptions")
|
|
for path, data in paths:
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
path.write_text(json.dumps(data, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
|
print(f"Generated {len(paths)} synthetic design/profile examples; none provides a runtime adapter")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|