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.
123 lines
5.0 KiB
Python
123 lines
5.0 KiB
Python
"""Measure command populations and exercise inherited partial-reset lifecycle."""
|
|
|
|
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
import torch
|
|
|
|
from mjrl.tasks.commands import AxisBalancedVelocityCommand, AxisBalancedVelocityCommandCfg
|
|
|
|
|
|
def _config(**kwargs):
|
|
return AxisBalancedVelocityCommandCfg(
|
|
entity_name="robot", resampling_time_range=(3., 7.),
|
|
ranges=AxisBalancedVelocityCommandCfg.Ranges((-.7, .7), (-.35, .35), (-.8, .8)),
|
|
**{"rel_standing_envs": .1, **kwargs},
|
|
)
|
|
|
|
|
|
def _term(count=30000, **kwargs):
|
|
zeros = torch.zeros(count, 3)
|
|
robot = SimpleNamespace(data=SimpleNamespace(root_link_lin_vel_b=zeros, root_link_ang_vel_b=zeros))
|
|
env = SimpleNamespace(num_envs=count, device="cpu", step_dt=.02, scene={"robot": robot})
|
|
return _config(**kwargs).build(env)
|
|
|
|
|
|
def test_population_has_balanced_isolated_axes_both_signs_standing_and_mixed_commands():
|
|
torch.manual_seed(1027)
|
|
term = _term()
|
|
assert isinstance(term, AxisBalancedVelocityCommand)
|
|
term.reset(torch.arange(term.num_envs))
|
|
term.compute(0.)
|
|
values = term.command
|
|
standing = term.is_standing_env
|
|
moving = ~standing
|
|
assert standing.float().mean().item() == pytest.approx(.1, abs=.012)
|
|
assert torch.count_nonzero(values[standing]) == 0
|
|
active_axes = (values != 0).sum(dim=1)
|
|
isolated = moving & (active_axes == 1)
|
|
assert isolated.sum().item() / moving.sum().item() == pytest.approx(.3, abs=.015)
|
|
assert ((active_axes[moving] == 1) | (active_axes[moving] == 3)).all()
|
|
assert (active_axes == 3).float().mean().item() == pytest.approx(.63, abs=.02)
|
|
for axis, maximum in enumerate((.7, .35, .8)):
|
|
assert (values[:, axis].abs() <= maximum).all()
|
|
chosen = isolated & (values[:, axis] != 0)
|
|
assert chosen.sum().item() / isolated.sum().item() == pytest.approx(1/3, abs=.025)
|
|
assert (values[chosen, axis] > 0).float().mean().item() == pytest.approx(.5, abs=.04)
|
|
assert values[chosen, axis].min() < -.9 * maximum
|
|
assert values[chosen, axis].max() > .9 * maximum
|
|
original = values.clone()
|
|
term.compute(.01)
|
|
torch.testing.assert_close(values, original, atol=0, rtol=0)
|
|
|
|
|
|
def test_isolation_keeps_original_sampled_axis_and_partial_reset_keeps_other_envs():
|
|
def sampled(fraction):
|
|
torch.manual_seed(29)
|
|
term = _term(256, isolated_axis_fraction=fraction, rel_standing_envs=0.)
|
|
term.reset(torch.arange(256))
|
|
term.compute(0.)
|
|
return term
|
|
|
|
mixed, isolated = sampled(0.), sampled(1.)
|
|
mask = isolated.command != 0
|
|
assert (mask.sum(dim=1) == 1).all()
|
|
assert (mixed.command != 0).all() # The control actually includes all axes.
|
|
torch.testing.assert_close(isolated.command[mask], mixed.command[mask], atol=0, rtol=0)
|
|
before = {name: getattr(isolated, name).clone() for name in
|
|
("vel_command_b", "vel_command_w", "time_left", "command_counter", "is_standing_env")}
|
|
ids = torch.tensor([1, 7, 19, 97])
|
|
untouched = torch.ones(256, dtype=torch.bool)
|
|
untouched[ids] = False
|
|
isolated.reset(ids)
|
|
isolated.compute(0., env_ids=ids)
|
|
for name, old in before.items():
|
|
torch.testing.assert_close(getattr(isolated, name)[untouched], old[untouched], atol=0, rtol=0)
|
|
assert not torch.equal(isolated.command[ids], before["vel_command_b"][ids])
|
|
assert (isolated.command_counter[ids] == 1).all()
|
|
assert ((isolated.time_left[ids] >= 3) & (isolated.time_left[ids] <= 7)).all()
|
|
|
|
|
|
@pytest.mark.parametrize("field", ["isolated_axis_fraction", "rel_standing_envs", "init_velocity_prob"])
|
|
@pytest.mark.parametrize("value", [-.1, 1.1, float("nan"), float("inf"), True, "0.3"])
|
|
def test_invalid_probabilities_fail(field, value):
|
|
with pytest.raises(ValueError, match=field):
|
|
_config(**{field: value})
|
|
|
|
|
|
@pytest.mark.parametrize("kwargs", [
|
|
{"heading_command": True}, {"rel_world_envs": .1}, {"rel_forward_envs": .1},
|
|
{"rel_world_envs": float("nan")},
|
|
])
|
|
def test_modes_that_overwrite_axes_are_rejected(kwargs):
|
|
with pytest.raises(ValueError):
|
|
_config(**kwargs)
|
|
|
|
|
|
@pytest.mark.parametrize("bounds", [(.1, .5), (-.5, -.1), (.5, -.5), (float("nan"), .5)])
|
|
def test_ranges_must_allow_zero_masking(bounds):
|
|
cfg = _config()
|
|
cfg.ranges.lin_vel_x = bounds
|
|
with pytest.raises(ValueError, match="lin_vel_x"):
|
|
cfg.build(None) # Validation precedes environment access.
|
|
|
|
|
|
def test_heading_range_and_mutated_mode_are_rejected():
|
|
cfg = _config()
|
|
cfg.ranges.heading = (-1., 1.)
|
|
with pytest.raises(ValueError, match="without heading"):
|
|
cfg.build(None)
|
|
cfg = _config()
|
|
cfg.rel_world_envs = .2
|
|
with pytest.raises(ValueError, match="body-frame"):
|
|
cfg.build(None)
|
|
|
|
|
|
def test_all_standing_stays_zero_with_isolation_enabled():
|
|
term = _term(16, rel_standing_envs=1., isolated_axis_fraction=1.)
|
|
term.reset(torch.arange(16))
|
|
term.compute(0.)
|
|
assert term.is_standing_env.all()
|
|
assert torch.count_nonzero(term.command) == 0
|
|
assert torch.count_nonzero(term.vel_command_w) == 0
|