"""Whole-clip tracking evaluation and drift measurements for any WBC algorithm.
Protocol: SuperMimic (Liao et al., 2026), Appendix F,
https://arxiv.org/abs/2610.28480. Queueing and drift decomposition are adapted from
CuboiLeo/SuperMimic-Reproduced (ba61053), src/supermimic/eval/benchmark.py and
scripts/diag_drift.py:
https://github.com/CuboiLeo/SuperMimic-Reproduced/tree/ba6105349069bf040f1fdad60873d13138d505a2
The implementation uses the repo's presets, motion library and policy interface;
normalization bounds are supplied per task, never assumed to be 0.1.
"""
from __future__ import annotations
import math
from dataclasses import asdict, dataclass
from pathlib import Path
import torch
from mjlab.utils.lab_api.math import quat_apply, quat_conjugate, quat_mul, yaw_quat
from tensordict import TensorDict
def clip_finished(env, command_name: str = "motion") -> torch.Tensor:
command = env.command_manager.get_term(command_name)
return (
command.time_steps >= command.motion.segment_end_idx[command.trajectory_ids] - 1
)
[docs]
def deterministic_cfg(cfg):
"""Keep preset terms/weights, disable noise, randomization and external events."""
cfg.rewards = {
name: term
for name, term in cfg.rewards.items()
if not getattr(term.func, "training_only", False)
}
cfg.seed = 0
cfg.episode_length_s = 1e9
cfg.curriculum = {}
cfg.recorders = {}
# Explicit deterministic protocol: startup physical parameters are the nominal model.
from mjlab.envs.mdp import dr
events = {}
for name, event in cfg.events.items():
if event.func is dr.geom_friction:
event.mode = "startup"
event.params["ranges"] = (1.0, 1.0)
event.params["operation"] = "abs"
events[name] = event
elif event.mode == "reset" and not getattr(event.func, "__module__", "").startswith(
dr.__name__
):
events[name] = event
if not any(event.func is dr.geom_friction for event in events.values()):
from mjlab.managers.event_manager import EventTermCfg
from mjlab.managers.scene_entity_config import SceneEntityCfg
events["benchmark_friction"] = EventTermCfg(
func=dr.geom_friction,
mode="startup",
params={
"asset_cfg": SceneEntityCfg(
cfg.commands["motion"].entity_name, geom_names=(".*",)
),
"operation": "abs",
"ranges": (1.0, 1.0),
"shared_random": True,
},
)
cfg.events = events
for group in cfg.observations.values():
group.enable_corruption = False
for term in group.terms.values():
if term is not None:
term.noise = None
term.delay_min_lag = term.delay_max_lag = 0
command = cfg.commands["motion"]
command.pose_range = {}
command.velocity_range = {}
command.joint_position_range = (0.0, 0.0)
command.assistive_wrench_enabled = False
command.rsi.sampling_mode = "start"
from mjlab.managers.termination_manager import TerminationTermCfg
cfg.terminations = {n: t for n, t in cfg.terminations.items() if not t.time_out}
cfg.terminations["benchmark_clip_end"] = TerminationTermCfg(
func=clip_finished, time_out=True
)
return cfg
def reward_upper_bound(cfg, step_dt: float) -> float | None:
"""Infer only known kernel bounds and nonpositive penalties, by function identity."""
from mjlab.envs import mdp
from wbc_mjlab.env.mdp import rewards
kernels = {
func
for name, func in vars(rewards).items()
if name.startswith("motion_") and name.endswith("_exp") and callable(func)
}
# These costs are nonnegative even when their simulator-only inputs require
# learned reward heads in imagination. Negative weights contribute at most zero.
penalties = {
mdp.action_rate_l2,
mdp.joint_pos_limits,
rewards.action_rate_l1,
rewards.joint_acc_l1,
rewards.anti_shake_ang_vel_l2,
rewards.actuator_torque_soft_limit,
rewards.angular_momentum_penalty,
rewards.self_collision_cost,
rewards.mechanical_power_l1,
rewards.negative_mechanical_power_l2,
rewards.torque_envelope_violation_l2,
rewards.feet_slip,
}
bound = 0.0
for term in cfg.rewards.values():
if not term.weight:
continue
if hasattr(term.func, "upper_bound"):
bound += max(term.weight, 0.0) * term.func.upper_bound
elif term.func in kernels and term.params.get("kappa", 1.0) >= 0:
bound += max(term.weight, 0.0)
elif term.func is mdp.is_alive:
bound += max(term.weight, 0.0)
elif term.func not in penalties or term.weight > 0:
return None
bound *= step_dt if cfg.scale_rewards_by_dt else 1.0
return bound if bound > 0 else None
@dataclass
class ClipResult:
trajectory: int
name: str
frames: int
steps: int
return_sum: float
normalized_return: float | None
failed: bool
failure_step: int | None
failure_terms: list[str]
reward_terms: dict[str, float]
drift: dict[str, float | None]
tracking: dict[str, float] | None = None
def summarize(results: list[ClipResult]) -> dict:
n = len(results)
steps = sum(r.steps for r in results)
normalized = [r for r in results if r.normalized_return is not None]
tracked = [r.tracking for r in results if r.tracking]
tracking = {
f"mean_{k}": sum(t[k] for t in tracked) / len(tracked)
for k in (tracked[0] if tracked else ())
}
return {
"clips": n,
"failed": sum(r.failed for r in results),
"failure_rate": sum(r.failed for r in results) / n if n else 0.0,
"completion_rate": 1 - sum(r.failed for r in results) / n if n else 0.0,
**tracking,
"steps": steps,
"mean_normalized_return": sum(r.normalized_return for r in normalized)
/ len(normalized)
if normalized
else None,
"step_weighted_normalized_return": sum(
r.normalized_return * r.steps for r in normalized
)
/ sum(r.steps for r in normalized)
if normalized
else None,
"mean_reward_per_step": sum(r.return_sum for r in results) / steps
if steps
else None,
}
class DriftMeter:
"""Measurements before each action, aligned with that action's reference target.
Reports velocity MSE directly rather than exponentiating a time-averaged MSE
(which would not equal the mean exponential tracking reward).
"""
names = (
"xy_m",
"yaw_deg",
"linear_velocity_world_mse",
"linear_velocity_aligned_mse",
"angular_velocity_world_mse",
"angular_velocity_aligned_mse",
)
def __init__(self, num_envs, device):
self.sums = torch.zeros(num_envs, len(self.names), device=device)
self.last = torch.zeros(num_envs, 2, device=device)
self.crossing = torch.full((num_envs, 2), -1.0, device=device)
def update(self, command, active, steps, dt):
xy = (command.anchor_pos_w[:, :2] - command.robot_anchor_pos_w[:, :2]).norm(dim=-1)
# Difference of headings, not heading of the relative 3D rotation: the latter
# can report spurious yaw error when the two bodies have different roll/tilt.
q = quat_mul(
yaw_quat(command.anchor_quat_w),
quat_conjugate(yaw_quat(command.robot_anchor_quat_w)),
)
yaw = 2 * torch.atan2(q[:, 3], q[:, 0])
yaw = torch.atan2(yaw.sin(), yaw.cos()).abs() * (180 / math.pi)
body_q = q[:, None, :].expand(-1, command.robot_body_lin_vel_w.shape[1], -1)
errors = []
for desired, actual in (
(command.body_lin_vel_w, command.robot_body_lin_vel_w),
(command.body_ang_vel_w, command.robot_body_ang_vel_w),
):
errors.extend(
[
(desired - actual).square().sum(-1).mean(-1),
(desired - quat_apply(body_q, actual)).square().sum(-1).mean(-1),
]
)
values = torch.stack([xy, yaw, *errors], dim=-1)
self.sums += values * active[:, None]
self.last = torch.where(active[:, None], values[:, :2], self.last)
over = torch.stack([xy > 0.3, yaw > 15], dim=-1)
self.crossing = torch.where(
active[:, None] & (self.crossing < 0) & over, (steps * dt)[:, None], self.crossing
)
def result(self, env_id, steps):
values = (self.sums[env_id] / max(steps, 1)).tolist()
result = {f"mean_{n}": v for n, v in zip(self.names, values, strict=True)}
result.update(
final_xy_m=self.last[env_id, 0].item(), final_yaw_deg=self.last[env_id, 1].item()
)
for name, value in zip(
("time_xy_gt_0p3_m_s", "time_yaw_gt_15_deg_s"),
self.crossing[env_id].tolist(),
strict=True,
):
result[name] = value if value >= 0 else None
return result
def reset(self, ids):
self.sums[ids] = 0
self.last[ids] = 0
self.crossing[ids] = -1
def _anchor_local(pos, anchor_pos, anchor_quat):
"""Body positions relative to the anchor's ground point, in its heading frame."""
origin = anchor_pos.clone()
origin[:, 2] = 0.0
inv = quat_conjugate(yaw_quat(anchor_quat))[:, None, :].expand(-1, pos.shape[1], -1)
return quat_apply(inv, pos - origin[:, None, :])
class TrackingErrorMeter:
"""Per-step tracking errors averaged over each clip's alive steps.
``local_body_pos_mm`` is the sim2real leaderboard's MPJPE (robot and reference each
re-anchored to their own anchor's ground point and heading), as used to score
PartyMimic (Roboparty/PartyMimic, tools/sim2real.patch, MIT).
"""
names = ("local_body_pos_mm", "global_body_pos_mm", "joint_pos_deg")
def __init__(self, num_envs, device):
self.sums = torch.zeros(num_envs, len(self.names), device=device)
def update(self, command, active):
ref, robot = command.body_pos_w, command.robot_body_pos_w
local = (
_anchor_local(ref, command.anchor_pos_w, command.anchor_quat_w)
- _anchor_local(robot, command.robot_anchor_pos_w, command.robot_anchor_quat_w)
).norm(dim=-1)
joint = command._tracked_joint_pos_error().abs().mean(-1)
values = torch.stack(
[
1000 * local.mean(-1),
1000 * (ref - robot).norm(dim=-1).mean(-1),
joint * (180 / math.pi),
],
dim=-1,
)
self.sums += values * active[:, None]
def result(self, env_id, steps):
values = (self.sums[env_id] / max(steps, 1)).tolist()
return dict(zip(self.names, values, strict=True))
def reset(self, ids):
self.sums[ids] = 0
[docs]
@torch.no_grad()
def run_benchmark(
env, policy, *, trajectories=None, max_step_reward=None, clip_actions=None
):
"""Play each requested trajectory once. Returns serializable summary and clip rows.
``env`` must use deterministic_cfg; the caller owns and closes it. Reward means
include the terminal step. Drift is measured before stepping to avoid auto-reset
contamination. A failure is any configured non-timeout termination.
"""
if max_step_reward is not None and (
not math.isfinite(max_step_reward) or max_step_reward <= 0
):
raise ValueError("max_step_reward must be finite and positive")
command = env.command_manager.get_term("motion")
motion, device, n = command.motion, env.device, env.num_envs
selected = (
list(range(motion.num_trajectories)) if trajectories is None else list(trajectories)
)
if not selected or len(set(selected)) != len(selected):
raise ValueError("benchmark needs a nonempty list of unique trajectories")
if min(selected) < 0 or max(selected) >= motion.num_trajectories:
raise ValueError("benchmark trajectory is outside the motion library")
queue = torch.tensor(selected, device=device, dtype=torch.long)
slots = torch.arange(n, device=device)
def assignment(indices):
return queue[indices.clamp(max=len(selected) - 1)]
command.set_start_trajectories(assignment(slots))
obs, _ = env.reset()
command.set_start_trajectories(assignment(slots + n))
active = slots < len(selected)
returns = torch.zeros(n, device=device)
steps = torch.zeros(n, dtype=torch.long, device=device)
reward_names = env.reward_manager.active_terms
terms = torch.zeros(n, len(reward_names), device=device)
drift = DriftMeter(n, device)
tracking = TrackingErrorMeter(n, device)
results = []
policy.eval()
while len(results) < len(selected):
drift.update(command, active, steps, env.step_dt)
tracking.update(command, active)
action = policy(TensorDict(obs, batch_size=[n]))
if clip_actions is not None:
action = action.clamp(-clip_actions, clip_actions)
obs, reward, terminated, truncated, _ = env.step(action)
returns += reward * active
terms += env.reward_manager._step_reward * active[:, None]
steps += active.long()
done = terminated | truncated
ids = (done & active).nonzero().flatten()
for e in ids.tolist():
trajectory, count = selected[int(slots[e])], int(steps[e])
failed = bool(terminated[e])
result = ClipResult(
trajectory,
motion.segment_names[trajectory],
int(motion.segment_length[trajectory]),
count,
returns[e].item(),
returns[e].item() / (max_step_reward * count)
if max_step_reward is not None
else None,
failed,
count if failed else None,
[
name
for name in env.termination_manager.active_terms
if not env.termination_manager.get_term_cfg(name).time_out
and env.termination_manager.get_term(name)[e]
],
{name: terms[e, i].item() / count for i, name in enumerate(reward_names)},
drift.result(e, count),
tracking.result(e, count),
)
results.append(result)
# Inactive envs still auto-reset. Advance only active queue slots; their reset
# already used the next assignment installed before the step.
slots[ids] += n
active[ids] = slots[ids] < len(selected)
if ids.numel():
command.set_start_trajectories(assignment(slots[ids] + n), env_ids=ids)
print(f"[EVAL] clips {len(results)}/{len(selected)}", flush=True)
returns[ids], steps[ids], terms[ids] = 0, 0, 0
drift.reset(ids)
tracking.reset(ids)
order = {trajectory: i for i, trajectory in enumerate(selected)}
results.sort(key=lambda r: order[r.trajectory])
return {"summary": summarize(results), "clips": [asdict(r) for r in results]}
[docs]
@dataclass
class EvaluationConfig:
checkpoint_file: Path | str
task: str | None = None
dataset: str | None = None
dataset_path: str | None = None
num_envs: int = 128
device: str = "cuda:0"
clips: list[str] | None = None
max_step_reward: float | None = None
out: Path | str | None = None
[docs]
def evaluate_checkpoint(config: EvaluationConfig):
"""Shared CLI/Studio backend: restore a run, apply the protocol, evaluate, write JSON."""
import json
from pathlib import Path
from mjlab.envs import ManagerBasedRlEnv
from wbc_mjlab.data_paths import resolve_training_motion_file
from wbc_mjlab.rl.policy import load_actor
from wbc_mjlab.run_params import load_run_config
from wbc_mjlab.tasks import get_task_config
if config.num_envs < 1:
raise ValueError("num_envs must be positive")
checkpoint = Path(config.checkpoint_file).resolve()
run = load_run_config(checkpoint.parent, config.task)
cfg, agent = run.env, run.agent
motion = cfg.commands["motion"]
if config.dataset is not None or config.dataset_path is not None:
motion.motion_file = str(
resolve_training_motion_file(
get_task_config(run.task_id).robot_id,
dataset=config.dataset,
dataset_path=config.dataset_path,
)
)
cfg.scene.num_envs = config.num_envs
deterministic_cfg(cfg)
env = ManagerBasedRlEnv(cfg, device=config.device)
try:
env.reset()
actor = load_actor(env, asdict(agent), checkpoint, config.device)
names = env.command_manager.get_term("motion").motion.segment_names
if config.clips is not None and set(config.clips) - set(names):
raise ValueError(f"unknown clips: {set(config.clips) - set(names)}")
trajectories = (
[names.index(name) for name in config.clips] if config.clips else None
)
bound = config.max_step_reward
if bound is None:
bound = reward_upper_bound(cfg, env.step_dt)
result = run_benchmark(
env,
actor,
trajectories=trajectories,
max_step_reward=bound,
clip_actions=agent.clip_actions,
)
result["protocol"] = {
"task": run.task_id,
"checkpoint": str(checkpoint),
"motion_file": str(motion.motion_file),
"step_dt_s": env.step_dt,
"max_step_reward": bound,
"friction": 1.0,
"seed": 0,
"start": "first frame",
"deterministic_policy": True,
"noise": False,
"domain_randomization": False,
"reward_terms": "weighted mean per step, before dt scaling",
"sources": [
"https://arxiv.org/abs/2610.28480",
"https://github.com/CuboiLeo/SuperMimic-Reproduced/tree/ba6105349069bf040f1fdad60873d13138d505a2",
],
}
out = (
Path(config.out)
if config.out
else checkpoint.parent / f"benchmark_{checkpoint.stem}.json"
)
out.parent.mkdir(parents=True, exist_ok=True)
out.write_text(json.dumps(result, indent=2, allow_nan=False) + "\n")
return result, out
finally:
env.close()
SPLIT_METRICS = (
"mean_local_body_pos_mm",
"mean_global_body_pos_mm",
"completion_rate",
"mean_normalized_return",
)
def evaluate_splits(config: EvaluationConfig, splits: dict[str, str]):
"""Evaluate one checkpoint on named held-out sets (``name -> dataset path``).
Each split runs in its own environment; the combined summary is the unweighted mean
over splits, as in PartyMimic's five-set mean.
"""
import json
if not splits:
raise ValueError("evaluate_splits needs at least one split")
checkpoint = Path(config.checkpoint_file).resolve()
root = (
Path(config.out) if config.out else checkpoint.parent / f"splits_{checkpoint.stem}"
)
per_split = {}
for name, path in splits.items():
split_cfg = EvaluationConfig(
**{
**asdict(config),
"dataset": None,
"dataset_path": path,
"clips": None,
"out": root / f"{name}.json",
}
)
result, _ = evaluate_checkpoint(split_cfg)
per_split[name] = result["summary"]
mean = {}
for key in SPLIT_METRICS:
values = [s[key] for s in per_split.values() if s.get(key) is not None]
mean[key] = sum(values) / len(values) if len(values) == len(per_split) else None
combined = {"checkpoint": str(checkpoint), "splits": per_split, "mean": mean}
out = root / "summary.json"
out.parent.mkdir(parents=True, exist_ok=True)
out.write_text(json.dumps(combined, indent=2, allow_nan=False) + "\n")
return combined, out