320 lines
11 KiB
Python
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
|
|
)
|