154 lines
5.5 KiB
Python
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)
|