diff --git a/benchmark/benchmark_env_step.py b/benchmark/benchmark_env_step.py new file mode 100644 index 000000000..34b6eaaa3 --- /dev/null +++ b/benchmark/benchmark_env_step.py @@ -0,0 +1,252 @@ +"""Benchmark env.step performance for a given task and backend. + +Usage: + # Run all combinations (go1/go2/g1 x mujoco/motrix): + uv run python scripts/bench_env_step.py + + # Single task + backend: + uv run python scripts/bench_env_step.py task=g1_joystick training.sim_backend=motrix + + # Override bench params: + uv run python scripts/bench_env_step.py bench.num_envs=4096 bench.num_steps=500 +""" + +import sys +from pathlib import Path + +import numpy as np + +ROOT_DIR = Path(__file__).parent.parent + +TASK_CONFIGS = { + "go1": "task=go1_joystick", + "go2": "task=go2_joystick", + "g1": "task=g1_joystick", +} + +BACKENDS = ["mujoco", "motrix"] + + +def _is_matrix_mode(argv: list[str]) -> bool: + """Return True when user didn't specify task or backend explicitly.""" + for arg in argv: + if arg.startswith("task=") or arg.startswith("training.sim_backend="): + return False + return True + + +def _compose_cfg(extra_args: list[str]): + """Compose a Hydra config, handling GlobalHydra lifecycle.""" + from hydra import compose, initialize_config_dir + from hydra.core.global_hydra import GlobalHydra + + config_dir = str(ROOT_DIR / "conf" / "ppo") + + overrides = list(extra_args) + [ + "hydra.run.dir=.", + "hydra.output_subdir=null", + "hydra/job_logging=disabled", + "hydra/hydra_logging=disabled", + ] + + GlobalHydra.instance().clear() + with initialize_config_dir(config_dir=config_dir, version_base="1.3"): + return compose(config_name="config", overrides=overrides) + + +def _run_single(extra_args: list[str]) -> dict: + """Run a single bench in-process via Hydra and return timing records.""" + from omegaconf import OmegaConf + + cfg = _compose_cfg(extra_args) + + from unilab.training import BackendAdapter, create_env, ensure_registries + + ensure_registries() + + bench = OmegaConf.select(cfg, "bench", default={}) + num_envs = int(bench.get("num_envs", 2048)) if bench else 2048 + num_steps = int(bench.get("num_steps", 200)) if bench else 200 + warmup_steps = int(bench.get("warmup_steps", 10)) if bench else 10 + + task_name = cfg.training.task_name + sim_backend = cfg.training.sim_backend + + adapter = BackendAdapter(cfg, root_dir=ROOT_DIR, algo_name="ppo") + env_cfg_override = adapter.build_task_env_cfg_override() + + env = create_env( + cfg, + num_envs=num_envs, + env_cfg_override=env_cfg_override, + sim_backend=sim_backend, + task_name=task_name, + ) + + nu = env._backend.num_actuators + env.init_state() + + for _ in range(warmup_steps): + actions = np.random.uniform(-1, 1, size=(num_envs, nu)).astype(np.float32) + env.step(actions) + + timing_records: dict[str, list[float]] = {} + for _ in range(num_steps): + actions = np.random.uniform(-1, 1, size=(num_envs, nu)).astype(np.float32) + state = env.step(actions) + timing = state.info.get("timing", {}) + for k, v in timing.items(): + timing_records.setdefault(k, []).append(v) + + env.close() + + return { + "task_name": task_name, + "sim_backend": sim_backend, + "num_envs": num_envs, + "num_steps": num_steps, + "warmup_steps": warmup_steps, + "timing_records": timing_records, + } + + +def _print_single_report(result: dict) -> None: + tr = result["timing_records"] + total_arr = np.array(tr.get("env_step_total_ms", [])) + total_s = total_arr.sum() / 1000.0 + steps_per_env = result["num_steps"] * result["num_envs"] + + print(f"\n{'=' * 60}") + print(f" Task: {result['task_name']}") + print(f" Backend: {result['sim_backend']}") + print(f" Num envs: {result['num_envs']}") + print(f" Steps: {result['num_steps']} (warmup: {result['warmup_steps']})") + print(f"{'=' * 60}") + print(f" Total time: {total_s:.3f}s") + print(f" Mean step time: {total_arr.mean():.3f}ms") + print(f" Median step time: {np.median(total_arr):.3f}ms") + print(f" Std step time: {total_arr.std():.3f}ms") + print(f" Min step time: {total_arr.min():.3f}ms") + print(f" Max step time: {total_arr.max():.3f}ms") + print(f" Throughput: {steps_per_env / total_s:.0f} env-steps/s") + if tr: + print(f"{'- ' * 30}") + print(" Breakdown:") + for k, v in tr.items(): + if k == "env_step_total_ms": + continue + arr = np.array(v) + print(f" {k:25s} mean={arr.mean():.3f}ms median={np.median(arr):.3f}ms") + print(f"{'=' * 60}") + + +def _short_task_label(task_name: str) -> str: + """Shorten 'Go1JoystickFlatTerrain' → 'go1'.""" + name = task_name.lower() + for prefix in ("go1", "go2", "g1"): + if name.startswith(prefix): + return prefix + return task_name[:8] + + +def _print_comparison_table(results: list[dict]) -> None: + rows_spec: list[tuple[str, str]] = [ + # (display_label, timing_key_or_special) + ("total", "env_step_total_ms"), + (" apply_action", "apply_action_ms"), + (" step_core", "step_core_ms"), + (" set_ctrl", "backend_set_ctrl_ms"), + (" physics", "backend_physics_ms"), + (" refresh", "backend_refresh_cache_ms"), + (" update_state", "update_state_ms"), + (" reset_done", "reset_done_ms"), + ("throughput", "__throughput__"), + ] + + # Build short column labels + col_labels = [f"{_short_task_label(r['task_name'])}/{r['sim_backend']}" for r in results] + + metric_w = 16 + col_w = max(12, max(len(c) for c in col_labels) + 2) + + def hline(left: str, mid: str, right: str, fill: str = "─") -> str: + return left + fill * metric_w + mid + mid.join(fill * col_w for _ in results) + right + + # Header + print() + print(hline("┌", "┬", "┐")) + header = "│" + "metric".center(metric_w) + "│" + header += "│".join(c.center(col_w) for c in col_labels) + "│" + print(header) + + unit_row = "│" + "(median ms)".center(metric_w) + "│" + unit_row += "│".join(" " * col_w for _ in results) + "│" + print(unit_row) + print(hline("├", "┼", "┤")) + + # Data rows + for label, key in rows_spec: + cells: list[str] = [] + if key == "__throughput__": + for r in results: + total_arr = np.array(r["timing_records"].get("env_step_total_ms", [])) + total_s = total_arr.sum() / 1000.0 + steps_per_env = r["num_steps"] * r["num_envs"] + cells.append(f"{steps_per_env / total_s:,.0f}" if total_s > 0 else "-") + else: + for r in results: + arr = r["timing_records"].get(key) + cells.append(f"{np.median(arr):.3f}" if arr else "-") + + row = "│" + label.ljust(metric_w) + "│" + row += "│".join(v.rjust(col_w - 1) + " " for v in cells) + "│" + + # Separator before throughput + if key == "__throughput__": + print(hline("├", "┼", "┤")) + + print(row) + + print(hline("└", "┴", "┘")) + if results: + r0 = results[0] + print(f" ({r0['num_envs']} envs, {r0['num_steps']} steps, throughput = env-steps/s)") + + +def _run_matrix(extra_args: list[str]) -> None: + """Run all task x backend combinations and print comparison.""" + from unilab.base.backend.motrix_backend import MOTRIX_AVAILABLE + + backends = BACKENDS if MOTRIX_AVAILABLE else ["mujoco"] + if not MOTRIX_AVAILABLE: + print("Note: motrixsim not available, running mujoco only\n") + + results: list[dict] = [] + for task_key, task_override in TASK_CONFIGS.items(): + for backend in backends: + label = f"{task_key}/{backend}" + print(f"Running {label} ...", flush=True) + try: + args = [task_override, f"training.sim_backend={backend}"] + extra_args + result = _run_single(args) + results.append(result) + _print_single_report(result) + except Exception as e: + print(f" FAILED: {e}\n") + + if len(results) > 1: + _print_comparison_table(results) + + +def main() -> None: + argv = sys.argv[1:] + + if _is_matrix_mode(argv): + _run_matrix(argv) + else: + result = _run_single(argv) + _print_single_report(result) + + +if __name__ == "__main__": + main() diff --git a/conf/offpolicy/task/go2_joystick.yaml b/conf/offpolicy/task/go2_joystick.yaml index 647f51905..f635c7439 100644 --- a/conf/offpolicy/task/go2_joystick.yaml +++ b/conf/offpolicy/task/go2_joystick.yaml @@ -7,3 +7,9 @@ training: algo: num_envs: 1024 max_iterations: 3000 +motrix_legacy: + enabled: true + env_cfg_override: + domain_rand: + randomize_kp: false + randomize_kd: false diff --git a/conf/ppo/task/g1_joystick.yaml b/conf/ppo/task/g1_joystick.yaml index db3f8ede4..06d418fc0 100644 --- a/conf/ppo/task/g1_joystick.yaml +++ b/conf/ppo/task/g1_joystick.yaml @@ -26,6 +26,7 @@ motrix_legacy: learning_rate: 3.0e-4 entropy_coef: 5.0e-3 env_cfg_override: + iterations: 3 reward_config: scales: tracking_lin_vel: 3.0 diff --git a/src/unilab/base/backend/__init__.py b/src/unilab/base/backend/__init__.py index 7d591f22c..57d790ff9 100644 --- a/src/unilab/base/backend/__init__.py +++ b/src/unilab/base/backend/__init__.py @@ -13,7 +13,7 @@ def create_backend( model_file: 模型文件路径 num_envs: 环境数量 sim_dt: 仿真时间步长 - **kwargs: 其他参数 + **kwargs: 其他参数(iterations, position_actuator_gains 等) Returns: SimBackend 实例 diff --git a/src/unilab/base/backend/base.py b/src/unilab/base/backend/base.py index 8805abe7c..000ecad32 100644 --- a/src/unilab/base/backend/base.py +++ b/src/unilab/base/backend/base.py @@ -95,12 +95,15 @@ def get_joint_range(self) -> np.ndarray | None: # ------------------------------------------------------------------ # @abc.abstractmethod - def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None: + def step(self, ctrl: np.ndarray, nsteps: int = 1) -> dict | None: """执行物理步进 Args: ctrl: 控制输入 (num_envs, nu) nsteps: 步进次数 + + Returns: + 可选的 dict,可包含 "timing" key 记录各阶段耗时(ms) """ @abc.abstractmethod diff --git a/src/unilab/base/backend/motrix_backend.py b/src/unilab/base/backend/motrix_backend.py index 5ebb24624..d3740f139 100644 --- a/src/unilab/base/backend/motrix_backend.py +++ b/src/unilab/base/backend/motrix_backend.py @@ -1,4 +1,5 @@ import os +import time from collections.abc import Sequence import numpy as np @@ -33,6 +34,7 @@ def __init__( base_name: str = "base", np_dtype=np.float32, add_body_sensors: bool = False, + iterations: int | None = None, ): if not MOTRIX_AVAILABLE: raise ImportError("motrixsim not available") @@ -66,6 +68,8 @@ def __init__( } self._model.options.timestep = sim_dt + if iterations is not None: + self._model.options.max_iterations = int(iterations) self._num_envs = num_envs self._np_dtype = np_dtype @@ -150,11 +154,29 @@ def get_joint_range(self) -> np.ndarray | None: # Simulation control # # ------------------------------------------------------------------ # - def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None: + def step(self, ctrl: np.ndarray, nsteps: int = 1) -> dict | None: + t0 = time.perf_counter() self._data.actuator_ctrls = np.ascontiguousarray(ctrl) - for _ in range(nsteps): + set_ctrl_ms = (time.perf_counter() - t0) * 1000.0 + + t0 = time.perf_counter() + if nsteps == 1: self._model.step(self._data) + else: + self._model.step_n(self._data, nsteps) + physics_ms = (time.perf_counter() - t0) * 1000.0 + + t0 = time.perf_counter() self._refresh_link_pose_cache() + refresh_cache_ms = (time.perf_counter() - t0) * 1000.0 + + return { + "timing": { + "set_ctrl_ms": set_ctrl_ms, + "physics_ms": physics_ms, + "refresh_cache_ms": refresh_cache_ms, + } + } def set_state( self, @@ -182,8 +204,8 @@ def set_state( ctrl = qpos_motrix[:, 7:] data_slice.actuator_ctrls = np.ascontiguousarray(ctrl) - self._model.forward_kinematic(self._data) - self._refresh_link_pose_cache() + self._model.forward_kinematic(data_slice) + self._refresh_link_pose_cache(env_indices) def get_dr_capabilities(self) -> DomainRandomizationCapabilities: return DomainRandomizationCapabilities( @@ -279,24 +301,21 @@ def _get_link_poses_w(self, body_ids: np.ndarray) -> np.ndarray: def _get_link_lin_vel_w(self, body_ids: np.ndarray) -> np.ndarray: ids = self._as_body_ids(body_ids) return np.stack( - [np.asarray(self._link_cache[int(bid)].get_linear_velocity(self._data)) for bid in ids], + [self._link_cache[int(bid)].get_linear_velocity(self._data) for bid in ids], axis=1, ) def _get_link_ang_vel_w(self, body_ids: np.ndarray) -> np.ndarray: ids = self._as_body_ids(body_ids) return np.stack( - [ - np.asarray(self._link_cache[int(bid)].get_angular_velocity(self._data)) - for bid in ids - ], + [self._link_cache[int(bid)].get_angular_velocity(self._data) for bid in ids], axis=1, ) def _get_body_sensor_values(self, body_ids: np.ndarray, prefix: str) -> np.ndarray: return np.stack( [ - np.asarray(self._model.get_sensor_value(f"{prefix}_{name}", self._data)) + self._model.get_sensor_value(f"{prefix}_{name}", self._data) for name in self._get_body_names(body_ids) ], axis=1, @@ -306,8 +325,13 @@ def _xyzw_to_wxyz(self, q: np.ndarray) -> np.ndarray: """motrix xyzw → wxyz""" return q[..., [3, 0, 1, 2]] - def _refresh_link_pose_cache(self) -> None: - self._link_poses = np.ascontiguousarray(np.asarray(self._model.get_link_poses(self._data))) + def _refresh_link_pose_cache(self, env_indices: np.ndarray | None = None) -> None: + if env_indices is None: + self._link_poses = self._model.get_link_poses(self._data) + else: + mask = np.zeros(self._num_envs, dtype=bool) + mask[env_indices] = True + self._link_poses[env_indices] = self._model.get_link_poses(self._data[mask]) def push_robots(self, force_range): ex_force = np.random.rand(self.num_envs, 3) * 2 - 1 # [x_force, y_force, z_force] diff --git a/src/unilab/base/backend/mujoco_backend.py b/src/unilab/base/backend/mujoco_backend.py index 5e8a635d9..629d60052 100644 --- a/src/unilab/base/backend/mujoco_backend.py +++ b/src/unilab/base/backend/mujoco_backend.py @@ -1,4 +1,5 @@ import os +import time from collections.abc import Sequence from multiprocessing import cpu_count from typing import Optional, cast @@ -41,6 +42,7 @@ def __init__( np_dtype=None, add_body_sensors: bool = False, position_actuator_gains: dict | None = None, + iterations: int | None = None, ): self.add_body_sensors = add_body_sensors self._base_name = base_name @@ -72,6 +74,8 @@ def __init__( self._body_id_to_tracked_idx[bid] = idx self._model.opt.timestep = sim_dt + if iterations is not None: + self._model.opt.iterations = int(iterations) if position_actuator_gains is not None: self._apply_position_actuator_gains_to_model(self._model, **position_actuator_gains) self._base_body_id = ( @@ -230,8 +234,12 @@ def get_joint_range(self) -> np.ndarray | None: # Simulation control # # ------------------------------------------------------------------ # - def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None: + def step(self, ctrl: np.ndarray, nsteps: int = 1) -> dict | None: + t0 = time.perf_counter() control_traj = np.broadcast_to(ctrl[:, None, :], (self._num_envs, nsteps, ctrl.shape[-1])) + set_ctrl_ms = (time.perf_counter() - t0) * 1000.0 + + t0 = time.perf_counter() state_np = self._pool.step( self._physics_state, nstep=nsteps, @@ -239,9 +247,20 @@ def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None: control_spec=int(mujoco.mjtState.mjSTATE_CTRL), ) self._physics_state[:] = state_np.astype(self._np_dtype) + physics_ms = (time.perf_counter() - t0) * 1000.0 + t0 = time.perf_counter() sensor_np = self._pool.forward(self._physics_state) self._sensor_data[:] = sensor_np.astype(self._np_dtype) + refresh_cache_ms = (time.perf_counter() - t0) * 1000.0 + + return { + "timing": { + "set_ctrl_ms": set_ctrl_ms, + "physics_ms": physics_ms, + "refresh_cache_ms": refresh_cache_ms, + } + } def set_state( self, diff --git a/src/unilab/base/base.py b/src/unilab/base/base.py index 0e92ea8fc..e067458ad 100644 --- a/src/unilab/base/base.py +++ b/src/unilab/base/base.py @@ -18,6 +18,7 @@ class EnvCfg: max_episode_seconds: Optional[float] = None ctrl_dt: float = 0.01 render_spacing: float = 1.0 + iterations: Optional[int] = None @property def max_episode_steps(self) -> Optional[int]: diff --git a/src/unilab/base/np_env.py b/src/unilab/base/np_env.py index 7edad69ca..ca3594d8b 100644 --- a/src/unilab/base/np_env.py +++ b/src/unilab/base/np_env.py @@ -90,7 +90,10 @@ def step(self, actions: np.ndarray) -> NpEnvState: self.init_state() assert self._state is not None + + t0 = time.perf_counter() ctrl = self.apply_action(actions, self._state) + apply_action_time = time.perf_counter() - t0 if self._dr_manager is not None: self._dr_manager.apply_interval_randomization_if_due(self.step_counter) @@ -100,7 +103,7 @@ def step(self, actions: np.ndarray) -> NpEnvState: final_obs_mask.fill(False) t0 = time.perf_counter() - self._backend.step(ctrl, self._cfg.sim_substeps) + backend_result = self._backend.step(ctrl, self._cfg.sim_substeps) step_core_time = time.perf_counter() - t0 t0 = time.perf_counter() @@ -120,9 +123,15 @@ def step(self, actions: np.ndarray) -> NpEnvState: timing = self._state.info.setdefault("timing", {}) timing["env_step_total_ms"] = (time.perf_counter() - step_t0) * 1000.0 + timing["apply_action_ms"] = apply_action_time * 1000.0 timing["step_core_ms"] = step_core_time * 1000.0 timing["update_state_ms"] = update_state_time * 1000.0 timing["reset_done_ms"] = reset_done_time * 1000.0 + if backend_result is not None: + backend_timing = backend_result.get("timing") + if backend_timing: + for k, v in backend_timing.items(): + timing[f"backend_{k}"] = v return self._state diff --git a/src/unilab/envs/locomotion/g1/joystick.py b/src/unilab/envs/locomotion/g1/joystick.py index 5726621c2..eceb4a61f 100644 --- a/src/unilab/envs/locomotion/g1/joystick.py +++ b/src/unilab/envs/locomotion/g1/joystick.py @@ -183,7 +183,12 @@ def __init__(self, cfg: G1JoystickPPOCfg, num_envs=1, backend_type="mujoco"): raise ValueError("reward_config must be provided via Hydra configuration") cfg.apply_backend_overrides(backend_type) backend = create_backend( - backend_type, cfg.model_file, num_envs, cfg.sim_dt, base_name=cfg.asset.base_name + backend_type, + cfg.model_file, + num_envs, + cfg.sim_dt, + base_name=cfg.asset.base_name, + iterations=cfg.iterations, ) super().__init__(cfg, backend, num_envs) self._enable_reward_log = True diff --git a/src/unilab/envs/locomotion/g1/joystick_sac.py b/src/unilab/envs/locomotion/g1/joystick_sac.py index ce926d3bd..3699a60a9 100644 --- a/src/unilab/envs/locomotion/g1/joystick_sac.py +++ b/src/unilab/envs/locomotion/g1/joystick_sac.py @@ -69,7 +69,12 @@ def __init__(self, cfg: G1JoystickSACCfg, num_envs=1, backend_type="mujoco"): if cfg.reward_config is None: raise ValueError("reward_config must be provided via Hydra configuration") backend = create_backend( - backend_type, cfg.model_file, num_envs, cfg.sim_dt, base_name=cfg.asset.base_name + backend_type, + cfg.model_file, + num_envs, + cfg.sim_dt, + base_name=cfg.asset.base_name, + iterations=cfg.iterations, ) G1BaseEnv.__init__(self, cfg, backend, num_envs) self._enable_reward_log = True diff --git a/src/unilab/envs/locomotion/go1/joystick.py b/src/unilab/envs/locomotion/go1/joystick.py index df3e83435..ac55a2708 100644 --- a/src/unilab/envs/locomotion/go1/joystick.py +++ b/src/unilab/envs/locomotion/go1/joystick.py @@ -105,6 +105,7 @@ def __init__(self, cfg: Go1JoystickCfg, num_envs=1, backend_type="mujoco"): cfg.sim_dt, base_name=cfg.asset.base_name, position_actuator_gains={"kp": cfg.control_config.Kp, "kd": cfg.control_config.Kd}, + iterations=cfg.iterations, ) super().__init__(cfg, backend, num_envs) self._enable_reward_log = True diff --git a/src/unilab/envs/locomotion/go2/joystick.py b/src/unilab/envs/locomotion/go2/joystick.py index c9a01b816..c4e9a090f 100644 --- a/src/unilab/envs/locomotion/go2/joystick.py +++ b/src/unilab/envs/locomotion/go2/joystick.py @@ -108,6 +108,7 @@ def __init__(self, cfg: Go2JoystickCfg, num_envs=1, backend_type="mujoco"): cfg.sim_dt, base_name=cfg.asset.base_name, position_actuator_gains={"kp": cfg.control_config.Kp, "kd": cfg.control_config.Kd}, + iterations=cfg.iterations, ) super().__init__(cfg, backend, num_envs) self._enable_reward_log = True diff --git a/src/unilab/envs/manipulation/inhand_rot_allegro/rotation.py b/src/unilab/envs/manipulation/inhand_rot_allegro/rotation.py index f9d5586b1..39bf652ea 100644 --- a/src/unilab/envs/manipulation/inhand_rot_allegro/rotation.py +++ b/src/unilab/envs/manipulation/inhand_rot_allegro/rotation.py @@ -173,6 +173,7 @@ def __init__(self, cfg: AllegroRotationCfg, num_envs: int = 1, backend_type: str "kd": cfg.control_config.kd, "actuator_ids": slice(0, 16), }, + iterations=cfg.iterations, ) super().__init__(cfg, backend, num_envs) self._enable_reward_log = True diff --git a/src/unilab/envs/manipulation/inhand_rot_allegro/rotation_sac.py b/src/unilab/envs/manipulation/inhand_rot_allegro/rotation_sac.py index 634e6f1f3..378c0fe87 100644 --- a/src/unilab/envs/manipulation/inhand_rot_allegro/rotation_sac.py +++ b/src/unilab/envs/manipulation/inhand_rot_allegro/rotation_sac.py @@ -170,6 +170,7 @@ def __init__(self, cfg: AllegroRotationSacCfg, num_envs: int = 1, backend_type: "kd": cfg.control_config.kd, "actuator_ids": slice(0, 16), }, + iterations=cfg.iterations, ) super().__init__(cfg, backend, num_envs) self._enable_reward_log = True diff --git a/src/unilab/envs/motion_tracking/g1/tracking.py b/src/unilab/envs/motion_tracking/g1/tracking.py index 3036e2fde..114ddd112 100644 --- a/src/unilab/envs/motion_tracking/g1/tracking.py +++ b/src/unilab/envs/motion_tracking/g1/tracking.py @@ -304,6 +304,7 @@ def __init__(self, cfg: G1MotionTrackingCfg, num_envs=1, backend_type="mujoco"): cfg.sim_dt, base_name=cfg.asset.base_name, add_body_sensors=True, + iterations=cfg.iterations, ) super().__init__(cfg, backend, num_envs) if backend_type not in {"mujoco", "motrix"}: