290 lines
8.8 KiB
Python
290 lines
8.8 KiB
Python
"""Tests for the velocity command's initial-velocity injection.
|
|
|
|
The init_velocity_prob path runs inside the reset pipeline, after reset
|
|
events wrote the new pose to qpos but before sim.forward(), so it must not
|
|
read (or write back) derived kinematics.
|
|
"""
|
|
|
|
from types import SimpleNamespace
|
|
from typing import TYPE_CHECKING, cast
|
|
|
|
import pytest
|
|
import torch
|
|
from conftest import get_test_device, load_fixture_xml, make_scene_and_sim
|
|
|
|
from mjlab.tasks.velocity.mdp.velocity_command import UniformVelocityCommandCfg
|
|
|
|
if TYPE_CHECKING:
|
|
from mjlab.envs import ManagerBasedRlEnv
|
|
|
|
|
|
@pytest.fixture(scope="module")
|
|
def device():
|
|
return get_test_device()
|
|
|
|
|
|
def test_init_velocity_preserves_fresh_reset_pose(device):
|
|
scene, sim = make_scene_and_sim(
|
|
device, load_fixture_xml("floating_base_articulated"), sensors=(), num_envs=2
|
|
)
|
|
env = cast(
|
|
"ManagerBasedRlEnv",
|
|
SimpleNamespace(scene=scene, sim=sim, num_envs=2, device=device),
|
|
)
|
|
cfg = UniformVelocityCommandCfg(
|
|
entity_name="robot",
|
|
resampling_time_range=(1e9, 1e9),
|
|
init_velocity_prob=1.0,
|
|
rel_heading_envs=0.0,
|
|
ranges=UniformVelocityCommandCfg.Ranges(
|
|
lin_vel_x=(0.5, 0.5), lin_vel_y=(0.2, 0.2), ang_vel_z=(0.0, 0.0)
|
|
),
|
|
)
|
|
term = cfg.build(env)
|
|
robot = scene["robot"]
|
|
env_ids = torch.arange(2, device=device)
|
|
|
|
# Derived kinematics now hold the spawn pose (the "previous episode" state).
|
|
sim.forward()
|
|
|
|
# Emulate a reset event: write a fresh pose to qpos, no forward yet.
|
|
pose = torch.tensor(
|
|
[
|
|
[1.0, 2.0, 1.5, 1.0, 0.0, 0.0, 0.0],
|
|
[3.0, -1.0, 1.5, 1.0, 0.0, 0.0, 0.0],
|
|
],
|
|
device=device,
|
|
)
|
|
robot.write_root_link_pose_to_sim(pose, env_ids=env_ids)
|
|
|
|
term.reset(env_ids=env_ids)
|
|
sim.forward()
|
|
|
|
# The fresh reset pose survives. Before the fix, the init-velocity path
|
|
# wrote the stale pre-reset pose back into the sim.
|
|
assert torch.allclose(robot.data.root_link_pos_w, pose[:, :3], atol=1e-6)
|
|
|
|
# Planar velocity matches the sampled command (identity orientation, so
|
|
# body frame equals world frame).
|
|
assert torch.allclose(
|
|
robot.data.root_link_lin_vel_b[:, :2],
|
|
term.vel_command_b[:, :2],
|
|
atol=1e-5,
|
|
)
|
|
|
|
|
|
def test_mid_episode_resample_does_not_write_velocity(device):
|
|
"""Init velocity applies on reset only; a timer-expiry resample runs after
|
|
step()'s forward and must not write sim state."""
|
|
scene, sim = make_scene_and_sim(
|
|
device, load_fixture_xml("floating_base_articulated"), sensors=(), num_envs=2
|
|
)
|
|
env = cast(
|
|
"ManagerBasedRlEnv",
|
|
SimpleNamespace(
|
|
scene=scene,
|
|
sim=sim,
|
|
num_envs=2,
|
|
device=device,
|
|
step_dt=0.02,
|
|
episode_length_buf=torch.ones(2, dtype=torch.long, device=device),
|
|
),
|
|
)
|
|
cfg = UniformVelocityCommandCfg(
|
|
entity_name="robot",
|
|
resampling_time_range=(0.001, 0.001), # Expires on the first compute.
|
|
init_velocity_prob=1.0,
|
|
rel_heading_envs=0.0,
|
|
ranges=UniformVelocityCommandCfg.Ranges(
|
|
lin_vel_x=(0.7, 0.7), lin_vel_y=(0.3, 0.3), ang_vel_z=(0.0, 0.0)
|
|
),
|
|
)
|
|
term = cfg.build(env)
|
|
robot = scene["robot"]
|
|
env_ids = torch.arange(2, device=device)
|
|
|
|
term.reset(env_ids=env_ids)
|
|
sim.forward()
|
|
|
|
# Mid-episode the robot has decelerated to rest.
|
|
robot.write_root_link_velocity_to_sim(
|
|
torch.zeros(2, 6, device=device), env_ids=env_ids
|
|
)
|
|
sim.forward()
|
|
|
|
# Step's command compute: the 1 ms timer expires and resamples.
|
|
counter = term.command_counter.clone()
|
|
term.compute(dt=1.0)
|
|
assert (term.command_counter > counter).all()
|
|
|
|
# Only the command changed; sim velocity is untouched.
|
|
qvel_lin = sim.data.qvel[:, robot.indexing.free_joint_v_adr[:3]]
|
|
assert torch.allclose(qvel_lin, torch.zeros_like(qvel_lin), atol=1e-6)
|
|
assert torch.allclose(
|
|
robot.data.root_link_lin_vel_b,
|
|
torch.zeros_like(robot.data.root_link_lin_vel_b),
|
|
atol=1e-6,
|
|
)
|
|
|
|
|
|
def test_commanded_displacement_and_episode_start(device):
|
|
scene, sim = make_scene_and_sim(
|
|
device, load_fixture_xml("floating_base_articulated"), sensors=(), num_envs=2
|
|
)
|
|
episode_length_buf = torch.zeros(2, dtype=torch.long, device=device)
|
|
env = cast(
|
|
"ManagerBasedRlEnv",
|
|
SimpleNamespace(
|
|
scene=scene,
|
|
sim=sim,
|
|
num_envs=2,
|
|
device=device,
|
|
step_dt=0.02,
|
|
episode_length_buf=episode_length_buf,
|
|
),
|
|
)
|
|
cfg = UniformVelocityCommandCfg(
|
|
entity_name="robot",
|
|
resampling_time_range=(1e9, 1e9),
|
|
rel_heading_envs=0.0,
|
|
ranges=UniformVelocityCommandCfg.Ranges(
|
|
lin_vel_x=(1.0, 1.0), lin_vel_y=(0.0, 0.0), ang_vel_z=(0.0, 0.0)
|
|
),
|
|
)
|
|
term = cfg.build(env)
|
|
env_ids = torch.arange(2, device=device)
|
|
|
|
# Env 0 faces +x, env 1 is yawed 90 degrees and faces +y.
|
|
half = 0.5**0.5
|
|
pose = torch.tensor(
|
|
[[1.0, 2.0, 1.5, 1.0, 0.0, 0.0, 0.0], [3.0, -1.0, 1.5, half, 0.0, 0.0, half]],
|
|
device=device,
|
|
)
|
|
scene["robot"].write_root_link_pose_to_sim(pose, env_ids=env_ids)
|
|
term.reset(env_ids=env_ids)
|
|
sim.forward()
|
|
term.compute(dt=0.0, env_ids=env_ids)
|
|
assert torch.allclose(term.episode_start_pos_w, pose[:, :2], atol=1e-6)
|
|
|
|
# 1 m/s forward for 0.5 s; env 0 is then auto-reset (zero dt) at a new pose.
|
|
episode_length_buf[:] = 1
|
|
for _ in range(25):
|
|
term.compute(dt=0.02)
|
|
scene["robot"].write_root_link_pose_to_sim(pose[1:], env_ids=env_ids[:1])
|
|
term.reset(env_ids=env_ids[:1])
|
|
episode_length_buf[0] = 0
|
|
sim.forward()
|
|
term.compute(dt=torch.tensor([0.0, 0.02], device=device))
|
|
|
|
expected = torch.tensor([[0.0, 0.0], [0.0, 0.52]], device=device)
|
|
assert torch.allclose(term.commanded_displacement_w, expected, atol=1e-5)
|
|
assert torch.allclose(term.episode_start_pos_w, pose[[1, 1], :2], atol=1e-6)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("sampled_x", "min_forward_velocity", "expected_x"),
|
|
[(-1.0, 0.3, 1.0), (-0.02, 0.3, 0.3), (-0.02, 0.075, 0.075)],
|
|
)
|
|
def test_forward_envs_ignore_heading_and_world_frame(
|
|
device, sampled_x, min_forward_velocity, expected_x
|
|
):
|
|
scene, sim = make_scene_and_sim(
|
|
device, load_fixture_xml("floating_base_articulated"), sensors=(), num_envs=2
|
|
)
|
|
env = cast(
|
|
"ManagerBasedRlEnv",
|
|
SimpleNamespace(
|
|
scene=scene,
|
|
sim=sim,
|
|
num_envs=2,
|
|
device=device,
|
|
step_dt=0.02,
|
|
episode_length_buf=torch.zeros(2, dtype=torch.long, device=device),
|
|
),
|
|
)
|
|
cfg = UniformVelocityCommandCfg(
|
|
entity_name="robot",
|
|
resampling_time_range=(1e9, 1e9),
|
|
heading_command=True,
|
|
rel_heading_envs=1.0,
|
|
rel_world_envs=1.0,
|
|
rel_forward_envs=1.0,
|
|
min_forward_velocity=min_forward_velocity,
|
|
ranges=UniformVelocityCommandCfg.Ranges(
|
|
lin_vel_x=(sampled_x, sampled_x),
|
|
lin_vel_y=(0.8, 0.8),
|
|
ang_vel_z=(-0.5, 0.5),
|
|
heading=(1.0, 1.0),
|
|
),
|
|
)
|
|
term = cfg.build(env)
|
|
env_ids = torch.arange(2, device=device)
|
|
|
|
sim.forward()
|
|
term.reset(env_ids=env_ids)
|
|
term.compute(dt=0.0, env_ids=env_ids)
|
|
term.compute(dt=0.02)
|
|
|
|
# Straight ahead at max(|lin_vel_x|, floor), no lateral or yaw command.
|
|
expected = torch.tensor([[expected_x, 0.0, 0.0]] * 2, device=device)
|
|
torch.testing.assert_close(term.vel_command_b, expected)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("rel_standing_envs", "expected"),
|
|
[(0.0, [0.2, -0.5, -1.0]), (1.0, [0.0, 0.0, 0.0])],
|
|
)
|
|
def test_init_velocity_matches_resolved_command(device, rel_standing_envs, expected):
|
|
"""Standing, heading and world-frame envs start at the command they will
|
|
actually receive, not at the raw sample."""
|
|
scene, sim = make_scene_and_sim(
|
|
device, load_fixture_xml("floating_base_articulated"), sensors=(), num_envs=2
|
|
)
|
|
env = cast(
|
|
"ManagerBasedRlEnv",
|
|
SimpleNamespace(
|
|
scene=scene,
|
|
sim=sim,
|
|
num_envs=2,
|
|
device=device,
|
|
step_dt=0.02,
|
|
episode_length_buf=torch.zeros(2, dtype=torch.long, device=device),
|
|
),
|
|
)
|
|
cfg = UniformVelocityCommandCfg(
|
|
entity_name="robot",
|
|
resampling_time_range=(1e9, 1e9),
|
|
init_velocity_prob=1.0,
|
|
heading_command=True,
|
|
rel_heading_envs=1.0,
|
|
rel_world_envs=1.0,
|
|
rel_standing_envs=rel_standing_envs,
|
|
ranges=UniformVelocityCommandCfg.Ranges(
|
|
lin_vel_x=(0.5, 0.5),
|
|
lin_vel_y=(0.2, 0.2),
|
|
ang_vel_z=(-1.0, 1.0),
|
|
heading=(0.0, 0.0),
|
|
),
|
|
)
|
|
term = cfg.build(env)
|
|
robot = scene["robot"]
|
|
env_ids = torch.arange(2, device=device)
|
|
|
|
# Derived kinematics hold the unrotated spawn pose; the reset yaws 90 degrees.
|
|
sim.forward()
|
|
half = 0.5**0.5
|
|
pose = torch.tensor([[0.0, 0.0, 1.5, half, 0.0, 0.0, half]], device=device)
|
|
robot.write_root_link_pose_to_sim(pose.repeat(2, 1), env_ids=env_ids)
|
|
|
|
term.reset(env_ids=env_ids)
|
|
sim.forward()
|
|
term.compute(dt=0.0, env_ids=env_ids)
|
|
|
|
expected = torch.tensor([expected, expected], device=device)
|
|
torch.testing.assert_close(term.vel_command_b, expected, atol=1e-5, rtol=0)
|
|
vel_b = torch.cat(
|
|
[robot.data.root_link_lin_vel_b[:, :2], robot.data.root_link_ang_vel_b[:, 2:]],
|
|
dim=-1,
|
|
)
|
|
torch.testing.assert_close(vel_b, expected, atol=1e-5, rtol=0)
|