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

320 lines
11 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_idle_keyboard_preserves_mocap_drag(tmp_path):
from mjrl.platform.play import RobotApplication
setup, _, _, _ = _setup(tmp_path, _ARM, kind="ik", joints=["x", "y"])
app = RobotApplication(
setup.model.parent,
{
"model": "robot.xml",
"default_mode": "ik",
"modes": {
"ik": {
"control_mode": "ik_mocap",
"contract": "controller.json",
"controls": "controls.yaml",
}
},
},
)
app.data.mocap_pos[0] = [0.08, -0.05, 0]
for _ in range(55):
app.step(keys=())
np.testing.assert_allclose(
app.data.site_xpos[app.controller.ik.site], [0.08, -0.05, 0], atol=1e-4
)
app.step(keys={"key_escape"})
np.testing.assert_equal(app.data.mocap_pos[0], [0, 0, 0])
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
)