ceca51b0d8
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
141 lines
7.0 KiB
Python
141 lines
7.0 KiB
Python
"""Flatten and relocate a model using MuJoCo's own parser, preserving its physics."""
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
from pathlib import Path
|
|
import xml.etree.ElementTree as ET
|
|
|
|
|
|
def copy_model(source: Path, output: Path) -> dict[str, str]:
|
|
"""Copy model dependencies by content hash; no absolute paths survive.
|
|
|
|
Includes are flattened by MjSpec. Mesh/texture directories are resolved from
|
|
the parsed spec, rather than guessed from basenames. Missing assets fail.
|
|
"""
|
|
import mujoco
|
|
from mjrl.onboarding._xml import checked_mjcf
|
|
|
|
source, output = Path(source).resolve(), Path(output)
|
|
authored = checked_mjcf(source)
|
|
from ._serialization import named_spec_tree, temporary_object_names
|
|
|
|
spec = mujoco.MjSpec.from_file(str(source))
|
|
authored_compiler = authored.find("compiler")
|
|
# An already flattened export needs only dependency relocation. Feeding it
|
|
# through MjSpec's writer recomposes mesh frames and renormalizes rotations,
|
|
# changing a few ulps that can split long contact-rich reference rollouts.
|
|
# Preserve its exact numeric attribute text, including inherited defaults.
|
|
direct = not any(element.tag in {"include", "model", "attach"}
|
|
for element in authored.iter()) and (
|
|
authored_compiler is None or not any(
|
|
authored_compiler.get(key) for key in ("assetdir", "meshdir", "texturedir", "strippath")
|
|
)
|
|
)
|
|
temporary_names = set()
|
|
if direct:
|
|
before = mujoco.MjModel.from_xml_path(str(source))
|
|
root = authored
|
|
fuse_static = spec.compiler.fusestatic
|
|
else:
|
|
temporary_names = temporary_object_names(spec)
|
|
before = spec.copy().compile()
|
|
# Compiling a fused spec mutates fixed-child geometry before serialization.
|
|
# Serialize the unfused tree and let the relocated model apply the original
|
|
# compiler setting, preserving both transforms and authored semantics.
|
|
fuse_static = spec.compiler.fusestatic
|
|
spec.compiler.fusestatic = False
|
|
spec.compile()
|
|
root = named_spec_tree(spec)
|
|
if root.find("extension") is not None:
|
|
raise ValueError("plugin models require an explicit packaged plugin runtime")
|
|
compiler = root.find("compiler")
|
|
if compiler is None:
|
|
compiler = ET.SubElement(root, "compiler")
|
|
base = Path(spec.modelfiledir or source.parent)
|
|
meshdir = Path(spec.meshdir or ".")
|
|
texturedir = Path(spec.texturedir or ".")
|
|
output.parent.mkdir(parents=True, exist_ok=True)
|
|
dependencies = {}
|
|
for element in root.iter():
|
|
for key in ("file", "fileup", "filedown", "fileleft", "fileright", "filefront", "fileback"):
|
|
value = element.get(key)
|
|
if not value:
|
|
continue
|
|
folder = meshdir if element.tag in {"mesh", "hfield", "skin"} else texturedir if element.tag == "texture" else Path(".")
|
|
path = Path(value)
|
|
path = path if path.is_absolute() else base / folder / path
|
|
if not path.is_file():
|
|
raise ValueError(f"missing {element.tag} dependency: {path}")
|
|
raw = path.read_bytes()
|
|
digest = hashlib.sha256(raw).hexdigest()
|
|
directory = "meshes" if element.tag == "mesh" else "textures" if element.tag == "texture" else "data"
|
|
relative = f"{directory}/{digest}{path.suffix.lower()}"
|
|
target = output.parent / relative
|
|
target.parent.mkdir(exist_ok=True)
|
|
target.write_bytes(raw)
|
|
element.set(key, relative)
|
|
dependencies[str(path)] = relative
|
|
for key in ("assetdir", "meshdir", "texturedir"):
|
|
compiler.attrib.pop(key, None)
|
|
if not direct:
|
|
compiler.set("fusestatic", "true" if fuse_static else "false")
|
|
ET.indent(root)
|
|
ET.ElementTree(root).write(output, encoding="utf-8", xml_declaration=True)
|
|
after = mujoco.MjModel.from_xml_path(str(output))
|
|
import numpy as np
|
|
for field in ("nq", "nv", "nu", "nbody", "ngeom", "njnt"):
|
|
if getattr(before, field) != getattr(after, field):
|
|
raise ValueError(f"relocating model changed {field}")
|
|
geom_order = (list(range(before.ngeom)) if direct else
|
|
[after.geom(before.geom(gid).name).id for gid in range(before.ngeom)])
|
|
for field in (
|
|
"body_mass", "body_inertia", "body_pos", "body_quat", "body_ipos", "body_iquat",
|
|
"jnt_range", "jnt_pos", "jnt_axis", "jnt_stiffness", "qpos0",
|
|
"dof_damping", "dof_armature", "geom_pos", "geom_quat", "geom_size",
|
|
"geom_contype", "geom_conaffinity", "geom_friction", "geom_priority",
|
|
"geom_solref", "geom_solimp", "actuator_trnid", "actuator_gear",
|
|
"actuator_gainprm", "actuator_biasprm", "actuator_dynprm",
|
|
"actuator_ctrlrange", "actuator_forcerange",
|
|
):
|
|
actual = getattr(after, field)
|
|
if field.startswith("geom_"):
|
|
actual = actual[geom_order]
|
|
if not np.allclose(getattr(before, field), actual, rtol=1e-7, atol=1e-10):
|
|
raise ValueError(f"relocating model changed {field}")
|
|
if direct and not np.array_equal(getattr(before, field), actual):
|
|
raise ValueError(f"relocating flattened model changed exact {field}")
|
|
if temporary_names:
|
|
for element in root.iter():
|
|
if element.tag in {"body", "geom"} and element.get("name") in temporary_names:
|
|
del element.attrib["name"]
|
|
ET.ElementTree(root).write(output, encoding="utf-8", xml_declaration=True)
|
|
return dependencies
|
|
|
|
|
|
def validate_contained_model(root: Path, source: Path) -> None:
|
|
"""A normalized app model can read only listed local assets, never plugins."""
|
|
from .bundle import safe_path
|
|
|
|
text = source.read_text("utf-8-sig")
|
|
import re
|
|
if re.search(r"<!\s*(DOCTYPE|ENTITY)\b", text, re.IGNORECASE):
|
|
raise ValueError("model entity declarations are not allowed in an app")
|
|
xml = ET.fromstring(text)
|
|
if xml.tag != "mujoco":
|
|
raise ValueError("application model must be normalized MJCF")
|
|
compiler = xml.find("compiler")
|
|
if compiler is not None and any(compiler.get(key) for key in ("assetdir", "meshdir", "texturedir")):
|
|
raise ValueError("normalized application models cannot use compiler asset directories")
|
|
for element in xml.iter():
|
|
if element.tag in {"plugin", "extension", "include", "model", "attach"} or "plugin" in element.attrib:
|
|
raise ValueError("application models must be flattened and cannot load plugins")
|
|
for key, value in element.attrib.items():
|
|
if key not in {"file", "fileup", "filedown", "fileleft", "fileright", "filefront", "fileback"}:
|
|
continue
|
|
# Validate each component before joining: a later absolute component
|
|
# must never discard the prefix chosen by the application.
|
|
safe_path(root, value)
|
|
target = source.parent / value
|
|
if not target.resolve().is_relative_to(root.resolve()) or not target.is_file():
|
|
raise ValueError(f"missing or external model dependency: {value}")
|