Files
UniversalTrain-ai/tests/test_calibration_inertia.py
chenlin c32df70326
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
release: v0.1 generic robot training and application platform
2026-11-05 11:05:01 +08:00

77 lines
2.5 KiB
Python

"""A floating chain can move while a joint is driven."""
from __future__ import annotations
import numpy as np
import pytest
mujoco = pytest.importorskip("mujoco")
from mjrl.onboarding.calibrate import _actuator_parameters
_XML = """
<mujoco>
<worldbody>
<body name="base">
<freejoint/>
<geom type="box" size=".1 .1 .05" mass="2"/>
<body name="upper">
<joint name="hip" axis="0 1 0"/>
<geom type="capsule" fromto="0 0 0 0 0 -.2" size=".02" mass=".25"/>
<body name="lower" pos="0 0 -.2">
<joint name="ankle" axis="0 1 0"/>
<geom type="capsule" fromto="0 0 0 0 0 -.2" size=".02" mass=".2"/>
</body>
</body>
</body>
</worldbody>
</mujoco>
"""
def test_inertia_modes_preserve_default_and_measure_free_coupling() -> None:
model = mujoco.MjModel.from_xml_string(_XML)
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
joints = {name: model.joint(name).id for name in ("hip", "ankle")}
dofs = model.jnt_dofadr[list(joints.values())]
mass = np.zeros((model.nv, model.nv))
mujoco.mj_fullM(model, data, mass)
config = {
"effort_limits": {"hip": 5.0, "ankle": 2.0},
"frequency_hz": 3,
"joint_frequency_hz": {"ankle": 6},
"damping_ratio": 0.8,
}
kp_default, _, _, diag = _actuator_parameters(model, data, joints, config)
kp, kd, limits, coupled = _actuator_parameters(
model, data, joints, {**config, "inertia_mode": "coupled"}
)
assert diag == pytest.approx(np.diag(mass)[dofs])
assert coupled == pytest.approx(1 / np.diag(np.linalg.inv(mass))[dofs])
assert np.all(coupled < diag)
omega = 2 * np.pi * np.array([3, 6])
assert kp_default == pytest.approx(diag * omega**2)
assert kp == pytest.approx(coupled * omega**2)
assert kd == pytest.approx(1.6 * coupled * omega)
assert limits.tolist() == [5, 2]
@pytest.mark.parametrize(
"changes",
[
{"inertia_mode": "guess"},
{"joint_frequency_hz": {"other_joint": 5}},
{"joint_frequency_hz": {"hip": 0}},
],
)
def test_invalid_inertia_gain_configuration_fails(changes: dict) -> None:
model = mujoco.MjModel.from_xml_string(_XML)
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
joints = {name: model.joint(name).id for name in ("hip", "ankle")}
with pytest.raises(ValueError):
_actuator_parameters(
model, data, joints, {"effort_limits": {"hip": 5.0, "ankle": 2.0}, **changes}
)