77 lines
2.5 KiB
Python
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}
|
|
)
|