"""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 = """ """ _DIFF = """ """ 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( '' '