7f39abb613
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
261 lines
11 KiB
Python
261 lines
11 KiB
Python
#!/usr/bin/env python3
|
|
"""Extract the real Go2 FL subtree without importing Isaac Lab (SI units)."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import copy
|
|
import hashlib
|
|
import json
|
|
import math
|
|
import shutil
|
|
import subprocess
|
|
import xml.etree.ElementTree as ET
|
|
from pathlib import Path
|
|
|
|
ROOT = Path(__file__).resolve().parents[2]
|
|
DEFAULT_SOURCE = Path("/home/cen/Embodied_Workspace/unitree_ros/robots/go2_description")
|
|
JOINTS = ("FL_hip_joint", "FL_thigh_joint", "FL_calf_joint")
|
|
Q_DEFAULT = (0.0, 0.8, -1.5)
|
|
|
|
|
|
def digest(path: Path) -> str:
|
|
return hashlib.sha256(path.read_bytes()).hexdigest()
|
|
|
|
|
|
def numbers(text: str) -> list[float]:
|
|
result = [float(x) for x in text.split()]
|
|
if not all(math.isfinite(x) for x in result):
|
|
raise ValueError(f"Non-finite numeric field: {text}")
|
|
return result
|
|
|
|
|
|
def origin(element: ET.Element) -> dict:
|
|
value = element.find("origin")
|
|
return {key: numbers(value.get(key, "0 0 0") if value is not None else "0 0 0") for key in ("xyz", "rpy")}
|
|
|
|
|
|
def inertia_record(link: ET.Element) -> dict | None:
|
|
inertial = link.find("inertial")
|
|
if inertial is None:
|
|
return None
|
|
mass = float(inertial.find("mass").get("value"))
|
|
values = {k: float(inertial.find("inertia").get(k, "0")) for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")}
|
|
a, b, c, d, e, f = (values[k] for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz"))
|
|
# Sylvester's criterion for the complete symmetric inertia, not just its diagonal.
|
|
determinant = a * (d * f - e * e) - b * (b * f - c * e) + c * (b * e - c * d)
|
|
if (
|
|
not all(math.isfinite(x) for x in (mass, a, b, c, d, e, f))
|
|
or mass <= 0
|
|
or min(a, a * d - b * b, determinant) <= 0
|
|
):
|
|
raise ValueError(f"Invalid inertia on {link.get('name')}")
|
|
return {"mass": mass, "inertia": values, "origin": origin(inertial)}
|
|
|
|
|
|
def matmul(a, b):
|
|
return [[sum(a[i][k] * b[k][j] for k in range(len(b))) for j in range(len(b[0]))] for i in range(len(a))]
|
|
|
|
|
|
def transform(xyz, rpy):
|
|
r, p, y = 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 [
|
|
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, xyz[0]],
|
|
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, xyz[1]],
|
|
[-sp, cp * sr, cp * cr, xyz[2]],
|
|
[0, 0, 0, 1],
|
|
]
|
|
|
|
|
|
def foot_center(robot: ET.Element, q=Q_DEFAULT) -> list[float]:
|
|
"""Collision sphere center in the anchor frame at the specified joint angles [m]."""
|
|
t = transform([0, 0, 0], [0, 0, 0])
|
|
by_name = {j.get("name"): j for j in robot.findall("joint")}
|
|
for name in (*JOINTS, "FL_foot_joint"):
|
|
j = by_name[name]
|
|
o = origin(j)
|
|
t = matmul(t, transform(o["xyz"], o["rpy"]))
|
|
if name in JOINTS:
|
|
angle = q[JOINTS.index(name)]
|
|
axis = numbers(j.find("axis").get("xyz"))
|
|
# Go2 uses the canonical x and y axes. Refuse unhandled topologies.
|
|
if axis not in ([1.0, 0.0, 0.0], [0.0, 1.0, 0.0]):
|
|
raise ValueError(f"Unsupported axis {axis}")
|
|
t = matmul(t, transform([0, 0, 0], [angle * axis[0], angle * axis[1], 0]))
|
|
foot = next(link for link in robot.findall("link") if link.get("name") == "FL_foot")
|
|
o = origin(foot.find("collision"))
|
|
t = matmul(t, transform(o["xyz"], o["rpy"]))
|
|
return [t[i][3] for i in range(3)]
|
|
|
|
|
|
def extract(source: Path, output: Path) -> dict:
|
|
source = source.resolve()
|
|
source_urdf = source / "urdf/go2_description.urdf"
|
|
original = ET.parse(source_urdf).getroot()
|
|
links = {link.get("name"): link for link in original.findall("link")}
|
|
joints = {j.get("name"): j for j in original.findall("joint")}
|
|
for name in (*JOINTS, "FL_foot_joint", "FL_hip_rotor_joint"):
|
|
if name not in joints:
|
|
raise ValueError(f"Missing required joint: {name}")
|
|
keep = {"FL_hip", "FL_hip_rotor"}
|
|
while True:
|
|
expanded = keep | {j.find("child").get("link") for j in joints.values() if j.find("parent").get("link") in keep}
|
|
if expanded == keep:
|
|
break
|
|
keep = expanded
|
|
if "FL_foot" not in keep or any(not n.startswith("FL_") for n in keep):
|
|
raise ValueError("FL branch must contain only the FL leg and its independent foot")
|
|
robot = ET.Element("robot", name="go2_fl_anchor")
|
|
anchor = ET.SubElement(robot, "link", name="anchor_base")
|
|
anchor.append(
|
|
ET.fromstring(
|
|
'<inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="1"/>'
|
|
'<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/></inertial>'
|
|
)
|
|
)
|
|
shift = origin(joints[JOINTS[0]])["xyz"]
|
|
for link in original.findall("link"):
|
|
if link.get("name") in keep:
|
|
inertia_record(link)
|
|
robot.append(copy.deepcopy(link))
|
|
for j in original.findall("joint"):
|
|
if j.find("child").get("link") in keep:
|
|
j = copy.deepcopy(j)
|
|
if j.find("parent").get("link") == "base":
|
|
j.find("parent").set("link", "anchor_base")
|
|
old_xyz = origin(j)["xyz"]
|
|
j.find("origin").set("xyz", " ".join(f"{x - s:.12g}" for x, s in zip(old_xyz, shift)))
|
|
robot.append(j)
|
|
revolute = [j.get("name") for j in robot.findall("joint") if j.get("type") != "fixed"]
|
|
if tuple(revolute) != JOINTS:
|
|
raise ValueError(f"Expected exactly the three ordered joints, got {revolute}")
|
|
output.mkdir(parents=True, exist_ok=True)
|
|
resources = {}
|
|
for mesh in robot.findall(".//mesh"):
|
|
filename = mesh.get("filename")
|
|
prefix = "package://go2_description/"
|
|
if not filename.startswith(prefix):
|
|
raise ValueError(f"Unexpected mesh URI: {filename}")
|
|
relative = Path(filename[len(prefix) :])
|
|
if relative.is_absolute() or ".." in relative.parts:
|
|
raise ValueError(f"Unsafe mesh path: {relative}")
|
|
src, dst = source / relative, output / relative
|
|
dst.parent.mkdir(parents=True, exist_ok=True)
|
|
shutil.copy2(src, dst)
|
|
mesh.set("filename", relative.as_posix())
|
|
resources[relative.as_posix()] = digest(src)
|
|
if src.suffix.lower() == ".dae":
|
|
for image in ET.parse(src).getroot().findall(".//{*}image/{*}init_from"):
|
|
image_relative = Path(image.text.strip())
|
|
if image_relative.is_absolute() or ".." in image_relative.parts:
|
|
raise ValueError("Unsafe DAE image reference")
|
|
image_src, image_dst = src.parent / image_relative, dst.parent / image_relative
|
|
image_dst.parent.mkdir(parents=True, exist_ok=True)
|
|
shutil.copy2(image_src, image_dst)
|
|
resources[image_dst.relative_to(output).as_posix()] = digest(image_src)
|
|
license_path = source.parents[1] / "LICENSE"
|
|
shutil.copy2(license_path, output / "LICENSE")
|
|
records = []
|
|
for name in JOINTS:
|
|
j = joints[name]
|
|
limits = {k: float(j.find("limit").get(k)) for k in ("lower", "upper", "effort", "velocity")}
|
|
if (
|
|
not all(math.isfinite(x) for x in limits.values())
|
|
or limits["lower"] >= limits["upper"]
|
|
or min(limits["effort"], limits["velocity"]) <= 0
|
|
):
|
|
raise ValueError(f"Invalid limits: {name}")
|
|
midpoint = (limits["lower"] + limits["upper"]) / 2
|
|
records.append(
|
|
{
|
|
"name": name,
|
|
"axis": numbers(j.find("axis").get("xyz")),
|
|
"limits": limits,
|
|
"soft_limits": [midpoint + 0.9 * (limits[k] - midpoint) for k in ("lower", "upper")],
|
|
}
|
|
)
|
|
radius = float(links["FL_foot"].find("collision/geometry/sphere").get("radius"))
|
|
center = foot_center(robot)
|
|
manifest = {
|
|
"schema_version": 1,
|
|
"asset": "Go2 FL fixed anchor",
|
|
"source": {
|
|
"urdf": str(source_urdf),
|
|
"sha256": digest(source_urdf),
|
|
"commit": subprocess.check_output(["git", "-C", str(source), "rev-parse", "HEAD"], text=True).strip(),
|
|
},
|
|
"resources": dict(sorted(resources.items())),
|
|
"license": "BSD-3-Clause",
|
|
"joint_order": list(JOINTS),
|
|
"joints": records,
|
|
"links": {link.get("name"): inertia_record(link) for link in robot.findall("link")},
|
|
"transforms": {
|
|
j.get("name"): {"parent": j.find("parent").get("link"), "child": j.find("child").get("link"), **origin(j)}
|
|
for j in robot.findall("joint")
|
|
},
|
|
"missing_inertial_fixed_links": [
|
|
link.get("name") for link in robot.findall("link") if link.find("inertial") is None
|
|
],
|
|
"anchor": {
|
|
"fixed": True,
|
|
"original_mount_translation": shift,
|
|
"synthetic_root_mass": 1.0,
|
|
"default_height": radius - center[2] + 0.01,
|
|
},
|
|
"foot": {"frame": "FL_foot", "radius": radius, "collision_origin": origin(links["FL_foot"].find("collision"))},
|
|
"control": {
|
|
"q_default": list(Q_DEFAULT),
|
|
"action_scale": 0.25,
|
|
"kp": 25.0,
|
|
"kd": 0.5,
|
|
"physics_dt": 0.002,
|
|
"decimation": 5,
|
|
},
|
|
"interface": {
|
|
"action_shape": ["B", 3],
|
|
"policy_shape": ["B", 19],
|
|
"device": "cuda",
|
|
"dtype": "float32",
|
|
"quaternion_order": "xyzw",
|
|
"policy_order": [
|
|
"q_rel",
|
|
"dq",
|
|
"foot_pos_anchor",
|
|
"foot_vel_anchor",
|
|
"foot_normal_force_anchor",
|
|
"contact",
|
|
"last_action",
|
|
],
|
|
"force_semantics": "normal contact force, not full friction wrench",
|
|
},
|
|
"skill": {"weights": None, "latent": None, "reaction_wrench": None, "valid": False},
|
|
"modifications": [
|
|
"Replace whole body with fixed synthetic anchor root",
|
|
"Translate mount origin to anchor origin",
|
|
"Preserve FL fixed subtrees and root-side rotor",
|
|
"Relative mesh URIs; no ROS runtime dependency",
|
|
],
|
|
}
|
|
ET.indent(robot)
|
|
ET.ElementTree(robot).write(output / "go2_fl.urdf", encoding="utf-8", xml_declaration=True)
|
|
manifest["derived_urdf_sha256"] = digest(output / "go2_fl.urdf")
|
|
(output / "manifest.json").write_text(json.dumps(manifest, indent=2, sort_keys=True) + "\n")
|
|
return manifest
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--source", type=Path, default=DEFAULT_SOURCE)
|
|
parser.add_argument("--output", type=Path, default=ROOT / "assets/generated/go2_fl")
|
|
args = parser.parse_args()
|
|
manifest = extract(args.source, args.output)
|
|
print(
|
|
f"Extracted {manifest['joint_order']} to {args.output}; "
|
|
f"anchor height={manifest['anchor']['default_height']:.6f} m"
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|