302 lines
14 KiB
Python
302 lines
14 KiB
Python
"""Omit learner-only work during measured policy replay.
|
|
|
|
A task's play configuration disables noise and training disturbances. This module
|
|
also avoids reward evaluation when replay only consumes observations. A historical
|
|
single-world Warp benchmark spent 4.7 of 11.3 ms per step computing unused rewards.
|
|
|
|
Skipping is safe only if observations do not depend on side effects in reward
|
|
terms. Stateful caches and clocks must produce the same observation whether the
|
|
reward manager ran or not. tests/test_replay.py checks every locked family with
|
|
identical inputs and reset sequences; it compares observations, not just speed.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import copy
|
|
import importlib.util
|
|
import json
|
|
import math
|
|
from functools import partial
|
|
from pathlib import Path
|
|
|
|
__all__ = [
|
|
"FixedCommandEvaluation",
|
|
"configure_velocity_replay",
|
|
"load_command_script",
|
|
"script_commands",
|
|
"skip_rewards",
|
|
]
|
|
|
|
|
|
def load_command_script(spec: str):
|
|
"""`FILE[:NAME]` -> the callable `NAME` (default `script`) defined in `FILE`.
|
|
|
|
The file is loaded by path, not imported by name, so standalone evaluation
|
|
or video-capture scripts outside the package tree can be passed directly.
|
|
"""
|
|
path, sep, name = spec.rpartition(":")
|
|
if not sep or not name.isidentifier():
|
|
path, name = spec, "script"
|
|
file = Path(path)
|
|
if not file.is_file():
|
|
raise ValueError(f"no command script at {file}")
|
|
loader = importlib.util.spec_from_file_location(f"_mjrl_command_script_{file.stem}", file)
|
|
if loader is None or loader.loader is None:
|
|
raise ValueError(f"{file} is not a Python file")
|
|
module = importlib.util.module_from_spec(loader)
|
|
loader.loader.exec_module(module)
|
|
script = getattr(module, name, None)
|
|
if not callable(script):
|
|
raise TypeError(f"{file} defines no callable {name!r}")
|
|
return script
|
|
|
|
|
|
def script_commands(env, script) -> list[str]:
|
|
"""Have command terms hold what `script` says, from each term's own `compute` on.
|
|
|
|
`script(t)` returns `{term name: values}` for `t` seconds since this was
|
|
installed; it is written into each named velocity command term (`vel_command_b`,
|
|
three values) **after** the term's own `compute`, which is where the operator
|
|
writes -- so a pad that is plugged in and nudged does not drive a scripted
|
|
term, and the term's own bookkeeping (metrics, resample timers) still runs.
|
|
"""
|
|
import torch
|
|
|
|
step_dt = env.step_dt
|
|
origin = env.common_step_counter
|
|
first = script(0.0)
|
|
if not isinstance(first, dict) or not first:
|
|
raise TypeError("a command script returns {term name: values}, and this one returned "
|
|
f"{type(first).__name__}")
|
|
names = list(first)
|
|
for name in names:
|
|
try:
|
|
term = env.command_manager.get_term(name)
|
|
except KeyError:
|
|
raise ValueError(f"the task has no command term {name!r} to script") from None
|
|
if not hasattr(term, "vel_command_b"):
|
|
raise ValueError(f"command term {name!r} is not a velocity "
|
|
"command; a script cannot drive it")
|
|
_install(term, name, script, env, origin, step_dt, torch)
|
|
return names
|
|
|
|
|
|
def _install(term, name, script, env, origin, step_dt, torch) -> None:
|
|
compute = term.compute
|
|
|
|
def scripted(dt, env_ids=None):
|
|
compute(dt, env_ids)
|
|
t = (env.common_step_counter - origin) * step_dt
|
|
values = list(script(t)[name])
|
|
columns = term.command.shape[1]
|
|
if len(values) != columns:
|
|
raise ValueError(f"a script of {len(values)} values for {name!r}, a command of "
|
|
f"{columns}")
|
|
term.vel_command_b[:] = torch.tensor(values, device=term.device, dtype=torch.float32)
|
|
if hasattr(term, "vel_command_w"):
|
|
term.vel_command_w[:] = term.vel_command_b
|
|
|
|
term.compute = scripted
|
|
|
|
|
|
def configure_velocity_replay(env_cfg, training_cfg, checkpoint: dict,
|
|
command=None) -> dict:
|
|
"""Use the saved command rung, without running or restoring a curriculum.
|
|
|
|
This is opt-in for ordinary replay. Fixed-command evaluation always uses it:
|
|
the task's final command ceiling is not evidence of a checkpoint's training
|
|
range. Old checkpoints missing a required rung are deliberately refused.
|
|
"""
|
|
from mjlab.tasks.velocity.mdp.velocity_command import UniformVelocityCommandCfg
|
|
|
|
trained = training_cfg.commands.get("twist")
|
|
if not isinstance(trained, UniformVelocityCommandCfg) or "twist" not in env_cfg.commands:
|
|
reason = "the task has no supported uniform 'twist' velocity command"
|
|
if command is not None:
|
|
raise ValueError(reason + "; fixed-command evaluation is unsupported")
|
|
return {"status": "not_applicable", "source": None, "curriculum_level": None,
|
|
"ranges": None, "resolved_ranges": None, "reason": reason}
|
|
ranges = copy.deepcopy(trained.ranges)
|
|
ladder = training_cfg.curriculum.get("command")
|
|
level = None
|
|
source = "training_task_config"
|
|
if ladder is not None:
|
|
params = ladder.params or {}
|
|
fractions, ceilings = params.get("fractions"), params.get("ceilings")
|
|
level_count = len(fractions) if fractions else 0
|
|
infos = checkpoint.get("infos") or {}
|
|
curriculum = infos.get("curriculum") or {}
|
|
saved = curriculum.get("command") or {}
|
|
level = saved.get("level")
|
|
if type(level) is not int or not 0 <= level < level_count:
|
|
raise ValueError("checkpoint has no valid saved command curriculum level")
|
|
if fractions and ceilings and len(ceilings) == 3:
|
|
x, y, yaw = (float(value) * float(fractions[level]) for value in ceilings)
|
|
else:
|
|
raise ValueError("unsupported command curriculum: no valid command range ladder")
|
|
if not all(math.isfinite(value) for value in (x, y, yaw)) or x <= 0 or y < 0 or yaw <= 0:
|
|
raise ValueError("saved command curriculum has invalid ranges")
|
|
ranges.lin_vel_x = (-x, x)
|
|
ranges.lin_vel_y = (-y, y)
|
|
ranges.ang_vel_z = (-yaw, yaw)
|
|
source = "checkpoint_curriculum"
|
|
axes = ("lin_vel_x", "lin_vel_y", "ang_vel_z")
|
|
bounds = {axis: list(getattr(ranges, axis)) for axis in axes}
|
|
if any(len(pair) != 2 or not all(math.isfinite(float(x)) for x in pair)
|
|
or pair[0] > pair[1] for pair in bounds.values()):
|
|
raise ValueError("training velocity ranges must be finite ordered pairs")
|
|
report = {"status": "matched", "source": source, "curriculum_level": level,
|
|
"ranges": bounds, "resolved_ranges": copy.deepcopy(bounds)}
|
|
if command is None:
|
|
env_cfg.commands["twist"].ranges = ranges
|
|
# A hard-coded forward-only floor can exceed an early curriculum rung.
|
|
env_cfg.commands["twist"].rel_forward_envs = 0.0
|
|
return report
|
|
values = tuple(float(x) for x in command)
|
|
if len(values) != 3 or not all(math.isfinite(x) for x in values):
|
|
raise ValueError("fixed body command must contain three finite values")
|
|
clamped = []
|
|
for axis, value in zip(axes, values, strict=True):
|
|
low, high = bounds[axis]
|
|
if value < low or value > high:
|
|
# A decimal command can be one representable float past an edge
|
|
# computed by multiplying a curriculum fraction and its ceiling.
|
|
within_roundoff = math.nextafter(low, -math.inf) <= value <= math.nextafter(high, math.inf)
|
|
if low == high or not within_roundoff:
|
|
raise ValueError(f"fixed {axis}={value:g} is outside trained range [{low:g}, {high:g}]")
|
|
value = min(high, max(low, value))
|
|
clamped.append(value)
|
|
values = tuple(clamped)
|
|
# Use a replay-only sampler, rather than an operator subclass that a pad or
|
|
# viewer could override. Zero random fractions alone are insufficient: the
|
|
# standard sampler tests random <= fraction, so an exact zero can trigger.
|
|
fixed = copy.deepcopy(trained)
|
|
fixed.ranges = ranges
|
|
for axis, value in zip(axes, values, strict=True):
|
|
setattr(fixed.ranges, axis, (value, value))
|
|
fixed.heading_command = False
|
|
fixed.ranges.heading = None
|
|
fixed.rel_heading_envs = fixed.rel_world_envs = fixed.rel_forward_envs = 0.0
|
|
fixed.rel_standing_envs = 0.0 # (0, 0, 0) itself still requests standing.
|
|
fixed.init_velocity_prob = 0.0
|
|
fixed.build = partial(_build_fixed_velocity_command, fixed, values)
|
|
env_cfg.commands["twist"] = fixed
|
|
report["fixed_body_command"] = list(values)
|
|
report["resolved_ranges"] = {axis: list(getattr(fixed.ranges, axis)) for axis in axes}
|
|
return report
|
|
|
|
|
|
def _build_fixed_velocity_command(cfg, values, env):
|
|
"""Keep standard command metrics/timers/reset, replacing only its producer."""
|
|
from mjlab.managers.command_manager import CommandTerm
|
|
from mjlab.tasks.velocity.mdp.velocity_command import UniformVelocityCommand
|
|
|
|
class FixedBodyVelocityCommand(UniformVelocityCommand):
|
|
def _resample_command(self, env_ids):
|
|
self._update_command(env_ids)
|
|
|
|
def _update_command(self, env_ids=None):
|
|
ids = slice(None) if env_ids is None else env_ids
|
|
self.vel_command_b[ids] = self.vel_command_b.new_tensor(values)
|
|
self.vel_command_w[ids] = self.vel_command_b[ids]
|
|
for flag in (self.is_heading_env, self.is_standing_env,
|
|
self.is_world_env, self.is_forward_env):
|
|
flag[ids] = False
|
|
|
|
def compute(self, dt, env_ids=None):
|
|
# UniformVelocityCommand.compute applies GUI sliders afterward.
|
|
# Fixed evaluation uses the base lifecycle without that override.
|
|
CommandTerm.compute(self, dt, env_ids)
|
|
|
|
return FixedBodyVelocityCommand(cfg, env)
|
|
|
|
|
|
class FixedCommandEvaluation:
|
|
"""Streaming body-frame tracking measurements, excluding auto-reset states.
|
|
|
|
The wrapper returns a post-reset pose on done steps. Counting that velocity
|
|
as the terminal gait would bias the result, so those samples are excluded and
|
|
counted explicitly. This reports tracking, not a policy acceptance score.
|
|
"""
|
|
|
|
def __init__(self, env, command, command_ranges: dict):
|
|
self.env = getattr(env, "unwrapped", env)
|
|
self.command = tuple(float(x) for x in command)
|
|
term = self.env.command_manager.get_term("twist")
|
|
self.robot = term.robot
|
|
self.ranges = command_ranges
|
|
self.steps = self.samples = self.excluded = 0
|
|
self.resets = [0] * self.env.num_envs
|
|
self.actual_sum = [0.0] * 3
|
|
self.absolute_sum = [0.0] * 3
|
|
self.square_sum = [0.0] * 3
|
|
self.xy_error_sum = 0.0
|
|
|
|
def update(self, dones) -> None:
|
|
lin = self.robot.data.root_link_lin_vel_b.detach().cpu().tolist()
|
|
ang = self.robot.data.root_link_ang_vel_b.detach().cpu().tolist()
|
|
reset = dones.detach().cpu().tolist()
|
|
self.steps += 1
|
|
for index, (linear, angular, done) in enumerate(zip(lin, ang, reset, strict=True)):
|
|
if done:
|
|
self.resets[index] += 1
|
|
self.excluded += 1
|
|
continue
|
|
actual = (linear[0], linear[1], angular[2])
|
|
if not all(math.isfinite(value) for value in actual):
|
|
raise ValueError("non-finite body velocity during fixed-command evaluation")
|
|
self.samples += 1
|
|
errors = [value - target for value, target in zip(actual, self.command, strict=True)]
|
|
for axis in range(3):
|
|
self.actual_sum[axis] += actual[axis]
|
|
self.absolute_sum[axis] += abs(errors[axis])
|
|
self.square_sum[axis] += errors[axis] ** 2
|
|
self.xy_error_sum += math.hypot(errors[0], errors[1])
|
|
|
|
def report(self) -> dict:
|
|
count = self.samples
|
|
return {
|
|
"schema": "fixed_body_command_evaluation/1", "frame": "body",
|
|
"axes": ["vx", "vy", "yaw_rate"], "units": ["m/s", "m/s", "rad/s"],
|
|
"command": list(self.command), "command_ranges": self.ranges,
|
|
"control_steps": self.steps, "num_envs": self.env.num_envs,
|
|
"simulated_seconds": self.steps * self.env.step_dt,
|
|
"valid_samples": count, "excluded_reset_samples": self.excluded,
|
|
"episode_resets": sum(self.resets), "episode_resets_per_env": self.resets,
|
|
"sampling": "post-step body velocities; auto-reset states excluded; no warmup exclusion",
|
|
"actual_velocity_mean": [x / count for x in self.actual_sum] if count else None,
|
|
"mean_absolute_error": [x / count for x in self.absolute_sum] if count else None,
|
|
"root_mean_square_error": [math.sqrt(x / count) for x in self.square_sum]
|
|
if count else None,
|
|
"mean_xy_tracking_error": self.xy_error_sum / count if count else None,
|
|
}
|
|
|
|
def write(self, path: Path, **metadata) -> dict:
|
|
result = {**self.report(), **metadata}
|
|
path = Path(path)
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
path.write_text(json.dumps(result, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
|
return result
|
|
|
|
|
|
def skip_rewards(env) -> int:
|
|
"""Stop `env` evaluating its reward terms. Returns how many it had.
|
|
|
|
`compute` is replaced on this **instance** only -- the same way the live viewer
|
|
wraps `sim.step` -- and hands back a buffer of zeros, so `env.step` still
|
|
returns a reward of the right shape. The rest of the manager is untouched:
|
|
`reset` still runs, and still resets the terms that carry state, which is what
|
|
keeps them coherent if anything later evaluates them again.
|
|
"""
|
|
import torch
|
|
|
|
manager = env.reward_manager
|
|
zeros = torch.zeros(env.num_envs, device=env.device)
|
|
|
|
def compute(dt: float) -> torch.Tensor:
|
|
del dt # nothing to scale
|
|
return zeros
|
|
|
|
manager.compute = compute
|
|
return len(manager.active_terms)
|