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

489 lines
14 KiB
Python

"""Base class for environment viewers.
The viewer runs a policy against an RL environment and displays the
result in real time.
The Problem
===========
Each call to ``env.step()`` advances the simulation by a fixed amount
of sim time (``step_dt``, set by the env config). The simplest viewer
would call ``env.step()`` once per loop iteration, but iterations take
however long the hardware needs. A fast machine loops 67 times per
second and the simulation runs too fast; a slow machine loops 30 times
and it runs too slow. Playback speed would depend on hardware.
The viewer needs to answer a different question: given how much real
time just elapsed, how many calls to ``env.step()`` will keep the
simulation advancing at the right pace?
Budget Accumulator
==================
A single variable, ``_sim_budget``, tracks how much sim time has
accumulated but not yet been simulated. Each tick of the main loop:
1. Measure real time elapsed since the last tick.
2. Multiply by the speed setting and add to the budget.
3. Call ``env.step()`` in a loop, subtracting ``step_dt`` from the
budget each time, until the budget is less than one step.
4. Carry the leftover to the next tick.
Example at 1x speed, step_dt = 0.02s (50 Hz control), 60 fps::
tick 1: +0.0167s -> budget = 0.0167 -> no step (< 0.02)
tick 2: +0.0167s -> budget = 0.0334 -> 1 step, 0.0134 left
tick 3: +0.0167s -> budget = 0.0301 -> 1 step, 0.0101 left
...
This averages to 50 steps per second on any hardware. At 2x speed the
elapsed time is doubled before adding to the budget, so steps happen
twice as often. At 0.5x, half as often.
Rendering is independent: the display refreshes at ``frame_rate``
(e.g. 60 Hz) whether or not a new step happened. Some frames will
re-display the same state.
If physics is too slow to keep up, the budget grows without bound. A
real time deadline (one frame period) caps each burst so the renderer
always gets a turn. Leftover budget is dropped and ``_was_capped`` is
set.
Main Loop
=========
::
run()
setup()
while running:
tick() -> True if a frame was produced
_process_actions() drain UI action queue
_step_physics(dt) accumulate budget, step until spent
sync_env_to_viewer() push state to display (at frame_rate)
sleep(1ms) yield CPU when no frame is due
Subclasses implement setup(), sync_env_to_viewer(), sync_viewer_to_env(),
close(), and is_running().
"""
from __future__ import annotations
import signal
import time
import traceback
from abc import ABC, abstractmethod
from collections import deque
from dataclasses import dataclass
from enum import Enum, IntEnum
from typing import TYPE_CHECKING, Any, Optional, Protocol
import torch
if TYPE_CHECKING:
from mjlab.envs import ManagerBasedRlEnvCfg
class VerbosityLevel(IntEnum):
SILENT = 0
INFO = 1
DEBUG = 2
class EnvProtocol(Protocol):
"""Interface we expect from RL environments, which can be either vanilla
`ManagerBasedRlEnv` objects or wrapped with `VideoRecorder`,
`RslRlVecEnvWrapper`, etc."""
num_envs: int
@property
def device(self) -> torch.device | str: ...
@property
def cfg(self) -> ManagerBasedRlEnvCfg: ...
@property
def unwrapped(self) -> Any: ...
def get_observations(self) -> Any: ...
def step(self, actions: torch.Tensor) -> tuple[Any, ...]: ...
def reset(self) -> Any: ...
def close(self) -> None: ...
class PolicyProtocol(Protocol):
def __call__(self, obs: torch.Tensor) -> torch.Tensor: ...
@dataclass(frozen=True)
class ViewerStatus:
paused: bool
step_count: int
speed_multiplier: float
speed_label: str
target_realtime: float
actual_realtime: float
smoothed_fps: float
capped: bool
last_error: str | None
class ViewerAction(Enum):
RESET = "reset"
TOGGLE_PAUSE = "toggle_pause"
SINGLE_STEP = "single_step"
RESET_SPEED = "reset_speed"
SPEED_UP = "speed_up"
SPEED_DOWN = "speed_down"
PREV_ENV = "prev_env"
NEXT_ENV = "next_env"
TOGGLE_PLOTS = "toggle_plots"
TOGGLE_DEBUG_VIS = "toggle_debug_vis"
TOGGLE_SHOW_ALL_ENVS = "toggle_show_all_envs"
FETCH_CHECKPOINT = "fetch_checkpoint"
CUSTOM = "custom"
class BaseViewer(ABC):
"""Abstract base class for environment viewers."""
SPEED_MULTIPLIERS = [1 / 32, 1 / 16, 1 / 8, 1 / 4, 1 / 2, 1.0, 2.0, 4.0, 8.0]
def __init__(
self,
env: EnvProtocol,
policy: PolicyProtocol,
frame_rate: float = 30.0,
verbosity: int = VerbosityLevel.SILENT,
):
self.env = env
self.policy = policy
self.frame_rate = frame_rate
self.frame_time = 1.0 / frame_rate
self.verbosity = VerbosityLevel(verbosity)
self.cfg = env.cfg.viewer
# State.
self._is_paused = False
self._step_count = 0
self._last_error: str | None = None
# Speed.
self._speed_index = self.SPEED_MULTIPLIERS.index(1.0)
self._time_multiplier = self.SPEED_MULTIPLIERS[self._speed_index]
# Physics accumulator and render timer.
self._sim_budget = 0.0
self._time_until_next_render = 0.0
self._last_tick_time = 0.0
self._was_capped = False
# Windowed stats, updated every 0.5s.
self._stats_frames = 0
self._stats_steps = 0
self._stats_last_time = 0.0
self._fps = 0.0
self._sps = 0.0
# Action queue, drained on main thread each tick.
self._actions: deque[tuple[ViewerAction, Optional[Any]]] = deque()
# Abstract hooks.
@abstractmethod
def setup(self) -> None: ...
@abstractmethod
def sync_env_to_viewer(self) -> None: ...
@abstractmethod
def sync_viewer_to_env(self) -> None: ...
@abstractmethod
def close(self) -> None: ...
@abstractmethod
def is_running(self) -> bool: ...
def _forward_paused(self) -> None: # noqa: B027
"""Hook for subclasses to run forward kinematics while paused."""
def _handle_custom_action(self, action: ViewerAction, payload: Optional[Any]) -> bool:
del action, payload
return False
# Logging.
def log(self, message: str, level: VerbosityLevel = VerbosityLevel.INFO) -> None:
if self.verbosity >= level:
print(message)
# Thread-safe action requests.
def request_reset(self) -> None:
self._actions.append((ViewerAction.RESET, None))
def request_toggle_pause(self) -> None:
self._actions.append((ViewerAction.TOGGLE_PAUSE, None))
def request_single_step(self) -> None:
self._actions.append((ViewerAction.SINGLE_STEP, None))
def request_speed_up(self) -> None:
self._actions.append((ViewerAction.SPEED_UP, None))
def request_speed_down(self) -> None:
self._actions.append((ViewerAction.SPEED_DOWN, None))
def request_reset_speed(self) -> None:
self._actions.append((ViewerAction.RESET_SPEED, None))
def request_action(self, name: str, payload: Optional[Any] = None) -> None:
try:
action = ViewerAction[name]
except KeyError:
action = ViewerAction.CUSTOM
self._actions.append((action, payload))
# Speed controls.
def increase_speed(self) -> None:
if self._speed_index < len(self.SPEED_MULTIPLIERS) - 1:
self._speed_index += 1
self._time_multiplier = self.SPEED_MULTIPLIERS[self._speed_index]
def decrease_speed(self) -> None:
if self._speed_index > 0:
self._speed_index -= 1
self._time_multiplier = self.SPEED_MULTIPLIERS[self._speed_index]
def reset_speed(self) -> None:
self._speed_index = self.SPEED_MULTIPLIERS.index(1.0)
self._time_multiplier = 1.0
# Pause and resume.
def pause(self) -> None:
self._is_paused = True
self.log("[INFO] Simulation paused", VerbosityLevel.INFO)
def resume(self) -> None:
self._is_paused = False
self._last_error = None
self._sim_budget = 0.0
self._last_tick_time = time.perf_counter()
self.log("[INFO] Simulation resumed", VerbosityLevel.INFO)
def toggle_pause(self) -> None:
if self._is_paused:
self.resume()
else:
self.pause()
# Core loop.
def _execute_step(self) -> bool:
"""Run one obs/policy/step cycle.
Returns True on success, False if step failed.
"""
try:
with torch.no_grad():
obs = self.env.get_observations()
actions = self.policy(obs)
dones = self.env.step(actions)[2]
reset_fn = getattr(self.policy, "reset", None)
if reset_fn is not None:
reset_fn(dones)
self._step_count += 1
self._stats_steps += 1
return True
except Exception:
self._last_error = traceback.format_exc()
self.log(
f"[ERROR] Exception during step:\n{self._last_error}",
VerbosityLevel.SILENT,
)
self.pause()
return False
def _step_physics(self, dt: float) -> None:
"""Run physics steps for this frame's sim-time budget."""
step_dt = self.env.unwrapped.step_dt
self._sim_budget += dt * self._time_multiplier
self._was_capped = False
if self._sim_budget < step_dt:
return
self.sync_viewer_to_env()
hit_deadline = False
deadline = time.perf_counter() + self.frame_time
while self._sim_budget >= step_dt:
if not self._execute_step():
self._sim_budget = 0.0
return
self._sim_budget -= step_dt
if time.perf_counter() > deadline:
hit_deadline = True
break
if hit_deadline:
# Only report capped if we actually had to drop remaining work. A transient stall
# (GC pause) during a single step triggers the deadline but leaves no remaining
# budget, so it's not a real cap.
self._was_capped = self._sim_budget >= step_dt
self._sim_budget = min(self._sim_budget, step_dt)
def _single_step(self) -> None:
"""Advance exactly one step while paused."""
if not self._is_paused:
return
self.sync_viewer_to_env()
self._execute_step()
def reset_environment(self) -> None:
self.env.reset()
reset_fn = getattr(self.policy, "reset", None)
if reset_fn is not None:
reset_fn()
self._step_count = 0
self._sim_budget = 0.0
self._last_error = None
self._last_tick_time = time.perf_counter()
def _process_actions(self) -> None:
"""Drain action queue. Runs on the main loop thread."""
while self._actions:
action, payload = self._actions.popleft()
if action == ViewerAction.RESET:
self.reset_environment()
elif action == ViewerAction.TOGGLE_PAUSE:
self.toggle_pause()
elif action == ViewerAction.SINGLE_STEP:
self._single_step()
elif action == ViewerAction.RESET_SPEED:
self.reset_speed()
elif action == ViewerAction.SPEED_UP:
self.increase_speed()
elif action == ViewerAction.SPEED_DOWN:
self.decrease_speed()
else:
_ = self._handle_custom_action(action, payload)
def tick(self) -> bool:
"""Advance one tick: drain actions, step physics, maybe render.
Returns True when a render frame was produced, False otherwise.
"""
now = time.perf_counter()
dt = now - self._last_tick_time
self._last_tick_time = now
self._process_actions()
if self._is_paused:
self._forward_paused()
else:
self._step_physics(dt)
# Render at fixed frame rate.
self._time_until_next_render -= dt
if self._time_until_next_render > 0:
return False
self._time_until_next_render += self.frame_time
if self._time_until_next_render < -self.frame_time:
self._time_until_next_render = 0.0
self.sync_env_to_viewer()
self._stats_frames += 1
return True
def run(self, num_steps: Optional[int] = None, catch_sigint: bool = True) -> None:
self._interrupted = False
self.setup()
now = time.perf_counter()
self._stats_last_time = now
self._last_tick_time = now
prev_handler = None
try:
if catch_sigint:
try:
prev_handler = signal.signal(signal.SIGINT, self._sigint_handler)
except ValueError:
pass # Non-main thread; skip gracefully.
while (
self.is_running()
and (num_steps is None or self._step_count < num_steps)
and not self._interrupted
):
if not self.tick():
time.sleep(0.001)
self._update_stats()
finally:
self.close()
if prev_handler is not None:
signal.signal(signal.SIGINT, prev_handler)
def _sigint_handler(self, signum, frame) -> None:
self._interrupted = True
print("\nCtrl+C received. Shutting down viewer...")
# Restore default so a second Ctrl+C kills immediately.
signal.signal(signal.SIGINT, signal.SIG_DFL)
# Stats.
def _update_stats(self) -> None:
if self._is_paused:
return
now = time.perf_counter()
dt = now - self._stats_last_time
if dt >= 0.5:
self._fps = self._stats_frames / dt
self._sps = self._stats_steps / dt
self._stats_frames = 0
self._stats_steps = 0
self._stats_last_time = now
if self.verbosity >= VerbosityLevel.DEBUG:
status = self.get_status()
print(
f"[{'PAUSED' if status.paused else 'RUNNING'}] "
f"Step {status.step_count} | "
f"FPS: {status.smoothed_fps:.0f} | "
f"Speed: {status.speed_label} | "
f"RTF: {status.actual_realtime:.2f}x / "
f"{status.target_realtime:.2f}x"
)
@property
def target_realtime(self) -> float:
return self._time_multiplier
@property
def actual_realtime(self) -> float:
return self._sps * self.env.unwrapped.step_dt
@staticmethod
def _format_speed(multiplier: float) -> str:
if multiplier == 1.0:
return "1x"
inv = 1.0 / multiplier
inv_rounded = round(inv)
if abs(inv - inv_rounded) < 1e-9 and inv_rounded > 0:
return f"1/{inv_rounded}x"
return f"{multiplier:.3g}x"
def get_status(self) -> ViewerStatus:
return ViewerStatus(
paused=self._is_paused,
step_count=self._step_count,
speed_multiplier=self._time_multiplier,
speed_label=self._format_speed(self._time_multiplier),
target_realtime=self.target_realtime,
actual_realtime=self.actual_realtime,
smoothed_fps=self._fps,
capped=self._was_capped,
last_error=self._last_error,
)