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