Files
chenlin c32df70326
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
release: v0.1 generic robot training and application platform
2026-11-05 11:05:01 +08:00

274 lines
9.4 KiB
Python

"""The controller, imported.
`deploy/fsm` is what the robot runs and what a browser runs. This is the same
crate as a Python extension, so `play` can run it too -- and the reason that
matters is not tidiness. An FSM is a file of rules in an order, and until this
existed the only way to find out what a particular file did was to bundle it and
upload it. A rule in the wrong place was a round trip through another repository
away from being visible.
What these pin is the boundary, not the controller: the controller has its own
55 tests in Rust. What can go wrong here is everything about crossing -- a joint
count nobody checked, a dict field read as an attribute, an optional key that
turns out to be required, an error that arrives as a panic instead of an
exception.
The extension is built here, by cargo, from this tree -- the same
`compile_extension` every bundle build runs. It used to be imported from
site-packages, where `deploy.py --python` put it: a build of any age, and on a
machine that had never run that command this whole file skipped and said
nothing a person would read. Skipped now only where there is no Rust toolchain,
which `pip install -e .` deliberately does not ask for: a machine that only
trains has no use for one.
"""
from __future__ import annotations
import importlib.util
import json
from pathlib import Path
import pytest
REPO = Path(__file__).resolve().parents[1]
def _extension():
"""This tree's controller, as the Python extension `play` imports."""
spec = importlib.util.spec_from_file_location("deploy_for_extension",
REPO / "scripts/deploy.py")
deploy = importlib.util.module_from_spec(spec)
spec.loader.exec_module(deploy)
cargo = deploy.which("cargo")
if cargo is None:
pytest.skip("cargo is not installed, so the controller extension cannot be built",
allow_module_level=True)
built = deploy.compile_extension(cargo, verbose=False)
# The name is not free: CPython looks for `PyInit_mjrl_fsm`.
spec = importlib.util.spec_from_file_location("mjrl_fsm", built)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
return module
mjrl_fsm = _extension()
ONE_MODE = """
[fsm]
initial_state = "walk"
warm_start_ref = "walk"
safe_state = "safe"
tilt_limit = 3.15
state_timeout_ms = 200
command_timeout_ms = 500
mode_switch_ramp_s = 0.0
pose_reach_tol = 0.10
warm_start_duration_s = 0.0
ramp_kp = 0.15
ramp_kd = 0.01
# A binding, not the `[fsm.key]` table this used to carry. That table stopped
# being read when a control became a button **plus a gesture**, and this file
# kept working for a while anyway -- against an `mjrl_fsm.so` in site-packages
# built before the change. Rebuilding the extension is what surfaced it, which
# is worth remembering: a compiled artifact is a place a test suite can go on
# passing after the thing it tests has moved.
[[fsm.button]]
name = "slow"
key = "keypad_1"
on = "toggle"
[[fsm.state]]
name = "safe"
hold_current = true
kd = 0.01
[[fsm.state]]
name = "walk"
model = "walk.onnx"
[[fsm.state]]
name = "slow"
model = "slow.onnx"
[[fsm.rule]]
when = "feedback_stale"
enter = "safe"
[[fsm.rule]]
when = "in_state:safe"
enter = "@initial"
[[fsm.rule]]
when = "button:slow"
enter = "slow"
[[fsm.rule]]
when = "always"
enter = "walk"
"""
@pytest.fixture(scope="module")
def parts(legacy_export: Path) -> tuple[str, list[str], list[float]]:
text = (legacy_export / "layout.json").read_text("utf-8")
contract = json.loads(text)
wire = contract["wire_joint_order"]
home = [contract["default_joint_pos"][j] for j in wire]
return text, wire, home
def make(parts, config: str = ONE_MODE):
text, wire, _ = parts
n = len(wire)
return mjrl_fsm.Fsm(
config,
{"walk": text, "slow": text},
{
"joint_names": wire,
"joint_pos_lo": [-3.3] * n,
"joint_pos_hi": [3.3] * n,
"output_rate_hz": 50.0,
"gait_period": 0.32,
"gait_gate_threshold": 0.05,
},
)
def press(fsm, parts, key: str) -> bool:
"""One press of a key: down, a tick, up, a tick.
The extension reports **edges**, not presses -- `rise` and `fall` are both
in the vocabulary, so a one-shot `pressed` call could only ever drive half
of it. This replaced `press_key`, which could not express the difference.
Returns whether any binding names the key, so "that key does nothing" stays
distinguishable from "the rule did not fire".
"""
known = fsm.set_key(key, True)
drive(fsm, parts, 1)
fsm.set_key(key, False)
drive(fsm, parts, 1)
return known
def drive(fsm, parts, ticks: int, pose: list[float] | None = None) -> int:
"""Run the loop a host would run, and count the inferences."""
layout, wire, home = parts
action_dim = json.loads(layout)["action"]["dim"]
n = len(wire)
q = home if pose is None else pose
infers = 0
for t in range(ticks):
now = t * 20_000
fsm.set_state(q, [0.0] * n, [0.0] * n, [1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0], now)
fsm.set_command(0.3, 0.0, 0.0, now_us=now)
if fsm.tick(now) is not None:
fsm.resume([0.0] * action_dim)
infers += 1
return infers
def test_it_runs_a_real_contract(parts) -> None:
fsm = make(parts)
assert sorted(fsm.modes) == ["slow", "walk"]
assert fsm.bindings == [("slow", None, "keypad_1", "toggle", None)]
infers = drive(fsm, parts, 40)
assert infers == 40, "at the home pose there is no ramp to wait through"
assert len(fsm.observation()) == json.loads(parts[0])["observation"]["dim"]
assert len(fsm.positions()) == len(parts[1])
assert fsm.running_policy
def test_a_pose_the_ramp_cannot_reach_never_starts_the_policy(parts) -> None:
"""The behaviour that is easiest to mistake for a broken build.
The hand-off waits on the *measured* pose, not on a timer, so gains too soft
to converge mean it waits indefinitely and the robot soft-holds. That is
deliberate -- the alternative is starting a policy from a pose it never
trained around -- and `running_policy` is how a host tells the two apart.
"""
ramped = ONE_MODE.replace("mode_switch_ramp_s = 0.0", "mode_switch_ramp_s = 1.0")
fsm = make(parts, ramped)
away = [0.0] * len(parts[1])
assert drive(fsm, parts, 200, pose=away) == 0
assert fsm.mode == "walk", "the FSM chose the mode"
assert not fsm.running_policy, "...and the policy is not what is running"
def test_a_key_switches_modes_and_an_unbound_one_says_so(parts) -> None:
fsm = make(parts)
drive(fsm, parts, 5)
assert fsm.mode == "walk"
assert press(fsm, parts, "keypad_1") is True
assert fsm.mode == "slow"
# Pressing again leaves: a press is one request and the latch toggles.
assert press(fsm, parts, "keypad_1") is True
assert fsm.mode == "walk"
# An unbound key is not an error and not a silent no-op.
assert press(fsm, parts, "keypad_9") is False
def test_the_operator_going_away_does_not_leave_a_mode_latched(parts) -> None:
"""From the C++, which learned it the useful way: a stuck mode keeps the FSM
in a state that may suspend the tilt fallback, with nobody left to release
it."""
fsm = make(parts)
drive(fsm, parts, 5)
press(fsm, parts, "keypad_1")
assert fsm.mode == "slow"
# State keeps arriving; the command does not. `command_timeout_ms` is 500.
_, wire, home = parts
n = len(wire)
now = 10_000_000
fsm.set_state(home, [0.0] * n, [0.0] * n, [1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0], now)
fsm.tick(now)
assert fsm.mode == "walk"
def test_nothing_crosses_the_boundary_unchecked(parts) -> None:
"""Every one of these arrives as an exception with the reason in it. A
panic across the boundary aborts the interpreter, which in `play` means
losing the viewer and the episode with it."""
text, wire, home = parts
n = len(wire)
with pytest.raises(ValueError, match="no 'joint_pos_lo'"):
mjrl_fsm.Fsm(ONE_MODE, {"walk": text}, {"joint_names": wire})
# A mode the contracts dict does not cover. Looked up by **state name**, not
# by model filename -- renaming the file would change nothing, which is the
# first thing this test tried.
third = ONE_MODE.replace(
'[[fsm.rule]]\nwhen = "feedback_stale"',
'[[fsm.state]]\nname = "sprint"\nmodel = "sprint.onnx"\n\n'
+ '[[fsm.rule]]\nwhen = "feedback_stale"',
1,
)
with pytest.raises(ValueError, match="no contract was installed"):
make(parts, third)
fsm = make(parts)
with pytest.raises(ValueError, match=f"{n} joints; q has 3"):
fsm.set_state([0.0] * 3, [0.0] * n, [0.0] * n, [1, 0, 0, 0], [0, 0, 0], 0)
with pytest.raises(ValueError, match="quat must be"):
fsm.set_state(home, [0.0] * n, [0.0] * n, [1, 0, 0], [0, 0, 0], 0)
# An action nobody asked for, and one of the wrong width.
with pytest.raises(ValueError, match="no inference outstanding"):
fsm.resume([0.0] * json.loads(text)["action"]["dim"])
def test_play_is_the_host_where_nothing_has_moved(parts) -> None:
"""`source.rs` reports every term a host supplies differently than training
did. On mjlab that list is empty by construction -- this is where the policy
trained -- and it is not empty on the robot. A host that reported
divergences here would be reporting that `play` is not mjlab."""
fsm = make(parts)
assert fsm.divergences("walk") == []