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.
162 lines
8.0 KiB
Python
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
|