Files
UniversalHumanoid/tests/test_robot_check.py
T
2026-11-03 19:46:08 +08:00

154 lines
5.5 KiB
Python

"""Negative conformance checks: plausible-looking adapters must not pass the wrong contract."""
import copy
import pytest
from universal_humanoid.robot_check import CHECK_IDS, assess, load_spec
def valid_facts(spec):
bodies = spec["contact_bodies"]
return {
"free_root_count": 1,
"foot_sites": {
n: {
"body": b,
"height_m": 0.02,
"local_position_m": spec["foot_site_positions_m"][n],
"local_quaternion": spec["foot_site_quaternions_wxyz"][n],
}
for n, b in spec["foot_sites"].items()
},
"imu": {
n: {
"type": kind,
"dimension": 3,
"site": spec["imu_site"],
"body": spec["imu_body"],
"site_position_m": spec["imu_site_position_m"],
"site_quaternion": spec["imu_site_quaternion_wxyz"],
}
for n, kind in spec["imu_sensors"].items()
},
"action_joint_order": spec["action_joint_order"],
"observation_joint_order": spec["observation_joint_order"],
"motors": {
m["joint"]: {
**m,
"count": 1,
"joint_transmission": True,
"force_limited": True,
"joint_limited": True,
"hinge_joint": True,
}
for m in spec["motors"]
},
"action_transform": {
k: spec[k] for k in ("action_scale", "action_offset", "policy_dt", "physics_dt")
},
"initial_pose": {
"default_joint_position": spec["default_joint_position_rad"],
"actual_joint_position": spec["default_joint_position_rad"],
"default_root_position": spec["default_root_position_m"],
"default_root_quaternion": spec["default_root_quaternion_wxyz"],
"actual_root_quaternion": spec["default_root_quaternion_wxyz"],
"actual_root_position": [
v + (0.02 if i == 2 else 0) for i, v in enumerate(spec["default_root_position_m"])
],
"actual_root_velocity": [0.0] * 6,
"actual_joint_velocity": [0.0] * len(spec["observation_joint_order"]),
"within_joint_limits": True,
"upright_cosine": 1.0,
},
"initial_penetration_m": 0,
"contacts": {
"bodies": bodies,
"terrain_filter": True,
"has_force_and_found": True,
"collision_enabled": dict.fromkeys(bodies, True),
"observed": dict.fromkeys(bodies, True),
"peak_force_n": dict.fromkeys(bodies, 100),
},
"probe": {"finite": True, "terminations": 0},
}
@pytest.mark.parametrize("task", ["g1.flat", "t1.flat"])
def test_declared_baseline_passes(task):
spec = load_spec(task)
checks = assess(spec, valid_facts(spec))
assert {c["id"] for c in checks} == set(CHECK_IDS)
assert all(c["passed"] for c in checks)
@pytest.mark.parametrize(
"case,check",
[
("site", "foot_sites"),
("site_position", "foot_sites"),
("reset_velocity", "initial_pose"),
("imu_type", "imu"),
("imu_body", "imu"),
("imu_frame", "imu"),
("order", "joint_order"),
("kp", "pd_and_limits"),
("limit", "pd_and_limits"),
("pose", "initial_pose"),
("penetration", "initial_penetration"),
("collision", "contact_configuration"),
("contact", "contact_response"),
("force", "contact_response"),
("finite", "finite_probe"),
("termination", "finite_probe"),
("timing", "action_transform"),
("root", "free_root"),
],
)
def test_corrupted_mechanics_fail(case, check):
spec = load_spec("g1.flat")
facts = copy.deepcopy(valid_facts(spec))
foot = spec["contact_bodies"][0]
joint = spec["action_joint_order"][0]
sensor = next(iter(spec["imu_sensors"]))
if case == "site":
facts["foot_sites"].pop(next(iter(spec["foot_sites"])))
elif case == "site_position":
facts["foot_sites"][next(iter(spec["foot_sites"]))]["local_position_m"][0] = 1.0
elif case == "reset_velocity":
facts["initial_pose"]["actual_root_velocity"][0] = 2.0
elif case.startswith("imu_"):
key, value = {
"imu_type": ("type", "accelerometer"),
"imu_body": ("body", foot),
"imu_frame": ("site_quaternion", [0, 1, 0, 0]),
}[case]
facts["imu"][sensor][key] = value
elif case == "order":
facts["action_joint_order"].reverse()
elif case == "kp":
facts["motors"][joint]["kp"] = -1
elif case == "limit":
facts["motors"][joint]["force_limited"] = False
elif case == "pose":
facts["initial_pose"]["upright_cosine"] = 0.5
elif case == "penetration":
facts["initial_penetration_m"] = 1
elif case == "collision":
facts["contacts"]["collision_enabled"][foot] = False
elif case == "contact":
facts["contacts"]["observed"][foot] = False
elif case == "force":
facts["contacts"]["peak_force_n"][foot] = 0
elif case == "finite":
facts["probe"]["finite"] = False
elif case == "termination":
facts["probe"]["terminations"] = 1
elif case == "timing":
facts["action_transform"]["policy_dt"] *= 2
elif case == "root":
facts["free_root_count"] = 0
result = {c["id"]: c["passed"] for c in assess(spec, facts)}
assert result[check] is False
assert all(value for key, value in result.items() if key != check)