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.
287 lines
12 KiB
Python
287 lines
12 KiB
Python
"""Check actor graph adapters on independently specified permutations and transforms."""
|
|
|
|
import copy
|
|
import json
|
|
|
|
import numpy as np
|
|
import onnx
|
|
import onnxruntime
|
|
import pytest
|
|
from onnx import TensorProto, helper, numpy_helper
|
|
|
|
mujoco = pytest.importorskip("mujoco")
|
|
|
|
from mjrl.adapter import adapt_policy, build_layout
|
|
|
|
|
|
def _metadata():
|
|
return {
|
|
"schema": "mjrl-policy-adapter/1",
|
|
"wire_joint_order": ["b", "hold", "a"],
|
|
"obs_joint_order": ["b", "a"],
|
|
"action_joint_order": ["a", "b"],
|
|
"source_action_joint_order": ["b", "a"],
|
|
"default_joint_pos": {"a": 0.3, "b": -0.2, "hold": 0.1},
|
|
"action_scale": {"a": 0.25, "b": 0.5},
|
|
"control": {"kp": 50, "kd": 4, "effort_limit": 30, "control_hz": 50},
|
|
"observation": {
|
|
"history_length": 2,
|
|
"terms": [
|
|
{
|
|
"name": "joint_pos",
|
|
"source_joint_order": ["a", "b"],
|
|
"clip": [-3, 3],
|
|
"scale": [2, 3],
|
|
},
|
|
{"name": "actions"},
|
|
],
|
|
"source_term_order": ["actions", "joint_pos"],
|
|
"source_history_order": "newest_first",
|
|
"source_order": "frame_major",
|
|
"inputs": [{"name": "newest", "dim": 4}, {"name": "oldest", "dim": 4}],
|
|
"normalization": {"mean": [1] * 8, "std": [1] * 8, "epsilon": 1},
|
|
},
|
|
}
|
|
|
|
|
|
def _actor(path, *, width=4):
|
|
def tensor(name, width):
|
|
return helper.make_tensor_value_info(name, TensorProto.FLOAT, [1, width])
|
|
|
|
nodes = [
|
|
helper.make_node("Gather", [name, "first_two"], [f"{name}_actions"], axis=1)
|
|
for name in ("newest", "oldest")
|
|
]
|
|
nodes.append(helper.make_node("Add", ["newest_actions", "oldest_actions"], ["raw"]))
|
|
graph = helper.make_graph(
|
|
nodes,
|
|
"actor",
|
|
[tensor("newest", width), tensor("oldest", 4)],
|
|
[tensor("raw", 2)],
|
|
[numpy_helper.from_array(np.array([0, 1], dtype=np.int64), "first_two")],
|
|
)
|
|
# Include a joint-position dependency. If it is omitted, checking only
|
|
# previous actions could pass with the joint-position indices scrambled.
|
|
graph.node[-1].input[1] = "joint_actions"
|
|
graph.node.insert(
|
|
2, helper.make_node("Gather", ["newest", "last_two"], ["joint_actions"], axis=1)
|
|
)
|
|
graph.initializer.append(numpy_helper.from_array(np.array([2, 3], dtype=np.int64), "last_two"))
|
|
graph.node.append(helper.make_node("Add", ["raw", "oldest_actions"], ["result"]))
|
|
graph.output[0].name = "result"
|
|
model = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)])
|
|
model.ir_version = 9
|
|
onnx.save(model, path)
|
|
|
|
|
|
def test_onnx_adapter_maps_frames_observation_and_actions_by_name(tmp_path):
|
|
path = tmp_path / "original.onnx"
|
|
_actor(path)
|
|
metadata = _metadata()
|
|
model = mujoco.MjModel.from_xml_string("""<mujoco><worldbody>
|
|
<body><joint name="hold" range="-90 90"/><geom size=".1"/></body>
|
|
<body><joint name="a" range="-90 90"/><geom size=".1"/></body>
|
|
<body><joint name="b" range="-90 90"/><geom size=".1"/></body>
|
|
</worldbody></mujoco>""")
|
|
adapted = adapt_policy(path, metadata, tmp_path / "export", model=model)
|
|
layout = json.loads(adapted.layout.read_text("utf-8"))
|
|
assert layout["wire_joint_order"] == ["b", "hold", "a"]
|
|
assert layout["obs_joint_order"] == ["b", "a"]
|
|
assert layout["action_joint_order"] == ["a", "b"]
|
|
assert layout["action_scale"] == 1
|
|
assert layout["unactuated_joints"] == {"hold": 0.1}
|
|
assert layout["observation"]["dim"] == 8
|
|
assert layout["joint_limits"]["hold"] == pytest.approx([-np.pi / 2, np.pi / 2])
|
|
assert adapted.max_abs_error < 1e-6
|
|
session = onnxruntime.InferenceSession(str(adapted.actor), providers=["CPUExecutionProvider"])
|
|
observation = np.array([[1, 2, 3, 4, 0.5, 1.5, 1, 2.5]], dtype=np.float32)
|
|
# Newest source row after transforms: [2, 1.5, 2.5, 4].
|
|
# Oldest source row: [1, .5, 1.5, 1]. The source sums the three pairs.
|
|
# Source [b, a] -> canonical [a, b], then multiply by [0.25, 0.5].
|
|
np.testing.assert_allclose(session.run(["actions"], {"obs": observation})[0], [[1.5, 2.75]])
|
|
|
|
|
|
def test_torchscript_parity_validates_the_original_module(tmp_path):
|
|
torch = pytest.importorskip("torch")
|
|
|
|
class Actor(torch.nn.Module):
|
|
def forward(self, x, y):
|
|
return (x[:, :2] - y[:, :2]) * 0.3
|
|
|
|
source = tmp_path / "source.pt"
|
|
torch.jit.trace(Actor(), (torch.zeros(1, 4), torch.zeros(1, 4))).save(str(source))
|
|
adapted = adapt_policy(source, _metadata(), tmp_path / "out")
|
|
assert adapted.max_abs_error < 1e-6
|
|
assert adapted.actor.is_file()
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("mutation", "message"),
|
|
[
|
|
(
|
|
lambda d: d["observation"]["terms"][0].update(name="foot_scan"),
|
|
"Unsupported observation",
|
|
),
|
|
(lambda d: d["observation"]["terms"][0].update(dim=3), "joint_pos.dim must be 4"),
|
|
(lambda d: d.update(source_action_joint_order=["hold", "b"]), "exactly"),
|
|
(lambda d: d.update(obs_joint_order=["missing"]), "absent from wire"),
|
|
(lambda d: d.update(raw_action_clip=[-1, 1]), "Previous actions cannot"),
|
|
],
|
|
)
|
|
def test_explicit_metadata_fails_closed(mutation, message):
|
|
metadata = copy.deepcopy(_metadata())
|
|
mutation(metadata)
|
|
with pytest.raises(ValueError, match=message):
|
|
build_layout(metadata)
|
|
|
|
|
|
def test_graph_width_mismatch_never_publishes_an_export(tmp_path):
|
|
source = tmp_path / "source.onnx"
|
|
_actor(source, width=3)
|
|
output = tmp_path / "export"
|
|
with pytest.raises(ValueError, match="Actor input 'newest'"):
|
|
adapt_policy(source, _metadata(), output)
|
|
assert not output.exists()
|
|
|
|
|
|
def test_adapter_writes_valid_bindings_only_for_supported_command_ranges(tmp_path):
|
|
from controller.controls import load_controls
|
|
|
|
source = tmp_path / "velocity.onnx"
|
|
node = helper.make_node("Gather", ["source_obs", "columns"], ["raw"], axis=1)
|
|
graph = helper.make_graph(
|
|
[node],
|
|
"velocity_actor",
|
|
[helper.make_tensor_value_info("source_obs", TensorProto.FLOAT, [1, 3])],
|
|
[helper.make_tensor_value_info("raw", TensorProto.FLOAT, [1, 2])],
|
|
[numpy_helper.from_array(np.array([0, 2], dtype=np.int64), "columns")],
|
|
)
|
|
proto = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)])
|
|
proto.ir_version = 9
|
|
onnx.save(proto, source)
|
|
meta = _metadata()
|
|
meta["observation"] = {"terms": [{"name": "commands"}]}
|
|
meta["command_ranges"] = {
|
|
"twist": {"lin_vel_x": [-0.2, 0.6], "lin_vel_y": [0, 0], "ang_vel_z": [-1, 1]}
|
|
}
|
|
adapted = adapt_policy(source, meta, tmp_path / "policy")
|
|
layout = json.loads(adapted.layout.read_text())
|
|
controls = load_controls(adapted.controls)
|
|
assert layout["observation"]["command_order"] == ["lin_vel_x", "lin_vel_y", "ang_vel_z"]
|
|
assert tuple(controls.bindings) == ("lin_vel_x", "ang_vel_z")
|
|
assert controls.bindings["lin_vel_x"][0].source == "Ly"
|
|
assert layout["controller"]["source"]["sha256"] == controls.source_sha256
|
|
assert json.loads(adapted.config.read_text()) == {
|
|
"kind": "rl_onnx",
|
|
"policy": "actor.onnx",
|
|
"layout": "layout.json",
|
|
}
|
|
|
|
# New metadata defaults to A/D yaw. An explicit pre-WASD contract
|
|
# regenerates its original keys and file bytes, including its ramp rate.
|
|
import yaml
|
|
from controller.controls import controller_contract
|
|
from mjrl.controllers.bindings import velocity_controls
|
|
|
|
assert "key_a" in controls.key_axes["ang_vel_z"].plus
|
|
previous = tmp_path / "controls.yaml"
|
|
previous.write_text(yaml.safe_dump(velocity_controls(tuple(controls.bindings)), sort_keys=False))
|
|
meta["controller"] = controller_contract(load_controls(previous), None)
|
|
restored = adapt_policy(source, meta, tmp_path / "previous")
|
|
assert restored.controls.read_bytes() == previous.read_bytes()
|
|
assert json.loads(restored.layout.read_text())["controller"] == meta["controller"]
|
|
assert load_controls(restored.controls).key_axes["ang_vel_z"].plus == ("key_j",)
|
|
meta["controller"]["devices"]["keyboard"]["full_after_s"] = 3.75
|
|
rejected = tmp_path / "mismatch"
|
|
with pytest.raises(ValueError, match="differs from generated controls"):
|
|
adapt_policy(source, meta, rejected)
|
|
assert not rejected.exists()
|
|
|
|
|
|
@pytest.mark.parametrize("observes_commands", [False, True])
|
|
def test_adapted_fixed_jit_runs_verified_native_application(tmp_path, observes_commands):
|
|
from mjrl.onboarding.verify import verify
|
|
from mjrl.platform.bundle import build_bundle, open_bundle
|
|
from mjrl.platform.play import RobotApplication
|
|
|
|
torch = pytest.importorskip("torch")
|
|
|
|
class Actor(torch.nn.Module):
|
|
def forward(self, obs):
|
|
return obs[:, :1] * 0 + 0.5
|
|
|
|
source = tmp_path / "target.pt"
|
|
torch.jit.trace(Actor(), torch.zeros(1, 3 if observes_commands else 1)).save(str(source))
|
|
model_path = tmp_path / "robot.xml"
|
|
model_path.write_text(
|
|
"""<mujoco>
|
|
<compiler angle="radian"/>
|
|
<option integrator="implicitfast" timestep=".002"/>
|
|
<worldbody>
|
|
<body name="base" pos="0 0 .5">
|
|
<joint name="hinge" axis="0 0 1" range="-1 1" armature=".05"/>
|
|
<geom type="capsule" size=".02" fromto="0 0 0 .2 0 0" mass=".15"/>
|
|
</body>
|
|
</worldbody>
|
|
<actuator><motor name="servo" joint="hinge" forcerange="-8 8"/></actuator>
|
|
</mujoco>""",
|
|
encoding="utf-8",
|
|
)
|
|
model = mujoco.MjModel.from_xml_path(str(model_path))
|
|
adapted = adapt_policy(
|
|
source,
|
|
{
|
|
"wire_joint_order": ["hinge"],
|
|
"obs_joint_order": ["hinge"],
|
|
"action_joint_order": ["hinge"],
|
|
"default_joint_pos": {"hinge": 0},
|
|
"action_scale": 0.4,
|
|
"observation": {
|
|
"terms": [{"name": "commands" if observes_commands else "joint_pos"}],
|
|
},
|
|
"control": {"kp": 15, "kd": 0.8, "effort_limit": 8, "control_hz": 50},
|
|
},
|
|
tmp_path / "policy",
|
|
model=model,
|
|
)
|
|
assert adapted.controls is None
|
|
axes = ["lin_vel_x", "lin_vel_y", "ang_vel_z"] if observes_commands else []
|
|
assert json.loads(adapted.layout.read_text())["observation"]["command_order"] == axes
|
|
report = verify(
|
|
model_path,
|
|
{
|
|
"controller": adapted.config,
|
|
"steps": 500,
|
|
"thresholds": {"position_atol": 0.005},
|
|
"probes": [{"joint_positions": {"hinge": 0.2}}],
|
|
},
|
|
)
|
|
assert report["passed"], report["stages"]
|
|
report_path = tmp_path / "verification.json"
|
|
report_path.write_text(json.dumps(report), encoding="utf-8")
|
|
app = build_bundle(
|
|
model_path,
|
|
tmp_path / "app",
|
|
robot_id="fixed_actor",
|
|
modes={
|
|
"main": {
|
|
"controller": adapted.config,
|
|
"policy": adapted.actor,
|
|
"layout": adapted.layout,
|
|
}
|
|
},
|
|
verification=report_path,
|
|
)
|
|
with open_bundle(app) as (root, manifest):
|
|
runtime = RobotApplication(root, manifest)
|
|
assert runtime.controller.command_axes == tuple(axes)
|
|
assert "controls" not in manifest["modes"]["main"]
|
|
for _ in range(50):
|
|
state = runtime.step()
|
|
assert state["qpos"][0] == pytest.approx(0.2, abs=0.005)
|
|
if observes_commands:
|
|
runtime.step({"lin_vel_x": 0.0})
|
|
message = "requires a declared trained range" if observes_commands else "Unknown policy command"
|
|
with pytest.raises(ValueError, match=message):
|
|
runtime.step({"lin_vel_x": 0.1})
|