280 lines
11 KiB
Python
280 lines
11 KiB
Python
"""Native policy contracts preserve target semantics and the robot link frame."""
|
|
|
|
import json
|
|
import math
|
|
|
|
import numpy as np
|
|
import onnx
|
|
import pytest
|
|
from onnx import TensorProto, helper, numpy_helper
|
|
|
|
mujoco = pytest.importorskip("mujoco")
|
|
|
|
from mjrl.adapter import adapt_policy
|
|
from mjrl.platform.graph import read_embedded_onnx
|
|
from mjrl.platform.policy import OnnxController
|
|
|
|
_XML = """<mujoco>
|
|
<compiler angle="radian"/>
|
|
<option gravity="0 0 0" timestep=".002"/>
|
|
<worldbody>
|
|
<body name="base">
|
|
<freejoint/>
|
|
<inertial pos="0 .1 0" quat=".5 .5 .5 .5" mass="2" diaginertia=".1 .2 .25"/>
|
|
<geom type="box" size=".1 .1 .1"/>
|
|
<body pos=".2 0 0">
|
|
<joint name="wheel" axis="0 1 0"/>
|
|
<geom size=".04" mass=".1"/>
|
|
</body>
|
|
<body pos="-.2 0 0">
|
|
<joint name="hip" axis="0 1 0" range="-1 1"/>
|
|
<geom size=".04" mass=".1"/>
|
|
</body>
|
|
</body>
|
|
</worldbody>
|
|
<actuator><motor name="wheel_motor" joint="wheel"/><motor name="hip_motor" joint="hip"/></actuator>
|
|
</mujoco>"""
|
|
|
|
|
|
def test_hybrid_adapter_and_runtime_retain_per_term_order_and_velocity_effort(tmp_path):
|
|
source = tmp_path / "original.onnx"
|
|
graph = helper.make_graph(
|
|
[helper.make_node("Gather", ["input", "indices"], ["actions"], axis=1)],
|
|
"actor",
|
|
[helper.make_tensor_value_info("input", TensorProto.FLOAT, [1, 6])],
|
|
[helper.make_tensor_value_info("actions", TensorProto.FLOAT, [1, 2])],
|
|
[numpy_helper.from_array(np.array([4, 3], dtype=np.int64), "indices")],
|
|
)
|
|
proto = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)])
|
|
proto.ir_version = 9
|
|
onnx.save(proto, source)
|
|
model = mujoco.MjModel.from_xml_string(_XML)
|
|
metadata = {
|
|
"wire_joint_order": ["hip", "wheel"],
|
|
"obs_joint_order": ["hip", "wheel"],
|
|
"action_joint_order": ["wheel", "hip"],
|
|
"default_joint_pos": {"hip": 0.1, "wheel": 0},
|
|
"action_scale": {"wheel": 2, "hip": 0.25},
|
|
"control": {
|
|
"kp": {"hip": 10, "wheel": 0},
|
|
"kd": {"hip": 3, "wheel": 2},
|
|
"effort_limit": {"hip": 8, "wheel": 6},
|
|
"control_hz": 50,
|
|
},
|
|
"observation": {
|
|
"terms": [
|
|
{"name": "base_ang_vel"},
|
|
{"name": "joint_pos", "joint_order": ["hip"]},
|
|
{"name": "joint_vel", "joint_order": ["wheel", "hip"]},
|
|
]
|
|
},
|
|
"action": {
|
|
"targets": {
|
|
"hip": {"kind": "position", "offset": 0.1},
|
|
"wheel": {"kind": "velocity", "offset": 0.2, "clip": [-1, 1]},
|
|
}
|
|
},
|
|
}
|
|
export = adapt_policy(source, metadata, tmp_path / "adapted", model=model)
|
|
layout = json.loads(export.layout.read_text("utf-8"))
|
|
assert layout["_schema"] == "mjrl-native-layout/1"
|
|
assert layout["joint_limits"] == {"hip": [-1, 1]}
|
|
# Even a legacy Rust reader that ignores _schema deserializes this field as
|
|
# f32. Its action loader cannot misinterpret a mixed contract as positions.
|
|
assert layout["action_scale"] == {"wheel": 1, "hip": 1}
|
|
|
|
policy = OnnxController(
|
|
model, {"layout": "layout.json", "policy": "actor.onnx"}, export.layout.parent
|
|
)
|
|
data = mujoco.MjData(model)
|
|
policy.reset(data)
|
|
data.qpos[policy.qadr] = [0.3, 4.0]
|
|
data.qvel[policy.vadr] = [0.2, 0.4]
|
|
data.qvel[3:6] = [1, 2, 3]
|
|
np.testing.assert_allclose(policy.observation(data, {}), [[1, 2, 3, 0.2, 0.4, 0.2]], atol=1e-7)
|
|
# Position: 10 * (.1 + .2*.25 - .3) - 3*.2. Wheel: 2 * (1.0 - .4).
|
|
# The physical actuator order is wheel, hip, distinct from the wire order.
|
|
np.testing.assert_allclose(policy.step(data), [1.2, -2.1], atol=1e-7)
|
|
data.qpos[policy.qadr[1]] = 8.0
|
|
np.testing.assert_allclose(policy.step(data), [1.2, -2.1], atol=1e-7)
|
|
|
|
|
|
def test_model_external_tensor_in_nested_sparse_initializer_is_refused(tmp_path):
|
|
external = TensorProto(
|
|
name="weights", data_type=TensorProto.FLOAT, dims=[1], data_location=TensorProto.EXTERNAL
|
|
)
|
|
field = external.external_data.add()
|
|
field.key, field.value = "location", "../../outside.bin"
|
|
indices = numpy_helper.from_array(np.array([0], dtype=np.int64), "indices")
|
|
sparse = helper.make_sparse_tensor(external, indices, [1])
|
|
nested = helper.make_graph([], "subgraph", [], [])
|
|
nested.sparse_initializer.append(sparse)
|
|
graph = helper.make_graph(
|
|
[helper.make_node("If", ["condition"], ["action"], then_branch=nested, else_branch=nested)],
|
|
"actor",
|
|
[],
|
|
[],
|
|
)
|
|
path = tmp_path / "actor.onnx"
|
|
path.write_bytes(helper.make_model(graph).SerializeToString())
|
|
with pytest.raises(ValueError, match="inside the ONNX file"):
|
|
read_embedded_onnx(path)
|
|
|
|
|
|
def test_passive_pendulum_observation_is_preserved_without_a_hold_motor(tmp_path):
|
|
torch = pytest.importorskip("torch")
|
|
|
|
class Actor(torch.nn.Module):
|
|
def forward(self, obs):
|
|
return obs[:, :1] * 0.5 + obs[:, 1:2] * 0.1
|
|
|
|
actor = torch.jit.trace(Actor(), torch.zeros(1, 2))
|
|
source = tmp_path / "pendulum_actor.pt"
|
|
actor.save(str(source))
|
|
model = mujoco.MjModel.from_xml_string("""<mujoco>
|
|
<compiler angle="radian"/>
|
|
<worldbody>
|
|
<body name="base" pos="0 0 1">
|
|
<body name="pendulum">
|
|
<joint name="passive" axis="0 1 0" damping=".02"/>
|
|
<geom type="capsule" fromto="0 0 0 .2 0 0" size=".02" mass=".2"/>
|
|
</body>
|
|
<body name="driven" pos="0 .2 0">
|
|
<joint name="servo" axis="0 0 1" range="-1 1"/>
|
|
<geom type="capsule" fromto="0 0 0 .2 0 0" size=".02" mass=".2"/>
|
|
</body>
|
|
</body>
|
|
</worldbody>
|
|
<actuator><motor name="motor" joint="servo"/></actuator>
|
|
</mujoco>""")
|
|
metadata = {
|
|
"wire_joint_order": ["servo", "passive"],
|
|
"obs_joint_order": ["passive", "servo"],
|
|
"action_joint_order": ["servo"],
|
|
"passive_joints": ["passive"],
|
|
"default_joint_pos": {"servo": 0.1, "passive": 0.4},
|
|
"action_scale": 0.4,
|
|
"control": {
|
|
"kp": {"servo": 8},
|
|
"kd": {"servo": 0.2},
|
|
"effort_limit": {"servo": 2},
|
|
"control_hz": 50,
|
|
},
|
|
"observation": {"terms": [{"name": "joint_pos"}]},
|
|
}
|
|
artifact = adapt_policy(source, metadata, tmp_path / "adapted", model=model)
|
|
layout = json.loads(artifact.layout.read_text())
|
|
assert layout["passive_joints"] == ["passive"]
|
|
assert layout["unactuated_joints"] == {}
|
|
assert layout["joint_limits"] == {"servo": [-1, 1]}
|
|
controller = OnnxController(
|
|
model, {"layout": "layout.json", "policy": "actor.onnx"}, artifact.layout.parent
|
|
)
|
|
data = mujoco.MjData(model)
|
|
controller.reset(data)
|
|
for _ in range(10):
|
|
obs = controller.observation(data, {})
|
|
with torch.no_grad():
|
|
expected = actor(torch.from_numpy(obs)).numpy()
|
|
controller.step(data)
|
|
np.testing.assert_allclose(controller.last_action, expected[0], atol=1e-6)
|
|
for _ in range(10):
|
|
controller.apply(data)
|
|
mujoco.mj_step(model, data)
|
|
assert data.qfrc_actuator[controller.vadr[1]] == 0
|
|
assert abs(data.qpos[controller.qadr[1]] - 0.4) > 0.05
|
|
assert abs(data.qpos[controller.qadr[0]] - 0.1) > 0.005
|
|
|
|
|
|
def test_onnx_commands_obey_declared_signed_bounds(tmp_path, monkeypatch):
|
|
from mjrl.platform.play import RobotApplication
|
|
|
|
root = tmp_path
|
|
source = root / "source.onnx"
|
|
graph = helper.make_graph(
|
|
[helper.make_node("Identity", ["command"], ["action"])],
|
|
"signed_actor",
|
|
[helper.make_tensor_value_info("command", TensorProto.FLOAT, [1, 1])],
|
|
[helper.make_tensor_value_info("action", TensorProto.FLOAT, [1, 1])],
|
|
)
|
|
proto = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)])
|
|
proto.ir_version = 9
|
|
onnx.save(proto, source)
|
|
xml = root / "robot.xml"
|
|
xml.write_text(
|
|
"""<mujoco><option timestep=".002"/>
|
|
<worldbody><body name="base"><joint name="hinge" range="-60 60"/>
|
|
<geom type="box" size=".1 .1 .1"/></body></worldbody>
|
|
<actuator><motor name="drive" joint="hinge"/></actuator></mujoco>""",
|
|
encoding="utf-8",
|
|
)
|
|
model = mujoco.MjModel.from_xml_path(str(xml))
|
|
metadata = {
|
|
"wire_joint_order": ["hinge"],
|
|
"obs_joint_order": [],
|
|
"action_joint_order": ["hinge"],
|
|
"default_joint_pos": {"hinge": 0.0},
|
|
"action_scale": 1.0,
|
|
"control": {"kp": 10, "kd": 0, "effort_limit": 10, "control_hz": 50},
|
|
"observation": {"terms": [{"name": "commands"}], "command_order": ["lin_vel_y"]},
|
|
}
|
|
fixed = adapt_policy(source, metadata, root / "fixed", model=model)
|
|
controller = OnnxController(model, json.loads(fixed.config.read_text()), fixed.config.parent)
|
|
data = mujoco.MjData(model)
|
|
controller.reset(data)
|
|
np.testing.assert_allclose(controller.step(data, {"lin_vel_y": 0}), [0])
|
|
with pytest.raises(ValueError, match="requires a declared trained range"):
|
|
controller.step(data, {"lin_vel_y": math.nextafter(0, 1)})
|
|
np.testing.assert_allclose(data.ctrl, [0])
|
|
|
|
metadata["command_ranges"] = {"twist": {"lin_vel_y": [0, 0]}}
|
|
zero = adapt_policy(source, metadata, root / "zero", model=model)
|
|
controller = OnnxController(model, json.loads(zero.config.read_text()), zero.config.parent)
|
|
controller.reset(data)
|
|
np.testing.assert_allclose(controller.step(data, {"lin_vel_y": 0}), [0])
|
|
with pytest.raises(ValueError, match="outside trained range"):
|
|
controller.step(data, {"lin_vel_y": math.nextafter(0, 1)})
|
|
|
|
edge = math.nextafter(0.14, 0)
|
|
metadata["command_ranges"] = {"twist": {"lin_vel_y": [-edge, edge]}}
|
|
active = adapt_policy(source, metadata, root, model=model)
|
|
app = RobotApplication(root, {
|
|
"default_mode": "active",
|
|
"model": xml.name,
|
|
"modes": {"active": {
|
|
"contract": active.config.name,
|
|
"controls": active.controls.name,
|
|
"control_mode": "rl_onnx",
|
|
}},
|
|
})
|
|
observed = []
|
|
observation = app.controller.observation
|
|
|
|
def capture(data, command):
|
|
result = observation(data, command)
|
|
observed.append((command["lin_vel_y"], result.copy()))
|
|
return result
|
|
|
|
monkeypatch.setattr(app.controller, "observation", capture)
|
|
command = {"lin_vel_y": -0.14}
|
|
app.step(command)
|
|
assert command == {"lin_vel_y": -0.14}
|
|
assert observed[0][0] == -edge
|
|
np.testing.assert_array_equal(observed[0][1], np.array([[-edge]], dtype=np.float32))
|
|
assert app.controller.last_action[0] == pytest.approx(-0.14)
|
|
assert app.data.qpos[0] < 0
|
|
snapshot = app.snapshot()
|
|
controller_time = app.controller._time
|
|
for value in (-0.140001, 0.140001):
|
|
with pytest.raises(ValueError, match="outside trained range"):
|
|
app.step({"lin_vel_y": value})
|
|
assert app.snapshot() == snapshot
|
|
assert app.controller._time == controller_time
|
|
assert len(observed) == 1
|
|
command = {"lin_vel_y": 0.14}
|
|
app.step(command)
|
|
assert command == {"lin_vel_y": 0.14}
|
|
assert observed[1][0] == edge
|
|
np.testing.assert_array_equal(observed[1][1], np.array([[edge]], dtype=np.float32))
|
|
assert app.controller.last_action[0] == pytest.approx(0.14)
|