Files
2026-11-03 19:46:08 +08:00

237 lines
7.9 KiB
Python

"""Evaluate a trained tracking policy and compute metrics."""
from __future__ import annotations
import json
import sys
from dataclasses import asdict, dataclass
from pathlib import Path
from types import SimpleNamespace
from typing import cast
import torch
import tyro
import wandb
from mjlab.envs import ManagerBasedRlEnv
from mjlab.rl import MjlabOnPolicyRunner, RslRlVecEnvWrapper
from mjlab.tasks.registry import list_tasks, load_env_cfg, load_rl_cfg, load_runner_cls
from mjlab.tasks.tracking.mdp import MotionCommandCfg
from mjlab.tasks.tracking.mdp.commands import MotionCommand
from mjlab.tasks.tracking.mdp.metrics import (
compute_ee_orientation_error,
compute_ee_position_error,
compute_joint_velocity_error,
compute_mpkpe,
compute_root_relative_mpkpe,
)
from mjlab.utils.os import get_wandb_checkpoint_path
from mjlab.utils.torch import configure_torch_backends
@dataclass(frozen=True)
class EvaluateConfig:
"""Configuration for policy evaluation."""
wandb_run_path: str
"""W&B run path in format 'entity/project/run_id'."""
wandb_checkpoint_name: str | None = None
"""Optional checkpoint name within the W&B run to load (e.g. 'model_4000.pt')."""
num_envs: int = 1024
"""Number of parallel environments (= number of episodes to evaluate)."""
device: str | None = None
"""Device to run on. Defaults to CUDA if available."""
output_file: str | None = None
"""Optional path to save metrics as JSON."""
log_root: str = "logs/rsl_rl"
"""Root directory under which experiment logs are written."""
def run_evaluate(task_id: str, cfg: EvaluateConfig) -> dict[str, float]:
"""Run policy evaluation and compute metrics."""
configure_torch_backends()
device = cfg.device or ("cuda:0" if torch.cuda.is_available() else "cpu")
# Load configs.
env_cfg = load_env_cfg(task_id, play=False)
agent_cfg = load_rl_cfg(task_id)
motion_cmd = env_cfg.commands.get("motion")
if not isinstance(motion_cmd, MotionCommandCfg):
raise ValueError(f"Task {task_id} is not a tracking task.")
# Load motion file from W&B run.
api = wandb.Api()
run = api.run(cfg.wandb_run_path)
art = next((a for a in run.used_artifacts() if a.type == "motions"), None)
if art is None:
raise RuntimeError("No motion artifact found in the run.")
motion_cmd.motion_file = str(Path(art.download()) / "motion.npz")
# Evaluation config.
motion_cmd.sampling_mode = "start"
env_cfg.observations["actor"].enable_corruption = True
env_cfg.events.pop("push_robot", None)
env_cfg.scene.num_envs = cfg.num_envs
env = ManagerBasedRlEnv(cfg=env_cfg, device=device)
env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions)
log_root_path = (Path(cfg.log_root) / agent_cfg.experiment_name).resolve()
resume_path, _ = get_wandb_checkpoint_path(
log_root_path, Path(cfg.wandb_run_path), cfg.wandb_checkpoint_name
)
print(f"[INFO] Loading checkpoint: {resume_path}")
runner_cls = load_runner_cls(task_id) or MjlabOnPolicyRunner
runner = runner_cls(env, asdict(agent_cfg), device=device)
runner.load(str(resume_path), map_location=device)
policy = runner.get_inference_policy(device=device)
command = cast(MotionCommand, env.unwrapped.command_manager.get_term("motion"))
ee_body_names = env_cfg.terminations["ee_body_pos"].params["body_names"]
print(f"[INFO] End effector bodies: {ee_body_names}")
# Metric accumulators.
all_mpkpe: list[torch.Tensor] = []
all_r_mpkpe: list[torch.Tensor] = []
all_joint_vel_error: list[torch.Tensor] = []
all_ee_pos_error: list[torch.Tensor] = []
all_ee_ori_error: list[torch.Tensor] = []
all_active: list[torch.Tensor] = []
done_envs = torch.zeros(cfg.num_envs, dtype=torch.bool, device=device)
success = torch.zeros(cfg.num_envs, dtype=torch.bool, device=device)
obs = env.get_observations()
print(f"[INFO] Running {cfg.num_envs} evaluation episodes...")
step = 0
while not done_envs.all():
# Snapshot the reference frame the upcoming step will be scored against.
# env.step computes the reward (against the current reference) and only
# afterwards advances the command's motion frame, so reading the
# reference after stepping would pair the robot with the *next* frame.
# We snapshot here and pair it with the post-step robot state below,
# matching how the reward is computed.
ref = SimpleNamespace(
num_envs=command.num_envs,
device=command.device,
cfg=command.cfg,
body_pos_w=command.body_pos_w.clone(),
body_pos_relative_w=command.body_pos_relative_w.clone(),
body_quat_relative_w=command.body_quat_relative_w.clone(),
joint_vel=command.joint_vel.clone(),
)
with torch.no_grad():
actions = policy(obs)
obs, _, dones, _ = env.step(actions)
# Pair the snapshotted reference with the post-step robot state.
ref.robot_body_pos_w = command.robot_body_pos_w
ref.robot_body_quat_w = command.robot_body_quat_w
ref.robot_joint_vel = command.robot_joint_vel
ref_command = cast(MotionCommand, ref)
# Envs that finished this step were auto-reset inside env.step, so their
# robot state is the new episode's; don't score it.
active = ~done_envs & ~dones.bool()
all_active.append(active.float())
all_mpkpe.append(torch.where(active, compute_mpkpe(ref_command), 0.0))
all_r_mpkpe.append(
torch.where(active, compute_root_relative_mpkpe(ref_command), 0.0)
)
all_joint_vel_error.append(
torch.where(active, compute_joint_velocity_error(ref_command), 0.0)
)
all_ee_pos_error.append(
torch.where(active, compute_ee_position_error(ref_command, ee_body_names), 0.0)
)
all_ee_ori_error.append(
torch.where(active, compute_ee_orientation_error(ref_command, ee_body_names), 0.0)
)
# Track completions.
terminated = env.unwrapped.termination_manager.terminated
truncated = env.unwrapped.termination_manager.time_outs
newly_done = dones.bool() & ~done_envs
if newly_done.any():
success = success | (newly_done & truncated & ~terminated)
done_envs = done_envs | newly_done
print(
f"[INFO] {done_envs.sum().item()}/{cfg.num_envs} episodes completed "
f"(step {step}, truncated={(newly_done & truncated).sum().item()}, "
f"terminated={(newly_done & terminated).sum().item()})"
)
step += 1
# Compute mean metrics over the steps each env was active.
stacks = [
all_mpkpe,
all_r_mpkpe,
all_joint_vel_error,
all_ee_pos_error,
all_ee_ori_error,
]
stacks = [torch.stack(s, dim=0) for s in stacks]
active_steps = torch.stack(all_active, dim=0).sum(dim=0).clamp(min=1)
means = [s.sum(dim=0) / active_steps for s in stacks]
metrics = {
"success_rate": success.float().mean().item(),
"mpkpe": means[0].mean().item(),
"r_mpkpe": means[1].mean().item(),
"joint_vel_error": means[2].mean().item(),
"ee_pos_error": means[3].mean().item(),
"ee_ori_error": means[4].mean().item(),
}
print("\n" + "=" * 50)
print("Evaluation Results")
print("=" * 50)
for name, value in metrics.items():
print(f" {name}: {value:.4f}")
print("=" * 50)
if cfg.output_file:
output_path = Path(cfg.output_file)
output_path.parent.mkdir(parents=True, exist_ok=True)
with open(output_path, "w") as f:
json.dump(metrics, f, indent=2)
print(f"[INFO] Metrics saved to {output_path}")
env.close()
return metrics
def main():
import mjlab.tasks # noqa: F401
tracking_tasks = [t for t in list_tasks() if "Tracking" in t]
if not tracking_tasks:
print("No tracking tasks found.")
sys.exit(1)
chosen_task, remaining_args = tyro.cli(
tyro.extras.literal_type_from_choices(tracking_tasks),
add_help=False,
return_unknown_args=True,
config=mjlab.TYRO_FLAGS,
)
args = tyro.cli(
EvaluateConfig,
args=remaining_args,
prog=sys.argv[0] + f" {chosen_task}",
config=mjlab.TYRO_FLAGS,
)
run_evaluate(chosen_task, args)
if __name__ == "__main__":
main()