"""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_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( '' ' <{actuator} {transmission}="axis"/>""" @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])