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.
71 lines
2.4 KiB
Python
71 lines
2.4 KiB
Python
"""Preserve floating point precision when the native XML writer supports it."""
|
|
from __future__ import annotations
|
|
|
|
import ctypes
|
|
import xml.etree.ElementTree as ET
|
|
from threading import RLock
|
|
|
|
_XML_LOCK = RLock()
|
|
|
|
|
|
def _precision_api():
|
|
import mujoco
|
|
|
|
# MuJoCo exposes these private hooks through its shared library, but not
|
|
# through Python. Keep the optional dependency isolated: older builds use
|
|
# their ordinary writer and callers' unchanged round-trip checks still apply.
|
|
try:
|
|
library = ctypes.CDLL(mujoco._specs.__file__)
|
|
get = library._mjPRIVATE__get_xml_precision
|
|
set_ = library._mjPRIVATE__set_xml_precision
|
|
except (OSError, AttributeError):
|
|
return None
|
|
get.argtypes, get.restype = [], ctypes.c_int
|
|
set_.argtypes, set_.restype = [ctypes.c_int], None
|
|
return get, set_
|
|
|
|
|
|
def spec_xml(spec) -> str:
|
|
"""Use double precision XML and restore native writer state even on error."""
|
|
api = _precision_api()
|
|
if api is None:
|
|
return spec.to_xml()
|
|
get, set_ = api
|
|
with _XML_LOCK:
|
|
previous = get()
|
|
try:
|
|
set_(17)
|
|
return spec.to_xml()
|
|
finally:
|
|
set_(previous)
|
|
|
|
|
|
def temporary_object_names(spec) -> set[str]:
|
|
"""Give anonymous bodies/geoms stable identities for round-trip checks."""
|
|
temporary_names = set()
|
|
used_names = {item.name for item in [*spec.bodies, *spec.geoms]}
|
|
for index, item in enumerate([*spec.bodies, *spec.geoms]):
|
|
if item is spec.worldbody or item.name:
|
|
continue
|
|
name = f"__mjrl_roundtrip_{index}"
|
|
while name in used_names:
|
|
name += "_"
|
|
item.name = name
|
|
temporary_names.add(name)
|
|
used_names.add(name)
|
|
return temporary_names
|
|
|
|
|
|
def named_spec_tree(spec) -> ET.Element:
|
|
"""Serialize a spec whose geoms have stable names for round-trip checks."""
|
|
root = ET.fromstring(spec_xml(spec))
|
|
# MuJoCo can elide both mass and density for a zero-mass geom when its
|
|
# class supplies both as zero. On reload it then acquires the built-in
|
|
# density. Use the effective spec mass, including class inheritance.
|
|
massless = {geom.name for geom in spec.geoms if geom.mass == 0}
|
|
if massless:
|
|
for element in root.iterfind("./worldbody//geom"):
|
|
if element.get("name") in massless:
|
|
element.set("mass", "0")
|
|
return root
|