ceca51b0d8
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
217 lines
8.2 KiB
Python
217 lines
8.2 KiB
Python
"""`play.py --measure`: the foot-force channel, and the two ways it goes quiet.
|
|
|
|
The monitor writes a csv nobody reads until something looks wrong, and a plot
|
|
somebody glances at. Both fail silently in the same way -- a column of zeros, or
|
|
a column that is not the quantity its header names -- so what is pinned here is
|
|
which sensor the force comes from and that the csv's header still describes its
|
|
rows.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
|
|
torch = pytest.importorskip("torch", reason="the monitor is torch code")
|
|
|
|
from mjrl.viewer.monitor import _find_foot_sensor # noqa: E402
|
|
|
|
|
|
class _Match:
|
|
def __init__(self, pattern):
|
|
self.pattern = pattern
|
|
|
|
|
|
class _Cfg:
|
|
def __init__(self, name, fields, air, pattern):
|
|
self.name = name
|
|
self.fields = fields
|
|
self.track_air_time = air
|
|
self.primary = _Match(pattern)
|
|
|
|
|
|
class _Sensor:
|
|
def __init__(self, cfg, count=6):
|
|
self.cfg = cfg
|
|
self.data = type("D", (), {"force": torch.zeros(2, count, 3)})()
|
|
|
|
|
|
class _Scene:
|
|
def __init__(self, sensors):
|
|
self.sensors = sensors
|
|
|
|
|
|
class _Env:
|
|
def __init__(self, sensors):
|
|
self.scene = _Scene(sensors)
|
|
|
|
|
|
def test_the_force_comes_from_the_sensor_that_tracks_air_time() -> None:
|
|
"""Not from the first contact sensor, and not from one matched by name.
|
|
|
|
A scene has more than one contact sensor and only one of them is about
|
|
footfalls. `self_collision` on this robot requests `force` exactly like the
|
|
foot sensor does, so "the first sensor with a force field" picks the wrong one
|
|
-- and the failure is a panel of near-zeros that looks like a robot with very
|
|
soft feet.
|
|
|
|
`track_air_time` is the discriminator because it is what the *author* of the
|
|
sensor set when they meant footfalls, and because it is what
|
|
`compute_first_contact` needs, so a sensor that qualifies is exactly a sensor
|
|
the landing impact can be read from.
|
|
"""
|
|
feet = _Sensor(_Cfg("feet_ground_contact", ("found", "force"), True, ("LF", "RF")))
|
|
collision = _Sensor(_Cfg("self_collision", ("found", "force"), False, "base_link"))
|
|
|
|
# Declared first, so an implementation that takes the first force sensor picks
|
|
# it -- which is the mistake this is here to catch.
|
|
sensor, names = _find_foot_sensor(_Env({"self_collision": collision, "feet": feet}))
|
|
assert sensor is feet, "the collision sensor was taken for the feet"
|
|
assert names == ("LF", "RF"), "the names did not come from the primary pattern"
|
|
|
|
# No qualifying sensor is not an error: the monitor drops the channel.
|
|
sensor, names = _find_foot_sensor(_Env({"self_collision": collision}))
|
|
assert sensor is None and names == ()
|
|
|
|
# A regex primary cannot be turned into names, so it gets indices rather than
|
|
# a guess. A wrong name is worse than a number.
|
|
regex = _Sensor(_Cfg("feet", ("found", "force"), True, ".*_toe"))
|
|
sensor, names = _find_foot_sensor(_Env({"feet": regex}))
|
|
assert sensor is regex and names == tuple(f"feet[{i}]" for i in range(6))
|
|
|
|
|
|
def test_the_csv_header_still_describes_its_rows(tmp_path: Path) -> None:
|
|
"""A header one column short shifts every reading after it, silently.
|
|
|
|
The force columns were appended to both the header and the row, in two
|
|
different places. That is the shape of edit where they come apart: the file
|
|
still parses, every column still holds plausible numbers, and `tau/LF_hip` is
|
|
now somebody's foot.
|
|
"""
|
|
from mjrl.viewer.monitor import JointMonitor
|
|
|
|
joints = ("a", "b", "c")
|
|
actuators = ("a", "b")
|
|
feet = ("LF", "RF")
|
|
|
|
class _Entity:
|
|
joint_names = joints
|
|
actuator_names = actuators
|
|
|
|
class data:
|
|
joint_pos = torch.zeros(1, len(joints))
|
|
joint_vel = torch.zeros(1, len(joints))
|
|
actuator_force = torch.zeros(1, len(actuators))
|
|
|
|
class _Feet:
|
|
cfg = _Cfg("feet", ("found", "force"), True, feet)
|
|
data = type("D", (), {"force": torch.ones(1, len(feet), 3)})()
|
|
|
|
@staticmethod
|
|
def compute_first_contact(dt):
|
|
del dt
|
|
return torch.ones(1, len(feet), dtype=torch.bool)
|
|
|
|
monitor = JointMonitor(
|
|
_Entity(), 0, 0.01, tmp_path, sensor=_Feet(), foot_names=feet
|
|
)
|
|
for _ in range(4):
|
|
monitor.update()
|
|
monitor.close()
|
|
|
|
rows = (tmp_path / "measure.csv").read_text().splitlines()
|
|
header = rows[0].split(",")
|
|
assert header[-len(feet):] == [f"force/{n}" for n in feet]
|
|
for row in rows[1:]:
|
|
assert len(row.split(",")) == len(header), (
|
|
"a data row is a different width than the header, so every column "
|
|
"after the shortest one is mislabelled"
|
|
)
|
|
assert len(rows) - 1 == 4
|
|
|
|
# And the landing impacts were counted rather than the force sampled: every
|
|
# step reported a first contact on both feet.
|
|
assert len(monitor._impacts) == 4 * len(feet)
|
|
|
|
|
|
def test_a_bundle_is_a_directory_checkpoint_can_be_pointed_at(tmp_path) -> None:
|
|
"""The copied checkpoint keeps its own name, and that is the point of it.
|
|
|
|
`export.py` copies the checkpoint into the bundle so the artifact carries its
|
|
own weights -- otherwise the two things anyone does with a bundle later, a
|
|
comparison against the policy it claims to be and a re-export after the
|
|
contract gains a field, both need a path that a month of training has since
|
|
deleted or moved.
|
|
|
|
Keeping `model_<N>.pt` rather than renaming it to something fixed means the
|
|
repository's existing resolver finds it, so the bundle directory works
|
|
everywhere a run directory does -- `play --checkpoint <bundle>` and
|
|
`train --resume --checkpoint <bundle>`. A fixed name would have needed every
|
|
one of those to learn a second convention.
|
|
"""
|
|
from mjrl.checkpoint import find_checkpoint
|
|
|
|
bundle = tmp_path / "model_38000"
|
|
bundle.mkdir()
|
|
(bundle / "layout.json").write_text("{}")
|
|
(bundle / "actor.onnx").write_bytes(b"")
|
|
(bundle / "model_38000.pt").write_bytes(b"")
|
|
|
|
assert find_checkpoint(str(bundle), tmp_path) == bundle / "model_38000.pt"
|
|
|
|
# And a bundle without it is a bundle from before this, which has to fail
|
|
# loudly rather than resolve to something else in the tree.
|
|
bare = tmp_path / "model_1"
|
|
bare.mkdir()
|
|
(bare / "actor.onnx").write_bytes(b"")
|
|
with pytest.raises(FileNotFoundError):
|
|
find_checkpoint(str(bare), tmp_path)
|
|
|
|
|
|
def test_measurement_only_probes_gui_when_explicitly_interactive(tmp_path, monkeypatch):
|
|
from types import SimpleNamespace
|
|
from mjrl.viewer import monitor as module
|
|
|
|
events = []
|
|
|
|
def probe():
|
|
events.append('probe')
|
|
return 'QtAgg', {'QT_API': 'pyqt5'}
|
|
|
|
def open_window(self, backend, env):
|
|
events.append((backend, env))
|
|
|
|
monkeypatch.setattr(module, '_interactive_backend', probe)
|
|
monkeypatch.setattr(module.JointMonitor, '_open_window', open_window)
|
|
entity = SimpleNamespace(joint_names=('hip',), actuator_names=('hip',))
|
|
env = SimpleNamespace(step_dt=.01)
|
|
measurement = module.make_monitor(entity, 0, env, tmp_path)
|
|
assert measurement._fig is None
|
|
assert events == []
|
|
# Control proves the explicitly requested live view still probes and opens.
|
|
module.make_monitor(entity, 0, env, tmp_path, interactive=True)
|
|
assert events == ['probe', ('QtAgg', {'QT_API': 'pyqt5'})]
|
|
|
|
|
|
def test_saved_plot_uses_no_interactive_figure_even_with_live_window(tmp_path, monkeypatch):
|
|
from types import SimpleNamespace
|
|
from matplotlib import pyplot
|
|
from mjrl.viewer.monitor import JointMonitor
|
|
|
|
def unexpected(*args, **kwargs):
|
|
raise AssertionError('artifact export opened an interactive figure')
|
|
|
|
monkeypatch.setattr(pyplot, 'figure', unexpected)
|
|
monkeypatch.setattr(pyplot, 'subplots', unexpected)
|
|
measurement = JointMonitor(SimpleNamespace(joint_names=('hip',), actuator_names=('hip',)),
|
|
0, .01, tmp_path)
|
|
measurement._fig = object() # Existing live monitor must not affect export.
|
|
measurement._t = [0., .01]
|
|
measurement._pos = [[0.], [.1]]
|
|
measurement._vel = [[0.], [1.]]
|
|
measurement._tau = [[0.], [.5]]
|
|
png = tmp_path / 'measure.png'
|
|
measurement._write_png(png)
|
|
assert png.read_bytes().startswith(b'\x89PNG\r\n\x1a\n')
|