"""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])