237 lines
7.9 KiB
Python
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()
|