Files
UniversalTrain-ai/tests/test_controllers_generic.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

468 lines
18 KiB
Python

"""Check deterministic controllers against by-name targets and native dynamics."""
import json
import numpy as np
import pytest
mujoco = pytest.importorskip("mujoco")
from mjrl.controllers import load_controller, setup_controller
_ARM = """<mujoco model="slides">
<option gravity="0 0 0" timestep=".002" integrator="implicitfast"/>
<worldbody>
<body name="slide_x">
<joint name="x" type="slide" axis="1 0 0" range="-.2 .2" armature=".1"/>
<geom size=".02" mass=".1" contype="0" conaffinity="0"/>
<body name="tip">
<joint name="y" type="slide" axis="0 1 0" range="-.2 .2" armature=".1"/>
<geom size=".02" mass=".1" contype="0" conaffinity="0"/>
</body>
</body>
</worldbody>
<actuator><motor name="y_old" joint="y"/><motor name="x_old" joint="x"/></actuator>
</mujoco>"""
_DIFF = """<mujoco model="differential">
<option timestep=".002" integrator="implicitfast"/>
<default>
<joint damping=".02" armature=".01"/>
<geom friction="1 .01 .001"/>
</default>
<worldbody>
<geom type="plane" size="2 2 .1"/>
<body name="chassis" pos="0 0 .12">
<freejoint/>
<geom type="box" size=".14 .1 .025" mass="1.5"/>
<geom type="sphere" pos="-.13 0 -.095" size=".025" mass=".05" friction=".03 .01 .001"/>
<body name="right" pos="0 -.13 -.02">
<joint name="right" axis="0 1 0"/>
<geom type="cylinder" euler="90 0 0" size=".1 .02" mass=".25"/>
</body>
<body name="left" pos="0 .13 -.02">
<joint name="left" axis="0 1 0"/>
<geom type="cylinder" euler="90 0 0" size=".1 .02" mass=".25"/>
</body>
</body>
</worldbody>
</mujoco>"""
def _setup(tmp_path, xml, **config):
source = tmp_path / "source.xml"
source.write_text(xml, encoding="utf-8")
setup = setup_controller(source, config, tmp_path / config["kind"])
model = mujoco.MjModel.from_xml_path(str(setup.model))
controller = load_controller(model, setup.config, setup.controls)
data = mujoco.MjData(model)
controller.reset(data)
return setup, model, data, controller
def _step(model, data, controller, seconds, command=None):
nstep = round(1 / (controller.control_hz * model.opt.timestep))
for _ in range(round(seconds * controller.control_hz)):
controller.step(data, command)
mujoco.mj_step(model, data, nstep=nstep)
mujoco.mj_forward(model, data)
def test_mocap_ik_solves_named_site_and_tracks_without_editing_qpos(tmp_path):
setup, model, data, controller = _setup(
tmp_path,
_ARM,
kind="ik",
joints=["x", "y"],
end_effector_body="tip",
)
assert model.nmocap == 1
assert np.array_equal(data.qpos, [0, 0])
target = np.array([0.1, -0.07, 0.0])
data.mocap_pos[0] = target
# The actuator order is y,x. Confusing actuator order with joint order
# would move both joints and still leave a stable but wrong endpoint.
output = controller.step(data, keys=())
np.testing.assert_allclose(output, [-0.07, 0.1], atol=1e-5)
np.testing.assert_equal(data.qpos, [0, 0])
_step(model, data, controller, 1)
np.testing.assert_allclose(data.site_xpos[controller.ik.site], target, atol=2.5e-4)
assert not np.any(model.geom_contype[model.body_geomadr[-1] :])
assert setup.controls.is_file()
def test_application_native_mouse_drag_tracks_and_respects_workspace(tmp_path):
from mjrl.platform.bundle import IK_MOUSE_HELP, _manual
from mjrl.platform.play import RobotApplication
setup, _, _, _ = _setup(
tmp_path, _ARM, kind="ik", joints=["x", "y"],
command_ranges={"target_x": [-0.02, 0.02], "target_y": [-0.03, 0.03]},
)
manifest = {
"model": "robot.xml",
"default_mode": "ik",
"modes": {"ik": {
"control_mode": "ik_mocap", "contract": "controller.json", "controls": "controls.yaml",
}},
}
app = RobotApplication(setup.model.parent, manifest)
manual = _manual(setup.model.parent, "arm", manifest["modes"], "ik")
assert manual["modes"][0]["summary"] == IK_MOUSE_HELP
model, data, ik = app.model, app.data, app.controller.ik
# A calibrated starting pose can differ from the model reference position.
data.qpos[:] = [0.03, 0.04]
app.controller.reset(data)
mujoco.mj_forward(model, data)
origin = data.site_xpos[ik.site].copy()
np.testing.assert_allclose(data.mocap_pos[ik.mocap], origin)
camera, opt = mujoco.MjvCamera(), mujoco.MjvOption()
camera.lookat, camera.distance = origin, 0.5
camera.azimuth, camera.elevation = 90, -90
scene = mujoco.MjvScene(model, maxgeom=32)
perturb = mujoco.MjvPerturb()
mujoco.mjv_updateScene(model, data, opt, perturb, camera, mujoco.mjtCatBit.mjCAT_ALL, scene)
# The same picking, right-drag and pose application calls as MuJoCo's
# passive viewer. Aim at the marker rim, clear of the tool geometry.
point = np.zeros(3)
ids = [np.full(1, -1, dtype=np.int32) for _ in range(3)]
body = mujoco.mjv_select(model, data, opt, 1.0, 0.555, 0.5, scene, point, *ids)
assert body == model.body("mjrl_target").id
perturb.select = body
perturb.localpos[:] = data.xmat[body].reshape(3, 3).T @ (point - data.xpos[body])
mujoco.mjv_initPerturb(model, data, scene, perturb)
perturb.active = mujoco.mjtPertBit.mjPERT_TRANSLATE
mujoco.mjv_movePerturb(
model, data, mujoco.mjtMouse.mjMOUSE_MOVE_H, 0.4, 0.15, scene, perturb
)
mujoco.mjv_applyPerturbPose(model, data, perturb, 0)
raw_offset = data.mocap_pos[ik.mocap] - origin
assert abs(raw_offset[0]) > 0.02
expected = origin + np.clip(raw_offset, [-0.02, -0.03, -0.2], [0.02, 0.03, 0.2])
for _ in range(55):
app.step(keys=())
np.testing.assert_allclose(data.mocap_pos[ik.mocap], expected)
np.testing.assert_allclose(data.site_xpos[ik.site], expected, atol=1e-4)
app.step(keys={"key_escape"})
np.testing.assert_allclose(data.mocap_pos[ik.mocap], origin)
def test_sliders_and_trajectory_apply_joint_limits_and_interpolate_by_name(tmp_path):
_, _, data, sliders = _setup(tmp_path, _ARM, kind="joint_sliders", joints=["x", "y"])
np.testing.assert_allclose(sliders.step(data, {"x": 1.5, "y": -0.06}), [-0.06, 0.2])
# A native viewer can set one slider on data.ctrl while the operator
# sends an empty keyboard snapshot at each tick.
data.ctrl[0] = 0.05
np.testing.assert_allclose(sliders.step(data, keys=()), [0.05, 0.2])
np.testing.assert_allclose(sliders.step(data, {"x": 0.12}), [0.05, 0.12])
setup, model, data, motion = _setup(
tmp_path,
_ARM,
kind="trajectory",
joints=["x", "y"],
trajectory={
"joint_order": ["y", "x"],
"time_s": [0, 0.4, 1.6],
"q": [[0, 0], [-0.08, 0.12], [0.04, 0]],
},
)
assert (setup.config.parent / "motion.trajectory.json").is_file()
data.time = 0.2
np.testing.assert_allclose(motion.step(data), [-0.04, 0.06], atol=1e-10)
data.time = 1.0
np.testing.assert_allclose(motion.step(data), [-0.02, 0.06], atol=1e-10)
data.time = 2.0
np.testing.assert_allclose(motion.step(data), [0.04, 0], atol=1e-10)
mujoco.mj_resetData(model, data)
motion.reset(data)
np.testing.assert_equal(motion.step(data), [0, 0])
def test_cpg_generates_phase_shifted_pd_and_tracks_a_native_linkage(tmp_path):
_, model, data, controller = _setup(
tmp_path,
_ARM,
kind="cpg",
joints=["x", "y"],
frequency_hz=0.6,
amplitude=[0.07, 0.04],
phase=[0, 1.0],
kd=5.0,
effort_limit=4,
)
output = controller.step(data)
assert output[0] == pytest.approx(
40 * 0.04 * np.sin(1.0) + 5 * 0.04 * (1.2 * np.pi) * np.cos(1.0)
)
assert output[1] == pytest.approx(5 * 0.07 * 1.2 * np.pi)
nstep = round(1 / (controller.control_hz * model.opt.timestep))
errors, q = [], []
for _ in range(150):
control = controller.step(data)
assert np.max(np.abs(control)) <= 4
if data.time > 0.5:
target = np.array([0.07, 0.04]) * np.sin(1.2 * np.pi * data.time + np.array([0, 1]))
errors.append(np.max(np.abs(target - data.qpos)))
q.append(data.qpos.copy())
mujoco.mj_step(model, data, nstep=nstep)
assert max(errors) < 0.01
assert np.ptp(q, axis=0)[0] > 0.11
def test_differential_forward_turn_keyboard_and_native_travel(tmp_path):
setup, model, data, controller = _setup(
tmp_path,
_DIFF,
kind="differential",
left_joint="left",
right_joint="right",
wheel_radius=0.1,
track_width=0.26,
max_wheel_speed=8,
)
np.testing.assert_allclose(controller.step(data, pad={"Ly": -1}), [5, 5])
np.testing.assert_allclose(controller.step(data, pad={"Rx": -1}), [-1.3, 1.3])
np.testing.assert_equal(controller.step(data, pad={"Ly": -1, "B": True}), [0, 0])
controller.step(data, keys={"key_w"})
data.time = 0.5
np.testing.assert_allclose(controller.step(data, keys={"key_w"}), [5, 5])
controller.step(data, keys={"key_a"})
data.time = 1.0
np.testing.assert_allclose(controller.step(data, keys={"key_a"}), [-1.3, 1.3])
assert json.loads(setup.config.read_text())["command_ranges"]["lin_vel_y"] == [-0.0, 0.0]
mujoco.mj_resetData(model, data)
controller.reset(data)
_step(model, data, controller, 1.5, {"lin_vel_x": 0.35})
assert data.qpos[0] > 0.35
assert abs(data.qpos[1]) < 0.015
def test_interactive_play_polls_normalized_pad_and_releases_on_disconnect(tmp_path, monkeypatch):
from contextlib import contextmanager
from types import SimpleNamespace
import mujoco.viewer
import controller as devices
from controller import GamepadState
from mjrl.platform import play as native
setup, _, _, _ = _setup(
tmp_path,
_DIFF,
kind="differential",
left_joint="left",
right_joint="right",
wheel_radius=0.1,
track_width=0.26,
)
@contextmanager
def opened(_path):
yield (
setup.model.parent,
{
"model": "robot.xml",
"robot_id": "drive",
"default_mode": "drive",
"modes": {
"drive": {
"control_mode": "wheeled_velocity",
"contract": "controller.json",
"controls": "controls.yaml",
}
},
},
)
@contextmanager
def viewer(*args, **kwargs):
yield SimpleNamespace(is_running=lambda: True, sync=lambda: None)
class Pad:
connected = True
reads = 0
closed = False
def state(self):
self.reads += 1
self.connected = self.reads < 2
# The disconnected reader can still return its last frame.
return GamepadState(ly=-1.0, buttons=frozenset({"view"}))
def close(self):
self.closed = True
pad = Pad()
monkeypatch.setattr(native, "open_bundle", opened)
monkeypatch.setattr(mujoco.viewer, "launch_passive", viewer)
monkeypatch.setattr(devices, "open", lambda: pad)
result = native.play(
setup.model, steps=3, realtime=False, reference_out=tmp_path / "frames.json"
)
np.testing.assert_allclose([f["ctrl"] for f in result["frames"]], [[5, 5], [0, 0], [0, 0]])
assert pad.reads == 2 and pad.closed
evaluation = tmp_path / "evaluation.json"
native.play(
setup.model,
steps=75,
headless=True,
command={"lin_vel_x": 0.35},
evaluation_out=evaluation,
)
assert pad.reads == 2
report = json.loads(evaluation.read_text("utf-8"))
assert report["schema"] == "mjrl-app-velocity-evaluation/1"
assert report["root_body"] == "chassis"
assert report["simulated_seconds"] == pytest.approx(1.5)
assert report["actual_velocity_final"][0] == pytest.approx(0.35, abs=0.03)
assert 0 < report["mean_absolute_error"][0] < 0.05
def test_omni_geometry_lateral_motion_and_uniform_saturation(tmp_path):
wheel_xml = _DIFF.replace(
'<body name="right"',
'<body name="rear" pos="-.13 0 -.02"><joint name="rear" axis="1 0 0"/>'
'<geom type="cylinder" size=".1 .02" mass=".25"/></body><body name="right"',
)
wheels = [
{"joint": "right", "position": [0, -0.13], "angle": 0, "radius": 0.1},
{"joint": "rear", "position": [-0.13, 0], "angle": np.pi / 2, "radius": 0.1},
{"joint": "left", "position": [0, 0.13], "angle": 0, "radius": 0.1},
]
_, _, data, controller = _setup(
tmp_path,
wheel_xml,
kind="omni",
wheels=wheels,
max_wheel_speed=4,
)
# Actuator order is the order the setup added them. A coupled lateral
# command saturates all rates together, preserving their requested ratio.
np.testing.assert_allclose(
controller.step(data, {"lin_vel_x": 0.25, "lin_vel_y": 0.5}), [2, 4, 2], atol=1e-10
)
np.testing.assert_allclose(
controller.step(data, {"ang_vel_z": 1}), [1.3, -1.3, -1.3], atol=1e-10
)
def test_slider_rate_limit_persists_partial_targets_and_accepts_viewer_edits(tmp_path):
_, model, data, sliders = _setup(
tmp_path, _ARM, kind="joint_sliders", joints=["x", "y"], target_rate_limits={"x": 0.5}
)
# The model actuators are ordered y,x. An unlisted joint moves immediately.
np.testing.assert_allclose(sliders.step(data, {"x": 1, "y": -.06}), [-.06, 0])
data.time = .1
np.testing.assert_allclose(sliders.step(data, {"y": .1}), [.1, .05])
np.testing.assert_allclose(sliders.step(data), [.1, .05])
data.time = .2
np.testing.assert_allclose(sliders.step(data, keys=()), [.1, .1])
data.time = .5
np.testing.assert_allclose(sliders.step(data), [.1, .2])
data.ctrl[1] = -.1
data.time = .6
np.testing.assert_allclose(sliders.step(data, keys=()), [.1, .15])
data.time = 1.2
np.testing.assert_allclose(sliders.step(data), [.1, -.1])
mujoco.mj_resetData(model, data)
data.qpos[0] = -.02
np.testing.assert_allclose(sliders.step(data), [0, -.02])
data.time = .1
np.testing.assert_allclose(sliders.step(data), [0, -.02])
@pytest.mark.parametrize("release", [{"keys": {"key_escape"}}, {"pad": {"B": True}}])
@pytest.mark.parametrize("hold_release", [True, False])
def test_slider_rate_limited_operator_return_completes_after_tap_or_hold(
tmp_path, release, hold_release
):
_, _, data, sliders = _setup(
tmp_path, _ARM, kind="joint_sliders", joints=["x", "y"], home={"x": -.1, "y": 0},
operator_axes=["x"], target_rate_limits={"x": 0.5},
)
data.time = .5
np.testing.assert_allclose(sliders.step(data, pad={"Ly": -1}), [0, .2])
data.time = .52
np.testing.assert_allclose(sliders.step(data, **release), [0, .19])
data.time = .54
following = release if hold_release else {"keys": ()}
np.testing.assert_allclose(sliders.step(data, **following), [0, .18])
data.time = 1.2
np.testing.assert_allclose(sliders.step(data, **following), [0, -.1])
@pytest.mark.parametrize("limits", [None, 0.5, {"other": 1}, {"x": 0}, {"x": -1},
{"y": float("nan")}, {"y": True}])
def test_slider_rate_limits_validate_names_and_positive_finite_rates(tmp_path, limits):
with pytest.raises(ValueError, match="target_rate_limits"):
_setup(tmp_path, _ARM, kind="joint_sliders", joints=["x", "y"], target_rate_limits=limits)
def _servo_fixture(actuator, joint_range="", transmission="joint"):
return f"""<mujoco><compiler angle="radian"/><worldbody><body>
<joint name="axis" {joint_range}/><geom size=".02"/>
</body></worldbody><actuator><{actuator} {transmission}="axis"/></actuator></mujoco>"""
@pytest.mark.parametrize("actuator,joint_range,expected", [
('position ctrlrange="-3.141592 3.141592"', "", [-3.141592, 3.141592]),
('position gear="-2" ctrlrange="-.5 1.5"', "", [-.75, .25]),
('general gear="2" gainprm="3" biastype="affine" biasprm="0 -3 -1" ctrlrange="-.5 1.5"',
'range="-.5 .5"', [-.25, .5]),
('position timeconst=".02" ctrlrange="-.1 .2"', "", [-.1, .2]),
('position kp="10" inheritrange=".5"', 'range="-1 1"', [-.5, .5]),
])
def test_normalized_servo_preserves_length_target_in_joint_units(
tmp_path, actuator, joint_range, expected
):
_, model, data, controller = _setup(
tmp_path, _servo_fixture(actuator, joint_range), kind="joint_sliders", joints=["axis"]
)
assert model.actuator_ctrllimited[0]
np.testing.assert_array_equal(model.actuator_ctrlrange[0], expected)
for command, bound in zip([-10, 10], expected, strict=True):
np.testing.assert_array_equal(controller.step(data, {"axis": command}), [bound])
# A raw MuJoCo user can bypass the controller. At the target boundary
# the servo must also hold zero force for an out-of-range native input.
data.qpos[0] = bound
data.ctrl[0] = command
mujoco.mj_forward(model, data)
assert data.actuator_force[0] == pytest.approx(0, abs=1e-12)
@pytest.mark.parametrize("actuator", [
'motor ctrlrange="-.1 .1"', 'velocity ctrlrange="-.1 .1"',
'position ctrllimited="false" ctrlrange="-.1 .1"',
])
def test_non_position_inputs_and_disabled_limits_do_not_restrict_joint_targets(tmp_path, actuator):
_, model, data, controller = _setup(
tmp_path, _servo_fixture(actuator), kind="joint_sliders", joints=["axis"]
)
assert not model.actuator_ctrllimited[0]
np.testing.assert_array_equal(controller.step(data, {"axis": .5}), [.5])
@pytest.mark.parametrize("actuator,joint_range,error", [
('position gear="-2" ctrlrange="2 4"', 'range="0 1"', "do not overlap"),
('general biastype="affine" biasprm=".1 -3 -1" gainprm="3" ctrlrange="-1 1"', "",
"Cannot infer position target bounds"),
('intvelocity kp="3" actrange="-2 2" ctrlrange="-1 1"', "",
"Cannot infer position target bounds"),
])
def test_normalization_rejects_incompatible_position_bounds(tmp_path, actuator, joint_range, error):
with pytest.raises(ValueError, match=error):
_setup(tmp_path, _servo_fixture(actuator, joint_range), kind="joint_sliders", joints=["axis"])
assert not (tmp_path / "joint_sliders").exists()
def test_parent_frame_scalar_transmission_reuses_the_source_servo(tmp_path):
xml = _servo_fixture('position gear="-2" ctrlrange="-.2 .8"', transmission="jointinparent")
_, model, data, controller = _setup(tmp_path, xml, kind="joint_sliders", joints=["axis"])
assert model.nu == 1
np.testing.assert_array_equal(model.actuator_ctrlrange[0], [-.4, .1])
np.testing.assert_array_equal(controller.step(data, {"axis": 1}), [.1])