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.
62 lines
2.9 KiB
Python
62 lines
2.9 KiB
Python
"""Authored controls are replaced by calibration using the supported MjSpec API."""
|
|
|
|
from dataclasses import replace
|
|
from xml.etree import ElementTree as ET
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
mujoco = pytest.importorskip("mujoco")
|
|
pytest.importorskip("mjlab")
|
|
|
|
from tasks.templates._locomotion import _load_model
|
|
from tasks.templates.Biped_Template.env_cfg import env_cfg
|
|
|
|
|
|
def test_authored_actuators_and_keys_are_replaced_without_modifying_source(neutral_robot, tmp_path):
|
|
source = ET.parse(neutral_robot.model_path)
|
|
actuators = ET.SubElement(source.getroot(), "actuator")
|
|
for index in range(2):
|
|
ET.SubElement(actuators, "position", name=f"authored_{index}", joint=f"leg_{index}",
|
|
kp="99", kv="9", forcerange="-90 90")
|
|
keys = ET.SubElement(source.getroot(), "keyframe")
|
|
for index in range(2):
|
|
ET.SubElement(keys, "key", name=f"authored_pose_{index}",
|
|
qpos="0 0 .4 1 0 0 0 .5 -.5", ctrl=".5 -.5")
|
|
path = tmp_path / "authored.xml"
|
|
source.write(path)
|
|
original_bytes = path.read_bytes()
|
|
authored = mujoco.MjModel.from_xml_path(str(path))
|
|
assert authored.nu == authored.nkey == 2 # Both removal loops must do work.
|
|
np.testing.assert_array_equal(authored.actuator_gainprm[:, 0], [99, 99])
|
|
|
|
joints = tuple(replace(joint, kp=11 + i, kd=.7 + i, effort_limit=8 + i,
|
|
home=.1 if i == 0 else -.2, armature=.01 + .01 * i)
|
|
for i, joint in enumerate(neutral_robot.position_joints))
|
|
robot = replace(neutral_robot, model_path=path, position_joints=joints)
|
|
spec, _, homes = _load_model(path, robot)
|
|
stripped = spec.compile()
|
|
assert stripped.nu == stripped.nkey == 0
|
|
assert homes == {joint.name: joint.home for joint in joints}
|
|
np.testing.assert_array_equal(stripped.body_mass, authored.body_mass)
|
|
np.testing.assert_array_equal(stripped.jnt_range, authored.jnt_range)
|
|
|
|
entity = env_cfg(robot).scene.entities["robot"].build()
|
|
calibrated = entity.spec.compile()
|
|
assert calibrated.nu == 2
|
|
assert calibrated.nkey == 1
|
|
assert calibrated.key(0).name == "init_state"
|
|
for index, (joint, actuator) in enumerate(zip(joints, entity.actuators, strict=True)):
|
|
assert actuator.cfg.stiffness == joint.kp
|
|
assert actuator.cfg.damping == joint.kd
|
|
assert actuator.cfg.effort_limit == joint.effort_limit
|
|
np.testing.assert_array_equal(calibrated.actuator_forcerange[index],
|
|
[-joint.effort_limit, joint.effort_limit])
|
|
jid = calibrated.joint(joint.name).id
|
|
assert calibrated.dof_armature[calibrated.jnt_dofadr[jid]] == joint.armature
|
|
assert calibrated.key("init_state").qpos[calibrated.jnt_qposadr[jid]] == joint.home
|
|
assert path.read_bytes() == original_bytes
|
|
reread = mujoco.MjModel.from_xml_path(str(path))
|
|
assert reread.nu == reread.nkey == 2
|
|
np.testing.assert_array_equal(reread.actuator_gainprm, authored.actuator_gainprm)
|