Files
chenlin ceca51b0d8
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
Improve generic robot onboarding, walking controls and end-effector interaction
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.
2026-11-06 10:54:58 +08:00

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()