Files
SkillFactory/scripts/assets/extract_go2_leg.py
T
chenlin 7f39abb613 release: SkillFactory v0.0.1
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
2026-10-10 09:58:30 +08:00

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()