Files
UniversalTrain-ai/tests/test_controller_named_gains.py
chenlin ceca51b0d8
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
Improve generic robot onboarding, walking controls and end-effector interaction
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.
2026-11-06 10:54:58 +08:00

114 lines
5.0 KiB
Python

"""Named heterogeneous servos retain physical limits and calibrated operator rest."""
import json
import numpy as np
import pytest
mujoco = pytest.importorskip("mujoco")
from mjrl.controllers import load_controller, setup_controller
XML = """<mujoco>
<compiler angle="radian"/>
<option timestep=".002" gravity="0 0 0" integrator="implicitfast"/>
<worldbody><body name="leg_body">
<joint name="leg" type="slide" axis="1 0 0" range="-.2 .2" actuatorfrcrange="-5 4"/>
<geom size=".02" mass="1" contype="0" conaffinity="0"/>
<body name="arm_body"><joint name="arm" type="slide" axis="0 1 0" range="-.2 .2" actuatorfrcrange="-3 5"/>
<geom size=".02" mass="1" contype="0" conaffinity="0"/>
</body></body></worldbody>
<actuator>
<motor name="arm_motor" joint="arm" gear="-2" forcerange="-4 3" ctrlrange="-1 2"/>
<position name="leg_servo" joint="leg" kp="10" ctrlrange="-.2 .2" forcerange="-8 12"/>
</actuator></mujoco>"""
GAINS = {"kp": {"arm": 30, "leg": 70}, "kd": {"arm": 3, "leg": 2},
"effort_limit": {"arm": 100, "leg": 6}}
def setup(tmp_path, **extra):
source = tmp_path / "source.xml"
source.write_text(XML)
config = {"kind": "joint_sliders", "joints": ["leg", "arm"], **GAINS, **extra}
result = setup_controller(source, config, tmp_path / "controller")
model = mujoco.MjModel.from_xml_path(str(result.model))
controller = load_controller(model, result.config, result.controls)
return result, model, controller
def test_named_servos_apply_gains_by_joint_and_do_not_enlarge_authored_effort(tmp_path):
result, model, controller = setup(tmp_path)
config = json.loads(result.config.read_text())
assert config["kp"] == GAINS["kp"]
assert config["kd"] == GAINS["kd"]
assert config["effort_limit"] == GAINS["effort_limit"]
# Actuator order arm,leg differs from controller order leg,arm.
np.testing.assert_array_equal(model.actuator_gainprm[:, 0], [30, 70])
np.testing.assert_array_equal(model.actuator_biasprm[:, 2], [-3, -2])
np.testing.assert_array_equal(model.actuator_gear[:, 0], [1, 1])
# Arm: motor control [-1,2] * gear -2, intersect joint [-3,5].
# Leg: requested +-6, source force [-8,12], intersect joint [-5,4].
np.testing.assert_array_equal(model.actuator_forcerange, [[-3, 2], [-5, 4]])
data = mujoco.MjData(model)
controller.reset(data)
controller.step(data, {"arm": .2, "leg": .2})
mujoco.mj_forward(model, data)
np.testing.assert_array_equal(data.actuator_force, [2, 4])
controller.step(data, {"arm": -.2, "leg": -.2})
mujoco.mj_forward(model, data)
np.testing.assert_array_equal(data.actuator_force, [-3, -5])
def test_cpg_uses_named_pd_gains_and_clips_to_preserved_effort(tmp_path):
_, model, controller = setup(
tmp_path, kind="cpg", frequency_hz=1, amplitude=[.1, .1], phase=[np.pi / 2] * 2,
)
data = mujoco.MjData(model)
controller.reset(data)
data.qpos[:] = [.09, .09]
data.qvel[:] = [.2, .1]
# leg: 70*.01 - 2*.2=.3; arm: 30*.01 - 3*.1=0.
np.testing.assert_allclose(controller.step(data), [0, .3], atol=1e-14)
data.qpos[:] = [-.1, -.1]
np.testing.assert_allclose(controller.step(data), [2, 4], atol=1e-14)
data.qpos[:] = [.2, .2]
np.testing.assert_allclose(controller.step(data), [-2, -4], atol=1e-14)
@pytest.mark.parametrize("release", [{"keys": {"key_escape"}}, {"pad": {"B": True}}])
def test_calibrated_home_releases_bound_arm_and_preserves_unbound_leg(tmp_path, release):
_, model, controller = setup(
tmp_path, home={"arm": -.08, "leg": .1}, operator_axes=["arm"],
command_ranges={"arm": [-.12, .04], "leg": [-.15, .15]},
)
data = mujoco.MjData(model)
data.qpos[:] = [.11, -.07]
controller.reset(data)
# Reset holds actual calibrated pose, rather than teleporting qpos to home.
np.testing.assert_array_equal(data.qpos, [.11, -.07])
np.testing.assert_array_equal(data.ctrl, [-.07, .11])
controller.step(data, {"arm": .03, "leg": .13})
controller.step(data, **release)
np.testing.assert_array_equal(data.ctrl, [-.08, .13])
assert controller.command_spans["arm"][2] == -.08
assert controller.command_spans["leg"][2] == .1
@pytest.mark.parametrize("field,value", [
("kp", {"arm": 30}), ("kp", {"arm": 30, "leg": 70, "extra": 1}),
("kd", {"arm": -1, "leg": 2}), ("kp", {"arm": 0, "leg": 2}),
("effort_limit", {"arm": float("nan"), "leg": 2}),
("effort_limit", {"arm": 0, "leg": 2}), ("kp", [30, 70]),
("home", {"arm": 0}), ("home", {"arm": 0, "leg": .3}),
("home", {"arm": float("inf"), "leg": .1}), ("home", [0, .1]),
])
def test_named_parameters_reject_missing_extra_invalid_or_positional_values(tmp_path, field, value):
with pytest.raises(ValueError):
setup(tmp_path, **{field: value})
assert not (tmp_path / "controller").exists()
def test_home_must_lie_in_declared_command_range(tmp_path):
with pytest.raises(ValueError, match="exceeds limits or excludes the rest"):
setup(tmp_path, home={"arm": -.08, "leg": .1}, command_ranges={"arm": [0, .1]})