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.
310 lines
11 KiB
Python
310 lines
11 KiB
Python
"""Native checks distinguish coplanarity from support and controls from motion."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import subprocess
|
|
import sys
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
|
|
mujoco = pytest.importorskip("mujoco")
|
|
|
|
from mjrl.onboarding.calibrate import calibrate
|
|
from mjrl.onboarding import scaffold_task
|
|
from mjrl.onboarding.verify import compiled_model_sha256, verify
|
|
|
|
REPO = Path(__file__).resolve().parents[1]
|
|
ARM = """
|
|
<mujoco model="arm">
|
|
<compiler angle="radian"/>
|
|
<option timestep="0.002" gravity="0 0 0" integrator="implicitfast"/>
|
|
<worldbody>
|
|
<body name="base">
|
|
<geom type="box" size="0.05 0.05 0.05"/>
|
|
<body name="arm" pos="0.08 0 0">
|
|
<joint name="elbow" axis="0 0 1" range="-1 1"/>
|
|
<geom type="capsule" fromto="0 0 0 0.20 0 0" size="0.02" mass="0.1"/>
|
|
<site name="tip" pos="0.2 0 0" size="0.01"/>
|
|
</body>
|
|
</body>
|
|
</worldbody>
|
|
<actuator>
|
|
<position name="elbow_ctrl" joint="elbow" kp="25" kv="3"
|
|
ctrlrange="-1 1" forcerange="-10 10"/>
|
|
</actuator>
|
|
</mujoco>
|
|
"""
|
|
|
|
|
|
@pytest.fixture
|
|
def arm(tmp_path: Path) -> Path:
|
|
path = tmp_path / "arm.xml"
|
|
path.write_text(ARM, encoding="utf-8")
|
|
return path
|
|
|
|
|
|
def test_stance_checks_actual_support_and_effort(tmp_path: Path) -> None:
|
|
"""A refined coplanar pose still fails when the same legs cannot support the load."""
|
|
feet, joints = [], []
|
|
legs = []
|
|
for index, (x, y) in enumerate(((-0.09, -0.07), (0.09, -0.07), (-0.09, 0.07), (0.09, 0.07))):
|
|
foot, joint = f"foot{index}", f"slide{index}"
|
|
feet.append(foot)
|
|
joints.append(joint)
|
|
legs.append(f"""
|
|
<body name="leg{index}" pos="{x} {y} -0.20">
|
|
<joint name="{joint}" type="slide" axis="0 0 1" range="-0.05 0.05"/>
|
|
<geom name="{foot}" type="sphere" size="0.025" mass="0.1"/>
|
|
</body>""")
|
|
source = tmp_path / "stance.xml"
|
|
source.write_text(
|
|
f"""
|
|
<mujoco>
|
|
<option timestep="0.001"/>
|
|
<worldbody>
|
|
<body name="base" pos="0 0 0.3">
|
|
<freejoint/>
|
|
<geom type="box" size="0.10 0.08 0.025" mass="0.8"/>
|
|
{"".join(legs)}
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>""",
|
|
encoding="utf-8",
|
|
)
|
|
config = {
|
|
"foot_geoms": feet,
|
|
"height_joints": dict(zip(feet, joints)),
|
|
"pose": {joints[0]: 0.02},
|
|
"effort_limits": dict.fromkeys(joints, 10.0),
|
|
"frequency_hz": 10.0,
|
|
"seconds": 0.5,
|
|
}
|
|
working = calibrate(source, config)
|
|
weak = calibrate(source, {**config, "effort_limits": dict.fromkeys(joints, 0.05)})
|
|
assert working["passed"], working["checks"]
|
|
assert working["solver"]["joint_adjustments"][joints[0]] == pytest.approx(-0.015, abs=1e-6)
|
|
assert working["feet"][feet[0]]["load_fraction_final_window"] == 1.0
|
|
assert working["joints"][joints[0]]["effective_inertia"] == pytest.approx(0.1)
|
|
assert not weak["passed"]
|
|
assert any(item["name"] == "effort_headroom" and not item["passed"] for item in weak["checks"])
|
|
assert any(item["name"] == "foot_coplanarity" and item["passed"] for item in weak["checks"])
|
|
|
|
recipe = {
|
|
"robot_id": "platform_fixture",
|
|
"family": "quadruped",
|
|
"model_path": source.name,
|
|
"action_scales": dict.fromkeys(joints, 0.02),
|
|
}
|
|
config_path = tmp_path / "recipe.json"
|
|
config_path.write_text(json.dumps(recipe), encoding="utf-8")
|
|
calibration_path = tmp_path / "calibration.json"
|
|
calibration_path.write_text(json.dumps(working), encoding="utf-8")
|
|
destination = tmp_path / "tasks"
|
|
command = [
|
|
sys.executable,
|
|
str(REPO / "scripts" / "onboard.py"),
|
|
"scaffold-tier3",
|
|
"--config",
|
|
str(config_path),
|
|
"--calibration",
|
|
str(calibration_path),
|
|
"--tasks-root",
|
|
str(destination),
|
|
]
|
|
result = subprocess.run(command, capture_output=True, text=True, check=False)
|
|
assert result.returncode == 0, result.stderr
|
|
from tasks.robot_spec import read_robot_spec
|
|
|
|
cli_path = destination / "platform_fixture" / "robot_spec.py"
|
|
report = scaffold_task(
|
|
recipe, base_dir=tmp_path, calibration=working, tasks_root=tmp_path / "api_tasks"
|
|
)
|
|
api_path = Path(report["robot_spec"])
|
|
assert {**report, "robot_spec": str(cli_path)} == json.loads(result.stdout)
|
|
assert api_path.read_bytes() == cli_path.read_bytes()
|
|
spec = read_robot_spec(cli_path)
|
|
assert read_robot_spec(api_path) == spec
|
|
assert spec.base_height == pytest.approx(working["base_position"][2])
|
|
assert [joint.name for joint in spec.position_joints] == working["joint_order"]
|
|
assert spec.position_joints[0].kp == working["joints"][joints[0]]["kp"]
|
|
source.write_text(source.read_text(encoding="utf-8") + "\n<!-- edit -->", encoding="utf-8")
|
|
stale = subprocess.run(command, capture_output=True, text=True, check=False)
|
|
assert stale.returncode == 1
|
|
failed_root = tmp_path / "failed_api_tasks"
|
|
with pytest.raises(ValueError, match="different model XML") as error:
|
|
scaffold_task(recipe, base_dir=tmp_path, calibration=working, tasks_root=failed_root)
|
|
assert stale.stderr.strip() == f"scaffold-tier3: {error.value}"
|
|
assert not failed_root.exists()
|
|
|
|
|
|
def test_biped_scaffold_inherits_walking_controls_unless_overridden(tmp_path, neutral_robot):
|
|
from dataclasses import asdict
|
|
from controller.controls import load_controls
|
|
from tasks.robot_spec import read_robot_spec
|
|
|
|
recipe = asdict(neutral_robot)
|
|
explicit = recipe.pop("controls_path")
|
|
for path_fields, expected in (({}, "key_a"), ({"controls_path": explicit}, "key_j"),
|
|
({"controls_path": None}, None)):
|
|
result = scaffold_task({**recipe, **path_fields}, base_dir=tmp_path,
|
|
tasks_root=tmp_path / str(expected))
|
|
path = read_robot_spec(Path(result["robot_spec"])).controls_path
|
|
if expected is None:
|
|
assert path is None
|
|
else:
|
|
assert expected in load_controls(path).key_axes["ang_vel_z"].plus
|
|
|
|
|
|
def test_verify_measures_real_actuated_response(arm: Path) -> None:
|
|
"""A well-shaped graph and finite controls can still drive a wrong target."""
|
|
config = {
|
|
"steps": 1000,
|
|
"actuator_targets": {"elbow_ctrl": 0.3},
|
|
"actuator_controls": {"elbow_ctrl": 0.3},
|
|
"joint_positions": {"elbow": 0.3},
|
|
}
|
|
report = verify(arm, config)
|
|
assert report["passed"], report["stages"]
|
|
assert report["model_sha256"] == hashlib.sha256(arm.read_bytes()).hexdigest()
|
|
assert report["compiled_model_sha256"] == compiled_model_sha256(
|
|
mujoco.MjModel.from_xml_path(str(arm))
|
|
)
|
|
wrong = verify(arm, {**config, "joint_positions": {"elbow": -0.3}})
|
|
assert wrong["stages"]["physics"]["passed"] and not wrong["stages"]["control"]["passed"]
|
|
passive = verify(
|
|
arm,
|
|
{
|
|
**config,
|
|
"actuator_targets": {"elbow_ctrl": 0},
|
|
"actuator_controls": {"elbow_ctrl": 0},
|
|
"joint_positions": {"elbow": 0},
|
|
},
|
|
)
|
|
assert not passive["passed"]
|
|
assert not next(
|
|
item
|
|
for item in passive["stages"]["control"]["checks"]
|
|
if item["name"] == "response_differs_with_actuators_disabled"
|
|
)["passed"]
|
|
|
|
|
|
def test_verify_reports_error_instead_of_passing_without_probes(arm: Path) -> None:
|
|
report = verify(arm, {"actuator_targets": {"elbow_ctrl": 0.3}})
|
|
assert not report["passed"]
|
|
assert not report["stages"]["physics"]["passed"]
|
|
assert not report["stages"]["control"]["passed"]
|
|
|
|
|
|
def test_verify_uses_setup_controller_and_records_provenance(tmp_path: Path, arm: Path) -> None:
|
|
from mjrl.controllers import setup_controller
|
|
|
|
bundle = setup_controller(arm, {"kind": "direct"}, tmp_path / "controller")
|
|
config = {
|
|
"controller": bundle.config.name,
|
|
"controls": bundle.controls.name,
|
|
"steps": 1000,
|
|
"probes": [{"command": {"elbow_ctrl": 0.3}, "joint_positions": {"elbow": 0.3}}],
|
|
}
|
|
report = verify(bundle.model, config, base_dir=bundle.model.parent)
|
|
assert report["passed"], report["stages"]
|
|
assert report["probes"][0]["controller_substeps"] == 10
|
|
probe = report["probes"][0]
|
|
assert probe["controller_updates"] == 100
|
|
assert probe["inputs"] == {"command": {"elbow_ctrl": 0.3}}
|
|
assert [sample["control_ticks"] for sample in probe["samples"]] == [0, 1, 50, 100]
|
|
assert probe["samples"][0]["qpos"] == [0.0]
|
|
assert probe["samples"][-1]["qpos"][0] == pytest.approx(0.3, abs=0.05)
|
|
assert report["controller_sha256"]
|
|
assert report["controls_sha256"] == hashlib.sha256(bundle.controls.read_bytes()).hexdigest()
|
|
|
|
|
|
def test_cli_saves_failed_measurements_and_returns_failure(tmp_path: Path, arm: Path) -> None:
|
|
config = tmp_path / "verify.json"
|
|
config.write_text(
|
|
json.dumps(
|
|
{
|
|
"actuator_targets": {"elbow_ctrl": 0.3},
|
|
"joint_positions": {"elbow": -0.3},
|
|
}
|
|
),
|
|
encoding="utf-8",
|
|
)
|
|
output = tmp_path / "report.json"
|
|
result = subprocess.run(
|
|
[
|
|
sys.executable,
|
|
str(REPO / "scripts" / "onboard.py"),
|
|
"verify",
|
|
"--model",
|
|
str(arm),
|
|
"--config",
|
|
str(config),
|
|
"--report",
|
|
str(output),
|
|
],
|
|
capture_output=True,
|
|
text=True,
|
|
check=False,
|
|
)
|
|
assert result.returncode == 1
|
|
assert json.loads(result.stdout) == json.loads(output.read_text(encoding="utf-8"))
|
|
assert json.loads(result.stdout)["passed"] is False
|
|
|
|
|
|
def test_verify_onnx_policy_measures_native_control(tmp_path: Path, arm: Path) -> None:
|
|
onnx = pytest.importorskip("onnx")
|
|
pytest.importorskip("onnxruntime")
|
|
from onnx import TensorProto, helper
|
|
|
|
actor = tmp_path / "actor.onnx"
|
|
graph = helper.make_graph(
|
|
[
|
|
helper.make_node(
|
|
"Constant",
|
|
[],
|
|
["action"],
|
|
value=helper.make_tensor("target", TensorProto.FLOAT, [1, 1], [0.3]),
|
|
)
|
|
],
|
|
"physical_probe",
|
|
[helper.make_tensor_value_info("observation", TensorProto.FLOAT, [1, 1])],
|
|
[helper.make_tensor_value_info("action", TensorProto.FLOAT, [1, 1])],
|
|
)
|
|
onnx.save(
|
|
helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)], ir_version=8), actor
|
|
)
|
|
layout = tmp_path / "layout.json"
|
|
layout.write_text(
|
|
json.dumps(
|
|
{
|
|
"wire_joint_order": ["elbow"],
|
|
"obs_joint_order": ["elbow"],
|
|
"action_joint_order": ["elbow"],
|
|
"joint_limits": {"elbow": [-1, 1]},
|
|
"default_joint_pos": {"elbow": 0},
|
|
"action_scale": 1,
|
|
"control": {"kp": 25, "kd": 0.3, "effort_limit": 10, "control_hz": 50},
|
|
"observation": {"dim": 1, "terms": [{"name": "joint_pos", "dim": 1, "offset": 0}]},
|
|
"action": {"dim": 1},
|
|
}
|
|
),
|
|
encoding="utf-8",
|
|
)
|
|
report = verify(
|
|
arm,
|
|
{
|
|
"controller": {"kind": "rl_onnx", "policy": actor.name, "layout": layout.name},
|
|
"steps": 1000,
|
|
"joint_positions": {"elbow": 0.3},
|
|
},
|
|
)
|
|
assert report["passed"], report["stages"]
|
|
assert report["probes"][0]["controller_updates"] == 100
|
|
assert report["controller_dependencies_sha256"] == {
|
|
"policy": hashlib.sha256(actor.read_bytes()).hexdigest(),
|
|
"layout": hashlib.sha256(layout.read_bytes()).hexdigest(),
|
|
}
|