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
"""Calibrated motor parity and observation frames for imported real policies."""
|
|
|
|
import json
|
|
|
|
import numpy as np
|
|
import onnx
|
|
from onnx import TensorProto, helper
|
|
import pytest
|
|
|
|
mujoco = pytest.importorskip("mujoco")
|
|
pytest.importorskip("bam")
|
|
|
|
from bam.model import load_model
|
|
from bam.mujoco import MujocoController
|
|
|
|
from mjrl.platform.motors import BamMotorModel, validate_motor_model
|
|
from mjrl.platform.policy import OnnxController
|
|
|
|
CONFIG = {
|
|
"kind": "bam", "motor_name": "xl330", "model": "m6", "kp": 100,
|
|
"vin": 7.0, "max_current": 1.5, "vin_drop_resistance": 0.1, "vin_min": 6,
|
|
}
|
|
XML = """<mujoco>
|
|
<compiler angle="radian"/><option timestep=".002"/>
|
|
<worldbody><body name="base" pos="0 0 1"><freejoint/>
|
|
<geom size=".1" mass="1"/>
|
|
<site name="imu" quat=".5 -.5 -.5 .5" pos=".1 0 0"/>
|
|
<body pos=".2 0 0"><joint name="a" range="-2 2" damping=".041" frictionloss=".013"/>
|
|
<geom type="capsule" fromto="0 0 0 .1 0 0" size=".02" mass=".1"/></body>
|
|
<body pos="-.2 0 0"><joint name="b" range="-2 2" damping=".041" frictionloss=".013"/>
|
|
<geom type="capsule" fromto="0 0 0 -.1 0 0" size=".02" mass=".1"/></body>
|
|
</body></worldbody>
|
|
<actuator><motor name="motor_b" joint="b"/><motor name="motor_a" joint="a"/></actuator>
|
|
</mujoco>"""
|
|
|
|
|
|
def test_bam_matches_upstream_dynamics_and_repeated_reset():
|
|
model = mujoco.MjModel.from_xml_string(XML)
|
|
actual = mujoco.MjData(model)
|
|
actual.qpos[:3] = [.2, -.3, 1.1]
|
|
preserved = actual.qpos.copy()
|
|
bridge = BamMotorModel(model, [1, 0], [.6405, .6405], CONFIG)
|
|
np.testing.assert_array_equal(actual.qpos, preserved)
|
|
original = mujoco.MjModel.from_xml_string(XML)
|
|
expected = mujoco.MjData(original)
|
|
motor = load_model(motor_name="xl330", model="m6")
|
|
motor.actuator.kp = 100
|
|
motor.actuator.vin = 7.0
|
|
motor.actuator.max_current = 1.5
|
|
upstream = MujocoController(
|
|
motor, ["motor_a", "motor_b"], original, expected,
|
|
vin_drop_resistance=.1, vin_min=6,
|
|
)
|
|
traces = []
|
|
for _ in range(2):
|
|
mujoco.mj_resetData(model, actual)
|
|
actual.qpos[:] = preserved
|
|
bridge.reset(actual, [0, 0])
|
|
mujoco.mj_forward(model, actual)
|
|
trace = []
|
|
for tick in range(100):
|
|
target = [.65 * np.sin(tick * .1), -.2]
|
|
bridge.apply(actual, target)
|
|
first = actual.ctrl.copy()
|
|
bridge.apply(actual, [10, 10]) # Duplicate physics tick cannot consume state twice.
|
|
np.testing.assert_array_equal(actual.ctrl, first)
|
|
mujoco.mj_step(model, actual)
|
|
trace.append(np.r_[actual.qpos, actual.qvel, actual.ctrl])
|
|
traces.append(trace)
|
|
np.testing.assert_array_equal(traces[0], traces[1])
|
|
expected.qpos[:] = preserved
|
|
motor.reset()
|
|
upstream.last_ts = expected.time
|
|
mujoco.mj_forward(original, expected)
|
|
reference = []
|
|
for tick in range(100):
|
|
upstream.q_target = np.array([.65 * np.sin(tick * .1), -.2])
|
|
upstream.update()
|
|
expected.ctrl[:] = np.clip(expected.ctrl, -.6405, .6405)
|
|
mujoco.mj_step(original, expected)
|
|
reference.append(np.r_[expected.qpos, expected.qvel, expected.ctrl])
|
|
np.testing.assert_allclose(traces[0], reference, rtol=0, atol=1e-12)
|
|
assert np.max(np.abs(np.asarray(traces[0])[:, -2:])) > .1
|
|
assert not np.allclose(model.dof_frictionloss, .013)
|
|
|
|
|
|
@pytest.mark.parametrize("change", [
|
|
{"kind": "other"}, {"model": "../m6"}, {"vin": 0}, {"vin_min": 8},
|
|
{"max_current": float("inf")}, {"vin_drop_resistance": -1}, {"extra": 1},
|
|
])
|
|
def test_bam_rejects_invalid_configuration(change):
|
|
with pytest.raises(ValueError):
|
|
validate_motor_model({**CONFIG, **change})
|
|
|
|
|
|
def test_site_gyro_and_body_gravity_are_distinct_observation_frames(tmp_path):
|
|
graph = helper.make_graph(
|
|
[helper.make_node("Constant", [], ["actions"], value=helper.make_tensor(
|
|
"target", TensorProto.FLOAT, [1, 2], [0, 0]))],
|
|
"constant", [helper.make_tensor_value_info("obs", TensorProto.FLOAT, [1, 9])],
|
|
[helper.make_tensor_value_info("actions", TensorProto.FLOAT, [1, 2])],
|
|
)
|
|
proto = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)])
|
|
proto.ir_version = 9
|
|
onnx.save(proto, tmp_path / "actor.onnx")
|
|
layout = {
|
|
"_schema": "mjrl-native-layout/1", "wire_joint_order": ["a", "b"],
|
|
"obs_joint_order": ["a", "b"], "action_joint_order": ["a", "b"],
|
|
"default_joint_pos": {"a": 0, "b": 0}, "action_scale": {"a": 1, "b": 1},
|
|
"joint_limits": {"a": [-2, 2], "b": [-2, 2]},
|
|
"control": {"kp": 1, "kd": 0, "control_hz": 50, "effort_limit": .6405},
|
|
"action": {"dim": 2, "targets": {
|
|
"a": {"kind": "position", "offset": 0}, "b": {"kind": "position", "offset": 0}}},
|
|
"observation": {"dim": 9, "terms": [
|
|
{"name": "base_ang_vel", "dim": 3}, {"name": "projected_gravity", "dim": 3},
|
|
{"name": "base_lin_vel", "dim": 3}],
|
|
"frames": {"base_ang_vel": {"kind": "site", "name": "imu"},
|
|
"base_lin_vel": {"kind": "site", "name": "imu"}}},
|
|
"motor_model": CONFIG,
|
|
}
|
|
(tmp_path / "layout.json").write_text(json.dumps(layout))
|
|
model = mujoco.MjModel.from_xml_string(XML)
|
|
controller = OnnxController(model, {"layout": "layout.json", "policy": "actor.onnx"}, tmp_path)
|
|
data = mujoco.MjData(model)
|
|
controller.reset(data)
|
|
data.qvel[:6] = [.1, .2, .3, 1, 2, 3]
|
|
mujoco.mj_forward(model, data)
|
|
site_velocity = np.zeros(6)
|
|
mujoco.mj_objectVelocity(model, data, mujoco.mjtObj.mjOBJ_SITE, model.site("imu").id,
|
|
site_velocity, 1)
|
|
observation = controller.observation(data, {})[0]
|
|
np.testing.assert_allclose(observation[:3], site_velocity[:3], atol=1e-7)
|
|
np.testing.assert_allclose(observation[3:6], [0, 0, -1], atol=1e-7)
|
|
np.testing.assert_allclose(observation[6:], site_velocity[3:], atol=1e-7)
|
|
assert not np.allclose(observation[:3], data.qvel[3:6])
|
|
controller.step(data)
|
|
snapshot = data.ctrl.copy()
|
|
controller.apply(data)
|
|
np.testing.assert_array_equal(data.ctrl, snapshot)
|
|
|
|
|
|
def test_passive_verification_replays_dynamic_friction_independently():
|
|
from mjrl.onboarding.verify import _run_probe, _THRESHOLDS
|
|
from mjrl.platform.velocity import VelocityWindow
|
|
|
|
class Controller:
|
|
reset_passive = OnnxController.reset_passive
|
|
apply_passive = OnnxController.apply_passive
|
|
|
|
def __init__(self, model):
|
|
self.model = model
|
|
self.actuators = np.array([1, 0])
|
|
self.driven_idx = np.array([0, 1])
|
|
self.qadr = np.array([7, 8])
|
|
self.motor_model = BamMotorModel(model, self.actuators, [.6405, .6405], CONFIG)
|
|
self.target = np.zeros(2)
|
|
|
|
def reset(self, data):
|
|
self.motor_model.reset(data, data.qpos[self.qadr])
|
|
|
|
def step(self, data, command):
|
|
self.target[:] = command["target"]
|
|
self.apply(data)
|
|
|
|
def apply(self, data):
|
|
return self.motor_model.apply(data, self.target)
|
|
|
|
model = mujoco.MjModel.from_xml_string(XML)
|
|
controller = Controller(model)
|
|
initial = mujoco.MjData(model)
|
|
initial.qpos[-2:] = [.3, -.5]
|
|
initial.qvel[3:6] = [.5, -.3, 1]
|
|
initial.qvel[-2:] = [4, -3]
|
|
mujoco.mj_forward(model, initial)
|
|
original_qpos = initial.qpos.copy()
|
|
metric = "base_angular_velocity_mean_rad_s"
|
|
specs = {metric: {"target": [0, 0, 0], "tolerance": [1, 1, 1]}}
|
|
results = []
|
|
for command in (-1.5, 1.5):
|
|
_, _, measurements = _run_probe(
|
|
model, initial, 1, {"a": 1, "b": 2}, controller,
|
|
{"command": {"target": command}, "expect": specs}, _THRESHOLDS, None, {}, 250, 10,
|
|
)
|
|
results.append(measurements["velocity_evaluation"]["metrics"][metric])
|
|
np.testing.assert_array_equal(initial.qpos, original_qpos)
|
|
# Active commands genuinely differ; the counterfactual must not inherit
|
|
# their final load-dependent friction or motor state.
|
|
assert not np.allclose(results[0]["observed"], results[1]["observed"])
|
|
np.testing.assert_array_equal(
|
|
results[0]["actuation_disabled_observed"], results[1]["actuation_disabled_observed"]
|
|
)
|
|
|
|
original = mujoco.MjModel.from_xml_string(XML)
|
|
baseline = mujoco.MjData(original)
|
|
motor = load_model(motor_name="xl330", model="m6")
|
|
motor.actuator.kp, motor.actuator.vin, motor.actuator.max_current = 100, 7.0, 1.5
|
|
upstream = MujocoController(
|
|
motor, ["motor_a", "motor_b"], original, baseline,
|
|
vin_drop_resistance=.1, vin_min=6,
|
|
)
|
|
baseline.qpos[:] = initial.qpos
|
|
baseline.qvel[:] = initial.qvel
|
|
original.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_ACTUATION
|
|
mujoco.mj_forward(original, baseline)
|
|
window = VelocityWindow(1, 0, .5, specs)
|
|
for index in range(250):
|
|
upstream.q_target = baseline.qpos[upstream.qpos_indexes].copy()
|
|
upstream.update()
|
|
baseline.ctrl[:] = 0
|
|
mujoco.mj_step(original, baseline)
|
|
mujoco.mj_forward(original, baseline)
|
|
window.sample(original, baseline, (index + 1) * .002)
|
|
np.testing.assert_allclose(
|
|
results[0]["actuation_disabled_observed"], window.results()[metric]["observed"],
|
|
rtol=0, atol=1e-12,
|
|
)
|
|
|
|
# A frozen-friction control reproduces the old bug and must be distinguishable.
|
|
controller.reset_passive = None
|
|
controller.apply_passive = None
|
|
_, _, stale = _run_probe(
|
|
model, initial, 1, {"a": 1, "b": 2}, controller,
|
|
{"command": {"target": 1.5}, "expect": specs}, _THRESHOLDS, None, {}, 250, 10,
|
|
)
|
|
assert not np.allclose(
|
|
stale["velocity_evaluation"]["metrics"][metric]["actuation_disabled_observed"],
|
|
window.results()[metric]["observed"], atol=1e-5,
|
|
)
|
|
|
|
|
|
@pytest.mark.parametrize("python_version", [(3, 10), (3, 11), (3, 13)])
|
|
def test_bam_reports_unsupported_python_before_loading_motor(monkeypatch, python_version):
|
|
from types import SimpleNamespace
|
|
|
|
from mjrl.platform import motors
|
|
|
|
monkeypatch.setattr(motors, "sys", SimpleNamespace(version_info=python_version))
|
|
with pytest.raises(ValueError, match="requires Python >=3.12,<3.13"):
|
|
BamMotorModel(None, [], [], CONFIG)
|
|
|
|
|
|
def test_bam_bundle_declares_dependency_python_constraint(tmp_path):
|
|
from mjrl.adapter import adapt_policy
|
|
from mjrl.onboarding.verify import verify
|
|
from mjrl.platform.bundle import build_bundle, open_bundle
|
|
|
|
model_path = tmp_path / "robot.xml"
|
|
model_path.write_text(XML.replace("<freejoint/>", ""))
|
|
model = mujoco.MjModel.from_xml_path(str(model_path))
|
|
graph = helper.make_graph(
|
|
[helper.make_node("Constant", [], ["actions"], value=helper.make_tensor(
|
|
"target", TensorProto.FLOAT, [1, 2], [.4, -.4]))],
|
|
"constant", [helper.make_tensor_value_info("obs", TensorProto.FLOAT, [1, 2])],
|
|
[helper.make_tensor_value_info("actions", TensorProto.FLOAT, [1, 2])],
|
|
)
|
|
proto = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)])
|
|
proto.ir_version = 9
|
|
onnx.save(proto, tmp_path / "source.onnx")
|
|
metadata = {
|
|
"wire_joint_order": ["a", "b"], "obs_joint_order": ["a", "b"],
|
|
"action_joint_order": ["a", "b"], "default_joint_pos": {"a": 0, "b": 0},
|
|
"action_scale": 1,
|
|
"control": {"kp": 1, "kd": 0, "control_hz": 50, "effort_limit": .6405},
|
|
"observation": {"terms": [{"name": "joint_pos"}]}, "motor_model": CONFIG,
|
|
}
|
|
result = adapt_policy(tmp_path / "source.onnx", metadata, tmp_path / "policy", model=model)
|
|
report = verify(model_path, {
|
|
"controller": str(result.config), "root_body": "base", "steps": 2000,
|
|
"probes": [{"command": {}, "joint_positions": {"a": .4, "b": -.4}}],
|
|
})
|
|
assert report["passed"], report
|
|
report_path = tmp_path / "verification.json"
|
|
report_path.write_text(json.dumps(report))
|
|
app = build_bundle(model_path, tmp_path / "bundle", robot_id="bench", modes={
|
|
"move": {"controller": result.config, "policy": result.actor,
|
|
"layout": result.layout, "verification": report_path},
|
|
})
|
|
with open_bundle(app) as (root, manifest):
|
|
assert manifest["runtimes"]["native"]["python"] == ">=3.12,<3.13"
|
|
assert "better-actuator-models==1.0.2" in (root / "runtime/requirements.txt").read_text()
|
|
viewer = build_bundle(model_path, tmp_path / "viewer", robot_id="bench", modes={
|
|
"view": {"control_mode": "model_viewer"},
|
|
})
|
|
with open_bundle(viewer) as (root, manifest):
|
|
assert manifest["runtimes"]["native"]["python"] == ">=3.10,<3.14"
|
|
assert "better-actuator-models" not in (root / "runtime/requirements.txt").read_text()
|