Files
UniversalTrain-ai/tests/test_locomotion_revision.py
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

162 lines
8.0 KiB
Python

"""Behavioral controls for the shared locomotion template revision."""
from pathlib import Path
from types import SimpleNamespace as NS
import pytest
import torch
from mjrl.tasks.mdp import (
base_roll_pitch_velocity,
base_vertical_velocity,
joint_home_deviation,
missing_support,
support_air_time,
support_slip,
)
def _env(data, sensor=None, command=(0.2, 0, 0)):
scene = {"robot": NS(data=data)}
if sensor is not None:
scene["support"] = sensor
return NS(scene=scene, num_envs=1, step_dt=.02,
command_manager=NS(get_command=lambda _: torch.tensor([command])))
def test_home_and_body_damping_do_not_penalize_wheel_angle_or_yaw():
data = NS(joint_pos=torch.tensor([[.3, 200.]]), default_joint_pos=torch.tensor([[.1, 0.]]),
root_link_ang_vel_b=torch.tensor([[0., 0., 4.]]),
root_link_lin_vel_b=torch.tensor([[2., 0., 0.]]))
cfg = NS(name="robot", joint_ids=[0])
env = _env(data)
assert joint_home_deviation(env, cfg, "twist").item() == pytest.approx(.04)
assert joint_home_deviation(_env(data, command=(0., 0., 0.)), cfg, "twist").item() == pytest.approx(.12)
assert base_roll_pitch_velocity(env).item() == 0
assert base_vertical_velocity(env).item() == 0
data.root_link_ang_vel_b[0, :2] = torch.tensor([1., 2.])
data.root_link_lin_vel_b[0, 2] = 3
assert base_roll_pitch_velocity(env).item() == 5
assert base_vertical_velocity(env).item() == 9
def test_only_completed_swings_pay_and_missing_middle_feet_cost():
sensor = NS(data=NS(found=torch.tensor([[1, 1, 0, 0, 1, 1]]),
current_air_time=torch.tensor([[0., 0., 2., 2., 0., 0.]]),
last_air_time=torch.full((1, 6), .2)),
compute_first_contact=lambda dt: torch.zeros(1, 6, dtype=torch.bool))
env = _env(NS(), sensor)
params = dict(sensor_name="support", threshold_min=.06, threshold_max=.35,
command_name="twist", command_threshold=.08)
assert support_air_time(env, **params).item() == 0 # Hovering never earns a swing bonus.
assert missing_support(env, "support", "twist").item() == pytest.approx(2/6)
sensor.data.current_air_time[:, 2:4] = .2
assert missing_support(env, "support", "twist").item() == 0 # Normal swings allowed.
assert missing_support(_env(NS(), sensor, command=(0., 0., 0.)), "support", "twist").item() == pytest.approx(2/6)
sensor.compute_first_contact = lambda dt: torch.ones(1, 6, dtype=torch.bool)
assert (support_air_time(env, **params) * env.step_dt).item() == pytest.approx(.14)
assert support_air_time(_env(NS(), sensor, command=(0., 0., 0.)), **params).item() == 0
@pytest.mark.parametrize("mode", ["geom", "body"])
def test_slip_uses_sensor_order_and_ignores_airborne_velocity(mode):
data = NS(geom_lin_vel_w=torch.tensor([[[100., 0., 0.], [2., 0., 8.]]]),
body_link_lin_vel_w=torch.tensor([[[100., 0., 0.], [2., 0., 8.]]]))
sensor = NS(cfg=NS(primary=NS(mode=mode)), primary_names=["second", "first"],
data=NS(found=torch.tensor([[1, 0]])))
env = _env(data, sensor)
def find(patterns, preserve_order):
assert patterns == ("second", "first") and preserve_order
return [1, 0], list(patterns)
env.scene["robot"].find_geoms = find
env.scene["robot"].find_bodies = find
assert support_slip(env, "support").item() == 2 # 2**2 / two supports, not 100**2.
sensor.data.found[:] = 0
assert support_slip(env, "support").item() == 0
def test_training_only_randomization_and_wheel_exclusion(neutral_robot):
from tasks.templates.Biped_Template.env_cfg import env_cfg
from tasks.templates.Wheeled_Biped_Template.env_cfg import env_cfg as wheel_cfg
cfg = env_cfg(neutral_robot)
assert cfg.observations["actor"].history_length == 4
assert {"pose", "single_support", "lin_vel_z", "ang_vel_xy", "support_slip", "missing_support"} <= cfg.rewards.keys()
assert {"base_density", "base_com", "pd_gains", "support_friction"} <= cfg.events.keys()
play = env_cfg(neutral_robot, play=True)
assert play.observations["actor"].history_length == 4
assert set(play.events) == {"reset_base", "reset_joints"}
from tasks.robot_spec import read_robot_spec
wheel = wheel_cfg(read_robot_spec(
Path(__file__).resolve().parents[1] / "tasks/wheeled_biped_example/robot_spec.py"
))
assert "support_slip" not in wheel.rewards
assert "swing_air_time" not in wheel.rewards
assert wheel.scene.sensors[0].track_air_time
def test_native_history_reset_export_and_actual_randomized_parameters(neutral_robot, monkeypatch):
monkeypatch.syspath_prepend(str(Path(__file__).resolve().parents[1] / "scripts"))
from mjlab.envs import ManagerBasedRlEnv
from mjlab.sim import get_simulation_cls, set_simulation_cls
from mjrl.backend import native_sim
from mjrl.backend.resolve import resolve
from mjrl.backend.select import use_backend
from scripts.export import _obs_terms
from tasks.templates.Biped_Template.env_cfg import env_cfg
saved_cls, threads = get_simulation_cls(), native_sim._DEFAULT_NTHREAD
use_backend(resolve(backend="native", device="cpu", num_envs=4))
env = None
try:
cfg = env_cfg(neutral_robot)
cfg.scene.num_envs = 4
cfg.seed = 7
cfg.observations["actor"].enable_corruption = False
env = ManagerBasedRlEnv(cfg, device="cpu")
obs, _ = env.reset()
assert obs["actor"].shape == (4, 4 * (9 + 3 * 2))
terms, width = _obs_terms(env)
assert width == obs["actor"].shape[1]
assert all(term["history_length"] == 4 for term in terms)
assert sum(term["dim"] for term in terms) == obs["actor"].shape[1]
# Root angular velocity is the first term. Histories are term-major,
# oldest first, and reset fills every frame rather than leaking episodes.
history = obs["actor"][:, :12].reshape(4, 4, 3)
torch.testing.assert_close(history, history[:, -1:, :].expand_as(history))
saw_distinct_frames = False
for _ in range(5):
previous = obs["actor"][:, :12].reshape(4, 4, 3).clone()
obs, reward, _, _, _ = env.step(torch.full(env.action_space.shape, .2))
assert torch.isfinite(reward).all() and torch.isfinite(obs["actor"]).all()
current = obs["actor"][:, :12].reshape(4, 4, 3)
torch.testing.assert_close(current[:, :-1], previous[:, 1:])
torch.testing.assert_close(current[:, -1], env.scene["robot"].data.root_link_ang_vel_b)
saw_distinct_frames |= not torch.allclose(current[:, 0], current[:, -1])
assert saw_distinct_frames # A constant-zero history must not pass vacuously.
obs, _ = env.reset()
history = obs["actor"][:, :12].reshape(4, 4, 3)
torch.testing.assert_close(history, history[:, -1:, :].expand_as(history))
robot = env.scene["robot"]
bid = int(robot.indexing.root_body_id)
default_mass = env.sim.get_default_field("body_mass")[bid]
ratios = env.sim.model.body_mass[:, bid] / default_mass
assert ((ratios >= .9) & (ratios <= 1.1)).all()
assert ratios.std() > 0 and not torch.allclose(ratios, torch.ones_like(ratios))
default_inertia = env.sim.get_default_field("body_inertia")[bid]
# Principal values may be permuted by eigendecomposition; trace scales.
inertia_ratio = env.sim.model.body_inertia[:, bid].sum(-1) / default_inertia.sum()
torch.testing.assert_close(inertia_ratio, ratios)
for actuator in robot.actuators:
kp_ratio = actuator.stiffness / actuator.default_stiffness
kd_ratio = actuator.damping / actuator.default_damping
assert ((kp_ratio >= .9) & (kp_ratio <= 1.1)).all()
assert ((kd_ratio >= .9) & (kd_ratio <= 1.1)).all()
assert kp_ratio.std() > 0 and kd_ratio.std() > 0
finally:
if env is not None:
env.close()
set_simulation_cls(saved_cls)
native_sim._DEFAULT_NTHREAD = threads