diff --git a/src/holosoma/holosoma/agents/callbacks/base_callback.py b/src/holosoma/holosoma/agents/callbacks/base_callback.py index a0fc5f722..1d670809d 100644 --- a/src/holosoma/holosoma/agents/callbacks/base_callback.py +++ b/src/holosoma/holosoma/agents/callbacks/base_callback.py @@ -1,3 +1,7 @@ +from __future__ import annotations + +from typing import Any + from torch.nn import Module @@ -8,6 +12,19 @@ def __init__(self, config, training_loop): self.training_loop = training_loop self.device = self.training_loop.device + def _get_env(self): + """Get the unwrapped BaseTask environment.""" + return self.training_loop._unwrap_env() + + def _require_recording_cb(self) -> Any: + """Find and return the EvalRecordingCallback, or raise.""" + from holosoma.agents.callbacks.recording import EvalRecordingCallback + + for cb in self.training_loop.eval_callbacks: + if isinstance(cb, EvalRecordingCallback): + return cb + raise RuntimeError(f"{type(self).__name__} requires EvalRecordingCallback. Set --recording.config.enabled=True") + def on_pre_evaluate_policy(self): pass diff --git a/src/holosoma/holosoma/agents/callbacks/gait_analysis.py b/src/holosoma/holosoma/agents/callbacks/gait_analysis.py new file mode 100644 index 000000000..c37518c03 --- /dev/null +++ b/src/holosoma/holosoma/agents/callbacks/gait_analysis.py @@ -0,0 +1,172 @@ +"""Gait cycle analysis for foot-height-based phase detection. + +Collects per-env foot height data over a configurable analysis window, +counts full gait cycles via zero-crossings, then provides vectorized +per-env gait phase detection. + +Phases detected (based on left foot state): + swing_to_stance = left foot touchdown (air -> ground) + stance_to_swing = left foot liftoff (ground -> air) + mid_stance = left foot on ground, right foot at peak + mid_swing = left foot at peak (in air) +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +from loguru import logger + +_PEAK_THRESHOLD = 0.65 + + +class GaitAnalyser: + """Analyses foot height oscillations to detect gait phases. + + Lifecycle: + 1. Call ``record_foot_heights()`` each step during the analysis window. + 2. Call ``try_finalize()`` once enough steps have elapsed. + When it returns True, phase detection is ready. + 3. Call ``detect_phases()`` each step to get per-env phase labels. + 4. Call ``get_metadata()`` to retrieve analysis results for recording. + """ + + def __init__( + self, + sim: Any, + left_foot_isaac_id: int, + right_foot_isaac_id: int, + num_conditions: int, + *, + foot_contact_threshold: float = 0.5, + min_gait_cycles: int = 3, + ): + self._sim = sim + self._left_foot_isaac_id = left_foot_isaac_id + self._right_foot_isaac_id = right_foot_isaac_id + self._num_conditions = num_conditions + self._foot_contact_threshold = foot_contact_threshold + self._min_gait_cycles = min_gait_cycles + + self._done = False + self._left_foot_z_history: list[np.ndarray] = [] + self._right_foot_z_history: list[np.ndarray] = [] + self._left_foot_z_range: tuple[np.ndarray, np.ndarray] | None = None + self._right_foot_z_range: tuple[np.ndarray, np.ndarray] | None = None + self._prev_left_on_ground: np.ndarray = np.zeros(num_conditions, dtype=bool) + + self._metadata: dict[str, Any] = {} + + @property + def done(self) -> bool: + return self._done + + def record_foot_heights(self) -> None: + n = self._num_conditions + left_z = self._sim._robot.data.body_pos_w[:n, self._left_foot_isaac_id, 2] + right_z = self._sim._robot.data.body_pos_w[:n, self._right_foot_isaac_id, 2] + self._left_foot_z_history.append(left_z.detach().cpu().numpy().copy()) + self._right_foot_z_history.append(right_z.detach().cpu().numpy().copy()) + + def try_finalize(self, *, force: bool = False) -> bool: + if len(self._left_foot_z_history) < 2: + return False + + left_z = np.stack(self._left_foot_z_history, axis=0) + right_z = np.stack(self._right_foot_z_history, axis=0) + cycles = _count_gait_cycles(left_z) + + if not force and cycles < self._min_gait_cycles: + return False + + if cycles < self._min_gait_cycles: + logger.warning( + f"GaitAnalyser: forced finalization with only " + f"{cycles} cycles (need {self._min_gait_cycles}). " + f"Phase detection may be unreliable." + ) + + self._left_foot_z_range = (np.min(left_z, axis=0), np.max(left_z, axis=0)) + self._right_foot_z_range = (np.min(right_z, axis=0), np.max(right_z, axis=0)) + self._done = True + + # Initialize ground-contact state from the last recorded frame + left_range = np.maximum(self._left_foot_z_range[1] - self._left_foot_z_range[0], 1e-4) + left_norm = (left_z[-1] - self._left_foot_z_range[0]) / left_range + self._prev_left_on_ground = left_norm < self._foot_contact_threshold + + self._metadata = { + "gait_left_foot_z_min": self._left_foot_z_range[0].tolist(), + "gait_left_foot_z_max": self._left_foot_z_range[1].tolist(), + "gait_right_foot_z_min": self._right_foot_z_range[0].tolist(), + "gait_right_foot_z_max": self._right_foot_z_range[1].tolist(), + "gait_analysis_cycles": cycles, + "gait_analysis_steps": len(self._left_foot_z_history), + } + + self._left_foot_z_history.clear() + self._right_foot_z_history.clear() + + logger.info( + f"GaitAnalyser: analysis complete ({cycles} cycles). " + f"Left Z: [{self._left_foot_z_range[0].mean():.3f}, {self._left_foot_z_range[1].mean():.3f}], " + f"Right Z: [{self._right_foot_z_range[0].mean():.3f}, {self._right_foot_z_range[1].mean():.3f}]" + ) + return True + + def detect_phases(self) -> np.ndarray: + """Vectorized gait phase detection for all envs. Single GPU sync.""" + assert self._left_foot_z_range is not None + assert self._right_foot_z_range is not None + + n = self._num_conditions + left_z = self._sim._robot.data.body_pos_w[:n, self._left_foot_isaac_id, 2].cpu().numpy() + right_z = self._sim._robot.data.body_pos_w[:n, self._right_foot_isaac_id, 2].cpu().numpy() + + threshold = self._foot_contact_threshold + left_min, left_max = self._left_foot_z_range + right_min, right_max = self._right_foot_z_range + + # --- Normalize foot heights to [0, 1] using observed range --- + left_range = np.maximum(left_max - left_min, 1e-4) + right_range = np.maximum(right_max - right_min, 1e-4) + + left_norm = (left_z - left_min) / left_range + right_norm = (right_z - right_min) / right_range + + # --- Detect ground contact and transitions --- + left_on_ground = left_norm < threshold + prev_left_on_ground = self._prev_left_on_ground + self._prev_left_on_ground = left_on_ground.copy() + + phases: np.ndarray = np.full(n, "", dtype=object) + + # Transitions: left foot crossing the ground threshold + swing_to_stance = left_on_ground & ~prev_left_on_ground + stance_to_swing = ~left_on_ground & prev_left_on_ground + phases[swing_to_stance] = "swing_to_stance" + phases[stance_to_swing] = "stance_to_swing" + + # Mid-phase: no transition, detect by peak height of the airborne foot + no_transition = ~swing_to_stance & ~stance_to_swing + phases[no_transition & ~left_on_ground & (left_norm >= _PEAK_THRESHOLD)] = "mid_swing" + phases[no_transition & left_on_ground & (right_norm >= _PEAK_THRESHOLD)] = "mid_stance" + + return phases + + def get_metadata(self) -> dict[str, Any]: + return self._metadata.copy() + + +def _count_gait_cycles(foot_z: np.ndarray) -> int: + """Count full gait cycles from zero-crossings of centered foot-Z signal.""" + # Center signal around mean, count sign changes (zero-crossings), + # two crossings = one full cycle. Return min across all envs. + mean_z = np.mean(foot_z, axis=0, keepdims=True) + centered = foot_z - mean_z + sign = np.sign(centered) + sign[sign == 0] = 1 + crossings = np.sum(np.abs(np.diff(sign, axis=0)) > 0, axis=0) + cycles = crossings // 2 + return int(np.min(cycles)) if cycles.size > 0 else 0 diff --git a/src/holosoma/holosoma/agents/callbacks/grid_conditions.py b/src/holosoma/holosoma/agents/callbacks/grid_conditions.py new file mode 100644 index 000000000..e8ec9bcb1 --- /dev/null +++ b/src/holosoma/holosoma/agents/callbacks/grid_conditions.py @@ -0,0 +1,195 @@ +"""Grid condition manager for multi-condition evaluation sweeps. + +Owns the cross-product logic that combines sweep axes (velocity, payload, +push, etc.) into a flat list of per-env conditions. Any callback can +register a sweep axis; after all registrations, ``finalize()`` expands +the Cartesian product and validates against ``num_envs``. + +The manager is created by EvalRecordingCallback (the single recorder) and +shared with other callbacks via ``_require_recording_cb()``. + +Terminology: + axis — one factor of the grid, added via ``add_axis()``. + Each axis contributes one "slot" to the Cartesian product. + An axis can control a single variable (``name="push_force_n"``) + or several coupled variables + (``name=["lin_vel_x", "lin_vel_y", "ang_vel_yaw"]``). + group — a label used only for metadata output grouping in the NPZ. + Does not affect the grid logic. +""" + +from __future__ import annotations + +import itertools +from typing import Any + +from loguru import logger + + +class GridConditionManager: + """Manages sweep axes and the resulting per-env condition list. + + Lifecycle: + 1. Created during ``on_pre_evaluate_policy`` by the recording callback. + 2. Other callbacks call ``add_axis()`` during their own + ``on_pre_evaluate_policy`` (callbacks execute in field-declaration + order from ``EvalCallbacksConfig``). + 3. Recording callback calls ``finalize()`` on the first env step + (deferred so all axes are registered). + """ + + def __init__(self) -> None: + self._axes: list[dict[str, Any]] = [] + self.conditions: list[dict[str, Any]] = [] + self.num_conditions: int = 0 + self.warmup_steps: int = 0 + self._finalized: bool = False + + def add_axis( + self, + name: str | list[str], + values: list, + *, + labels: list[str] | None = None, + group: str = "", + ) -> None: + """Register one axis of the evaluation grid. + + The grid is the Cartesian product of all registered axes. + + Single-variable axis (one key per condition entry):: + + cm.add_axis("push_force_n", [100.0, 150.0], group="push") + + Multi-variable axis (coupled keys that vary together):: + + cm.add_axis( + ["lin_vel_x", "lin_vel_y", "ang_vel_yaw"], + [(0.5, 0, 0), (1.0, 0, 0), (0, 0.3, 0)], + group="velocity", + ) + + Args: + name: A single key string, or a list of keys for coupled variables. + values: The sweep values for this axis. For a single key, a flat + list (e.g. ``[0.5, 1.0]``). For coupled keys, a list of + tuples with one element per key. + labels: Human-readable labels (optional, defaults to str(value)). + group: Logical group for metadata output (e.g. "velocity", "push"). + + Must be called before ``finalize()``. + """ + if self._finalized: + raise RuntimeError(f"Cannot add axis '{name}' after conditions are finalized.") + + if isinstance(name, str): + keys = [name] + normalized_values = [(v,) for v in values] + else: + keys = list(name) + normalized_values = [tuple(v) for v in values] + + self._axes.append( + { + "keys": keys, + "values": normalized_values, + "labels": labels, + "group": group, + } + ) + + def finalize(self, num_envs: int) -> None: + """Expand the Cartesian product of all axes and validate env count. + + After this call, ``conditions`` and ``num_conditions`` are set. + """ + if self._finalized: + return + + # --- Expand Cartesian product of all axes --- + if not self._axes: + self.conditions = [{}] + else: + value_lists = [ax["values"] for ax in self._axes] + key_lists = [ax["keys"] for ax in self._axes] + self.conditions = [] + for combo in itertools.product(*value_lists): + cond: dict[str, Any] = {} + for keys, vals in zip(key_lists, combo): + cond.update(dict(zip(keys, vals))) + self.conditions.append(cond) + + # --- Deduplicate (preserving order) --- + seen: set[tuple] = set() + unique: list[dict[str, Any]] = [] + for cond in self.conditions: + key = tuple(sorted(cond.items())) + if key not in seen: + seen.add(key) + unique.append(cond) + self.conditions = unique + self.num_conditions = len(unique) + + if num_envs < self.num_conditions: + raise RuntimeError( + f"GridConditionManager: need num_envs >= {self.num_conditions} " + f"for {self.num_conditions} conditions, but got num_envs={num_envs}. " + f"Set --training.num_envs={self.num_conditions}" + ) + if num_envs > self.num_conditions: + logger.warning( + f"GridConditionManager: num_envs={num_envs} > {self.num_conditions} conditions. " + f"Only using first {self.num_conditions} envs." + ) + + self._finalized = True + + logger.info(f"GridConditionManager: finalized {self.num_conditions} conditions from {len(self._axes)} axes") + for i, cond in enumerate(self.conditions): + parts = [f"{k}={v:.2f}" if isinstance(v, float) else f"{k}={v}" for k, v in cond.items()] + logger.info(f" env {i}: {' '.join(parts)}") + + def get_metadata(self) -> dict[str, Any]: + """Return condition metadata for NPZ recording. + + ``grid_conditions`` uses hierarchical dicts grouped by the ``group`` + parameter passed to ``add_axis()``. Ungrouped keys stay at + the top level. + + Example condition:: + + {'velocity': {'lin_vel_x': 0.5, 'lin_vel_y': 0.0, 'ang_vel_yaw': 0.0}, + 'push': {'body_label': 'torso', 'direction': 'forward', 'force_n': 150.0}} + """ + key_to_group: dict[str, str] = {} + for ax in self._axes: + grp = ax.get("group", "") + for k in ax["keys"]: + key_to_group[k] = grp + + hierarchical: list[dict[str, Any]] = [] + for cond in self.conditions: + grouped: dict[str, Any] = {} + for key, val in cond.items(): + grp = key_to_group.get(key, "") + if grp: + grouped.setdefault(grp, {})[key] = val + else: + grouped[key] = val + hierarchical.append(grouped) + + meta: dict[str, Any] = { + "num_conditions": self.num_conditions, + "grid_conditions": hierarchical, + } + for ax in self._axes: + keys = ax["keys"] + values = ax["values"] + if len(keys) == 1: + meta[f"sweep_{keys[0]}_values"] = [v[0] for v in values] + if ax["labels"] is not None: + meta[f"sweep_{keys[0]}_labels"] = ax["labels"] + else: + for k_idx, k in enumerate(keys): + meta[f"sweep_{k}_values"] = [v[k_idx] for v in values] + return meta diff --git a/src/holosoma/holosoma/agents/callbacks/grid_eval_payload.py b/src/holosoma/holosoma/agents/callbacks/grid_eval_payload.py new file mode 100644 index 000000000..ddac33f3d --- /dev/null +++ b/src/holosoma/holosoma/agents/callbacks/grid_eval_payload.py @@ -0,0 +1,206 @@ +"""Grid evaluation payload callback for multi-condition payload sweeps. + +Applies constant downward forces (simulating carried payload) during evaluation. Sweeps across +body-group placement (where the payload sits) and mass. Forces are injected every physics substep +by monkey-patching ``env._apply_force_in_physics_step``. Records payload body positions to the NPZ. + +Usage example (4 vel x 2 body_groups x 3 masses = 24 conditions): + + --grid-velocity.config.enabled + --grid-velocity.config.lin-vel-x 0.25 0.5 0.75 1.0 + --grid-payload.config.enabled + --grid-payload.config.body-groups pelvis left_wrist_yaw_link,right_wrist_yaw_link + --grid-payload.config.body-labels torso wrists + --grid-payload.config.mass-kg 0.0 1.0 2.0 + --training.num-envs 24 +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +from loguru import logger + +from holosoma.agents.callbacks.base_callback import RLEvalCallback +from holosoma.agents.callbacks.sim_utils import apply_body_force_world, resolve_body +from holosoma.config_types.eval_callback import GridEvalPayloadConfig +from holosoma.utils.safe_torch_import import torch + +GRAVITY = 9.81 + + +class GridEvalPayloadCallback(RLEvalCallback): + """Sweeps payload across body-group x mass dimensions in grid evaluation. + + Registers axes with the condition manager (via recording callback), + then applies per-env forces at each physics substep. + """ + + def __init__(self, config: GridEvalPayloadConfig, training_loop: Any = None): + super().__init__(config, training_loop) + + self._recording_cb: Any = None + self._sim: Any = None + self._original_apply_force: Any = None + + self._resolved_groups: list[tuple[str, list[tuple[str, int]]]] = [] + + # Per-env assignment (set in _deferred_setup): + # _payload_forces_per_env[i] = force per body (N) for env i + # _payload_group_idx_per_env[i] = which body-group env i uses + self._payload_forces_per_env: torch.Tensor | None = None + self._payload_group_idx_per_env: list[int] = [] + + # Same data reorganized by group for the substep hot path: + # _group_env_indices[g] = env indices with non-zero payload in group g + # _group_force_world[g] = [N, 3] world-frame force vectors for those envs + self._group_env_indices: list[torch.Tensor] = [] + self._group_force_world: list[torch.Tensor] = [] + + def on_pre_evaluate_policy(self) -> None: + self._recording_cb = self._require_recording_cb() + + env = self._get_env() + self._sim = env.simulator + + if self._sim.simulator_config.name != "isaacsim": + raise RuntimeError(f"GridEvalPayloadCallback requires IsaacSim, got '{self._sim.simulator_config.name}'") + + body_groups = self.config.body_groups + body_labels = self.config.body_labels + + # --- Resolve comma-separated body name strings to (name, isaac_body_id) pairs --- + self._resolved_groups = [ + ( + body_labels[i], + [resolve_body(self._sim, n.strip()) for n in bg_str.split(",") if n.strip()], + ) + for i, bg_str in enumerate(body_groups) + ] + + logger.info( + f"GridEvalPayloadCallback: {len(self._resolved_groups)} body groups, masses={list(self.config.mass_kg)}kg" + ) + for label, bodies in self._resolved_groups: + logger.info(f" group '{label}': bodies={[n for n, _ in bodies]}") + + # --- Register sweep axes: body_group x mass_kg --- + cm = self._recording_cb.condition_manager + cm.add_axis("payload_body_label", list(body_labels), labels=list(body_labels), group="payload") + cm.add_axis( + "payload_mass_kg", + list(self.config.mass_kg), + labels=[f"{m}kg" for m in self.config.mass_kg], + group="payload", + ) + + self._recording_cb.register_buffer_key("payload_body_pos_w") + + self._recording_cb.set_metadata( + "payload_body_groups", {label: [n for n, _ in bodies] for label, bodies in self._resolved_groups} + ) + self._recording_cb.set_metadata("payload_mass_kg_values", list(self.config.mass_kg)) + self._recording_cb.set_metadata("payload_body_labels", list(body_labels)) + + def _deferred_setup(self) -> None: + """Set up per-env forces after condition manager is finalized.""" + self._recording_cb.ensure_finalized() + + env = self._get_env() + cm = self._recording_cb.condition_manager + conditions = cm.conditions + num_c = cm.num_conditions + device = env.device + + # --- Map each env to its body-group and compute per-body force --- + label_to_group_idx = {label: i for i, (label, _) in enumerate(self._resolved_groups)} + self._payload_forces_per_env = torch.zeros(num_c, dtype=torch.float32, device=device) + self._payload_group_idx_per_env = [] + + for i, cond in enumerate(conditions): + mass_kg = cond.get("payload_mass_kg", 0.0) + body_label = cond.get("payload_body_label", "") + group_idx = label_to_group_idx.get(body_label, 0) + self._payload_group_idx_per_env.append(group_idx) + + if mass_kg > 0: + _label, bodies = self._resolved_groups[group_idx] + self._payload_forces_per_env[i] = (mass_kg * GRAVITY) / len(bodies) + + # --- Reorganize by group for substep hot path --- + group_idx_tensor = torch.tensor(self._payload_group_idx_per_env, device=device) + self._group_env_indices = [] + self._group_force_world = [] + + for group_idx, (_label, _bodies) in enumerate(self._resolved_groups): + mask = (group_idx_tensor == group_idx) & (self._payload_forces_per_env > 0) + if mask.any(): + indices = torch.where(mask)[0] + force_world = torch.zeros(len(indices), 3, dtype=torch.float32, device=device) + force_world[:, 2] = -self._payload_forces_per_env[indices] + else: + indices = torch.empty(0, dtype=torch.long, device=device) + force_world = torch.empty(0, 3, dtype=torch.float32, device=device) + self._group_env_indices.append(indices) + self._group_force_world.append(force_world) + + self._recording_cb.set_metadata( + "payload_mass_kg_per_condition", [c.get("payload_mass_kg", 0.0) for c in conditions] + ) + self._recording_cb.set_metadata( + "payload_body_label_per_condition", [c.get("payload_body_label", "") for c in conditions] + ) + + # --- Monkey-patch physics step to inject payload forces every substep --- + self._original_apply_force = env._apply_force_in_physics_step + + def _patched_apply_force(): + self._original_apply_force() + self._apply_payload_forces() + + env._apply_force_in_physics_step = _patched_apply_force + + non_zero = (self._payload_forces_per_env > 0).sum().item() + logger.info(f"GridEvalPayloadCallback: deferred setup, {num_c} conditions, non-zero payload on {non_zero} envs") + + def on_post_eval_env_step(self, actor_state: dict) -> dict: + if self._payload_forces_per_env is None: + self._deferred_setup() + + num_c = self._recording_cb.num_conditions + + # --- Record world positions of each group's bodies for each env --- + max_bodies = max(len(bodies) for _, bodies in self._resolved_groups) + body_positions: np.ndarray = np.zeros((num_c, max_bodies, 3), dtype=np.float32) + + for group_idx, (_label, bodies) in enumerate(self._resolved_groups): + env_mask = np.array(self._payload_group_idx_per_env) == group_idx + env_indices = np.where(env_mask)[0] + if len(env_indices) == 0: + continue + for b_idx, (_name, isaac_body_id) in enumerate(bodies): + pos = self._sim._robot.data.body_pos_w[env_indices, isaac_body_id] + body_positions[env_indices, b_idx] = pos.detach().cpu().numpy() + + self._recording_cb.append_buffer("payload_body_pos_w", body_positions) + return actor_state + + def on_post_evaluate_policy(self) -> None: + if self._original_apply_force is not None: + self._get_env()._apply_force_in_physics_step = self._original_apply_force + logger.info("GridEvalPayloadCallback: completed") + + def _apply_payload_forces(self) -> None: + for group_idx, (_label, bodies) in enumerate(self._resolved_groups): + env_indices = self._group_env_indices[group_idx] + if len(env_indices) == 0: + continue + force_world = self._group_force_world[group_idx] + for _name, isaac_body_id in bodies: + apply_body_force_world( + self._sim, + env_indices, + isaac_body_id, + force_world, + ) diff --git a/src/holosoma/holosoma/agents/callbacks/grid_eval_push.py b/src/holosoma/holosoma/agents/callbacks/grid_eval_push.py new file mode 100644 index 000000000..023e0a2fa --- /dev/null +++ b/src/holosoma/holosoma/agents/callbacks/grid_eval_push.py @@ -0,0 +1,448 @@ +"""Grid evaluation push callback for deterministic push perturbation sweeps. + +Sweeps push perturbations across body x direction x gait_phase (x force_magnitude), +cross-producted with velocity conditions from GridEvalVelocityCallback. Each condition +env receives exactly one push. Forces are injected every physics substep by +monkey-patching ``env._apply_force_in_physics_step``. + +Push lifecycle: + 1. **Assign** (_deferred_setup): map each env to its push params (body, direction, + phase, force). No-gait-phase path computes a random fire step per env. + 2. **Activate** (_try_activate_pushes): when the target gait phase is detected (or + the random fire step is reached), compute world-frame forces and mark env as active. + 3. **Apply force** (_apply_push_forces): called every physics substep via monkey-patch. + Groups active envs by body and applies forces via IsaacSim API. + 4. **Expire** (on_post_eval_env_step): decrement timers, clear forces when done. + 5. **Record** (on_post_eval_env_step): append push_active and push_force_w each step. + +Push timing: + With gait_phases: pushes fire when the target phase is detected by GaitAnalyser + after a warmup + gait analysis window. + Without gait_phases: pushes fire at a random step in + [warmup + gait_analysis_s, + 1s] to ensure stable walking. + +Push directions are in the robot's local frame, rotated by yaw at push time. +Direction vectors are defined in GridEvalPushConfig.DIRECTION_VECTORS. + +Usage example (4 vel x 2 bodies x 4 dirs x 4 phases = 128 conditions): + + --grid-velocity.config.enabled + --grid-velocity.config.lin-vel-x 0.5 + --grid-push.config.enabled + --grid-push.config.body-names torso_link pelvis + --grid-push.config.body-labels torso pelvis + --grid-push.config.directions forward backward left right + --grid-push.config.gait-phases swing_to_stance stance_to_swing mid_stance mid_swing + --grid-push.config.force-n 150.0 + --grid-push.config.duration-s 0.2 + --training.num-envs 128 +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +from loguru import logger + +from holosoma.agents.callbacks.base_callback import RLEvalCallback +from holosoma.agents.callbacks.gait_analysis import GaitAnalyser +from holosoma.agents.callbacks.sim_utils import apply_body_force_world, clear_body_force, resolve_body +from holosoma.config_types.eval_callback import GridEvalPushConfig +from holosoma.utils.safe_torch_import import torch + + +class GridEvalPushCallback(RLEvalCallback): + """Sweeps push perturbations across body x direction x gait_phase. + + Registers axes with the condition manager (via recording callback). + When gait_phases are configured, validates that the env is a + locomotion manager (the robot must be walking). + """ + + def __init__(self, config: GridEvalPushConfig, training_loop: Any = None): + super().__init__(config, training_loop) + + # External refs + self._recording_cb: Any = None + self._sim: Any = None + self._original_apply_force: Any = None + + self._resolved_bodies: list[tuple[str, int]] = [] + + # Gait analysis timing + self._gait_analyser: GaitAnalyser | None = None + self._gait_analysis_min_steps: int = 0 + self._gait_analysis_max_steps: int = 0 + + # Per-env push assignment (set in _deferred_setup) + self._push_body_idx_per_env: list[int] = [] + self._push_local_dir_per_env: torch.Tensor | None = None + self._push_gait_phase_per_env: list[str] = [] + self._push_force_n_per_env: torch.Tensor | None = None + + # Push execution state + self._push_active: torch.Tensor | None = None + self._push_force_w: torch.Tensor | None = None + self._push_steps_remaining: torch.Tensor | None = None + self._push_fired: torch.Tensor | None = None + self._push_fire_step: torch.Tensor | None = None + + self._step_count = 0 + self._setup_done = False + + def on_pre_evaluate_policy(self) -> None: + # --- Validate prerequisites --- + self._recording_cb = self._require_recording_cb() + + env = self._get_env() + self._sim = env.simulator + + if self.config.gait_phases: + from holosoma.envs.locomotion.locomotion_manager import LeggedRobotLocomotionManager + + if not isinstance(env, LeggedRobotLocomotionManager): + raise RuntimeError( + "GridEvalPushCallback with gait_phases requires a locomotion policy " + f"(LeggedRobotLocomotionManager), got {type(env).__name__}" + ) + + if self._sim.simulator_config.name != "isaacsim": + raise RuntimeError(f"GridEvalPushCallback requires IsaacSim, got '{self._sim.simulator_config.name}'") + + # --- Resolve body names to IsaacSim body IDs --- + body_names = self.config.body_names + body_labels = self.config.body_labels + + self._resolved_bodies = [resolve_body(self._sim, n) for n in body_names] + + left_foot_isaac_id = resolve_body(self._sim, self.config.left_foot_body)[1] + right_foot_isaac_id = resolve_body(self._sim, self.config.right_foot_body)[1] + + # --- Determine force magnitudes --- + if self.config.force_magnitudes: + self._force_magnitudes = list(self.config.force_magnitudes) + else: + self._force_magnitudes = [self.config.force_n] + + logger.info( + f"GridEvalPushCallback: {len(self._resolved_bodies)} bodies, " + f"directions={list(self.config.directions)}, " + f"gait_phases={list(self.config.gait_phases)}, " + f"forces={self._force_magnitudes}N, duration={self.config.duration_s}s, " + f"gait_analysis={self.config.gait_analysis_s}s" + ) + + # --- Register sweep axes with condition manager --- + cm = self._recording_cb.condition_manager + cm.add_axis( + name="push_body_label", + values=list(body_labels), + labels=list(body_labels), + group="push", + ) + cm.add_axis( + name="push_direction", + values=list(self.config.directions), + labels=list(self.config.directions), + group="push", + ) + if self.config.gait_phases: + cm.add_axis( + name="push_gait_phase", + values=list(self.config.gait_phases), + labels=list(self.config.gait_phases), + group="push", + ) + cm.add_axis( + name="push_force_n", + values=self._force_magnitudes, + labels=[f"{f:.0f}N" for f in self._force_magnitudes], + group="push", + ) + + # --- Register recording buffers and metadata --- + self._recording_cb.register_buffer_key("push_active") + self._recording_cb.register_buffer_key("push_force_w") + + self._recording_cb.set_metadata("push_body_labels", list(body_labels)) + self._recording_cb.set_metadata("push_body_names", [n for n, _ in self._resolved_bodies]) + self._recording_cb.set_metadata("push_directions", list(self.config.directions)) + self._recording_cb.set_metadata("push_gait_phases", list(self.config.gait_phases)) + self._recording_cb.set_metadata("push_force_magnitudes", self._force_magnitudes) + self._recording_cb.set_metadata("push_duration_s", self.config.duration_s) + self._recording_cb.set_metadata("push_gait_analysis_s", self.config.gait_analysis_s) + self._recording_cb.set_metadata("push_left_foot_body", self.config.left_foot_body) + self._recording_cb.set_metadata("push_right_foot_body", self.config.right_foot_body) + + # --- Create GaitAnalyser (only for gait-phase path) --- + if self.config.gait_phases: + self._gait_analyser = GaitAnalyser( + sim=self._sim, + left_foot_isaac_id=left_foot_isaac_id, + right_foot_isaac_id=right_foot_isaac_id, + num_conditions=0, # set in _deferred_setup after finalization + foot_contact_threshold=self.config.foot_contact_threshold, + min_gait_cycles=self.config.min_gait_cycles, + ) + + def _deferred_setup(self) -> None: + self._recording_cb.ensure_finalized() + + env = self._get_env() + cm = self._recording_cb.condition_manager + conditions = cm.conditions + num_c = cm.num_conditions + device = env.device + dt = float(env.dt) + + # --- Gait analysis timing --- + warmup_steps = cm.warmup_steps + self._gait_analysis_min_steps = warmup_steps + int(self.config.gait_analysis_s / dt) + self._gait_analysis_max_steps = warmup_steps + int(self.config.max_gait_analysis_s / dt) + + if self._gait_analyser is not None: + self._gait_analyser._num_conditions = num_c + self._gait_analyser._prev_left_on_ground = np.zeros(num_c, dtype=bool) + + # --- Map each env to its push params from conditions --- + body_labels = self.config.body_labels + label_to_body_idx = {label: i for i, label in enumerate(body_labels)} + + push_duration_steps = max(1, round(self.config.duration_s / dt)) + + self._push_body_idx_per_env = [] + self._push_gait_phase_per_env = [] + local_dirs = [] + force_ns = [] + has_gait_phases = bool(self.config.gait_phases) + + for cond in conditions: + body_label = cond.get("push_body_label", "") + direction = cond.get("push_direction", "forward") + force = float(cond.get("push_force_n", self.config.force_n)) + + self._push_body_idx_per_env.append(label_to_body_idx.get(body_label, 0)) + local_dirs.append(self.config.DIRECTION_VECTORS[direction]) + if has_gait_phases: + self._push_gait_phase_per_env.append(cond.get("push_gait_phase", "swing_to_stance")) + force_ns.append(force) + + self._push_local_dir_per_env = torch.tensor(local_dirs, dtype=torch.float32, device=device) + self._push_force_n_per_env = torch.tensor(force_ns, dtype=torch.float32, device=device) + + # --- Initialize push execution state --- + self._push_active = torch.zeros(num_c, dtype=torch.bool, device=device) + self._push_force_w = torch.zeros(num_c, 3, dtype=torch.float32, device=device) + self._push_steps_remaining = torch.zeros(num_c, dtype=torch.long, device=device) + self._push_fired = torch.zeros(num_c, dtype=torch.bool, device=device) + self._push_duration_steps = push_duration_steps + + if not self.config.gait_phases: + # No gait phases — assign each env a random fire step after the + # analysis window so walking is stable (warmup + gait_analysis + 0-1s) + delay_steps = int(1.0 / dt) + earliest = self._gait_analysis_min_steps + self._push_fire_step = torch.randint(earliest, earliest + delay_steps + 1, (num_c,), device=device) + + # --- Per-condition metadata --- + self._recording_cb.set_metadata( + "push_body_label_per_condition", [c.get("push_body_label", "") for c in conditions] + ) + self._recording_cb.set_metadata( + "push_direction_per_condition", [c.get("push_direction", "") for c in conditions] + ) + self._recording_cb.set_metadata( + "push_gait_phase_per_condition", [c.get("push_gait_phase", "") for c in conditions] + ) + self._recording_cb.set_metadata("push_force_n_per_condition", force_ns) + self._recording_cb.set_metadata("push_duration_steps", push_duration_steps) + + # --- Monkey-patch physics step to inject push forces every substep --- + self._original_apply_force = env._apply_force_in_physics_step + + def _patched_apply_force(): + self._original_apply_force() + self._apply_push_forces() + + env._apply_force_in_physics_step = _patched_apply_force + + self._setup_done = True + logger.info( + f"GridEvalPushCallback: deferred setup, {num_c} conditions, " + f"push_duration={push_duration_steps} steps, " + f"gait_analysis min step {self._gait_analysis_min_steps}, " + f"max step {self._gait_analysis_max_steps}, " + f"min_cycles={self.config.min_gait_cycles}" + ) + + def _compute_world_forces_batch(self, env_indices: np.ndarray) -> torch.Tensor: + """Rotate local-frame push directions by each robot's yaw, scale by force magnitude.""" + assert self._push_local_dir_per_env is not None + assert self._push_force_n_per_env is not None + device = self._sim.sim_device + idx = torch.tensor(env_indices, dtype=torch.long, device=device) + + local_dirs = self._push_local_dir_per_env[idx] + + # Extract yaw from root quaternion (xyzw) + root_quats = self._sim.robot_root_states[idx, 3:7] + x, y, z, w = root_quats[:, 0], root_quats[:, 1], root_quats[:, 2], root_quats[:, 3] + siny_cosp = 2.0 * (w * z + x * y) + cosy_cosp = 1.0 - 2.0 * (y * y + z * z) + yaw = torch.atan2(siny_cosp, cosy_cosp) + + # 2D rotation: local frame -> world frame + cos_yaw = torch.cos(yaw) + sin_yaw = torch.sin(yaw) + + world_x = local_dirs[:, 0] * cos_yaw - local_dirs[:, 1] * sin_yaw + world_y = local_dirs[:, 0] * sin_yaw + local_dirs[:, 1] * cos_yaw + + force_dirs = torch.stack([world_x, world_y, local_dirs[:, 2]], dim=1) + return force_dirs * self._push_force_n_per_env[idx].unsqueeze(1) + + def _try_activate_pushes(self) -> None: + assert self._push_fired is not None + assert self._push_active is not None + assert self._push_steps_remaining is not None + assert self._push_force_w is not None + num_c = self._recording_cb.num_conditions + + # Find candidate envs ready to fire + if self.config.gait_phases: + assert self._gait_analyser is not None + phases = self._gait_analyser.detect_phases() + candidates = [ + i + for i in range(num_c) + if not self._push_fired[i] + and not self._push_active[i] + and phases[i] != "" + and phases[i] == self._push_gait_phase_per_env[i] + ] + else: + assert self._push_fire_step is not None + candidates = [ + i + for i in range(num_c) + if not self._push_fired[i] and not self._push_active[i] and self._step_count >= self._push_fire_step[i] + ] + + if not candidates: + return + + # Compute world-frame forces and activate + env_indices = np.array(candidates) + forces = self._compute_world_forces_batch(env_indices) + + idx_tensor = torch.tensor(env_indices, device=self.device, dtype=torch.long) + self._push_fired[idx_tensor] = True + self._push_active[idx_tensor] = True + self._push_steps_remaining[idx_tensor] = self._push_duration_steps + self._push_force_w[idx_tensor] = forces + + for i in candidates: + body_name = self._resolved_bodies[self._push_body_idx_per_env[i]][0] + logger.debug(f"GridEvalPushCallback: env {i} push at step {self._step_count} (body={body_name})") + + def on_post_eval_env_step(self, actor_state: dict) -> dict: + if not self._setup_done: + self._deferred_setup() + + cm = self._recording_cb.condition_manager + num_c = cm.num_conditions + + # Gait analysis: record foot heights, try to finalize + if self._gait_analyser is not None and not self._gait_analyser.done: + if self._step_count >= cm.warmup_steps: + self._gait_analyser.record_foot_heights() + if self._step_count >= self._gait_analysis_min_steps: + force = self._step_count >= self._gait_analysis_max_steps + if self._gait_analyser.try_finalize(force=force): + for key, value in self._gait_analyser.get_metadata().items(): + self._recording_cb.set_metadata(key, value) + + # Activate pushes (gait-phase path waits for analyser.done, no-gait-phase uses fire_step) + can_activate = self._gait_analyser is None or self._gait_analyser.done + if can_activate: + self._try_activate_pushes() + + # Decrement push timers, expire finished pushes + assert self._push_active is not None + assert self._push_steps_remaining is not None + assert self._push_force_w is not None + assert self._push_fired is not None + active_mask = self._push_active[:num_c] + self._push_steps_remaining[:num_c][active_mask] -= 1 + + expired = active_mask & (self._push_steps_remaining[:num_c] <= 0) + if expired.any(): + self._push_active[:num_c][expired] = False + self._push_force_w[:num_c][expired] = 0.0 + self._clear_push_forces(torch.where(expired)[0]) + + # Record push state + self._recording_cb.append_buffer( + "push_active", + self._push_active[:num_c].float().cpu().numpy().copy(), + ) + self._recording_cb.append_buffer( + "push_force_w", + self._push_force_w[:num_c].cpu().numpy().copy(), + ) + + self._step_count += 1 + return actor_state + + def on_post_evaluate_policy(self) -> None: + # Restore original physics step + if self._original_apply_force is not None: + self._get_env()._apply_force_in_physics_step = self._original_apply_force + + # Record which conditions actually fired + assert self._push_fired is not None + num_c = self._recording_cb.num_conditions + self._recording_cb.set_metadata( + "push_fired_per_condition", + self._push_fired[:num_c].cpu().tolist(), + ) + + num_fired = int(self._push_fired[:num_c].sum().item()) + logger.info(f"GridEvalPushCallback: completed, {num_fired}/{num_c} pushes fired") + + def _apply_push_forces(self) -> None: + """Substep hot path: apply forces for all active pushes.""" + if self._push_active is None: + return + assert self._push_force_w is not None + + num_c = self._recording_cb.num_conditions + active_mask = self._push_active[:num_c] + if not active_mask.any(): + return + + # Group active envs by body for batched IsaacSim API calls + device = self._sim.sim_device + active_indices = torch.where(active_mask)[0] + + body_groups: dict[int, list[int]] = {} + for env_idx in active_indices.tolist(): + body_groups.setdefault(self._push_body_idx_per_env[env_idx], []).append(env_idx) + + for body_idx, env_idxs in body_groups.items(): + _name, isaac_body_id = self._resolved_bodies[body_idx] + env_ids = torch.tensor(env_idxs, dtype=torch.long, device=device) + apply_body_force_world(self._sim, env_ids, isaac_body_id, self._push_force_w[env_ids]) + + def _clear_push_forces(self, env_indices: torch.Tensor) -> None: + """Zero out forces on expired envs, grouped by body.""" + device = self._sim.sim_device + body_groups: dict[int, list[int]] = {} + for env_idx in env_indices.tolist(): + body_groups.setdefault(self._push_body_idx_per_env[env_idx], []).append(env_idx) + + for body_idx, env_idxs in body_groups.items(): + _name, isaac_body_id = self._resolved_bodies[body_idx] + env_ids = torch.tensor(env_idxs, dtype=torch.long, device=device) + clear_body_force(self._sim, env_ids, isaac_body_id) diff --git a/src/holosoma/holosoma/agents/callbacks/grid_eval_velocity.py b/src/holosoma/holosoma/agents/callbacks/grid_eval_velocity.py new file mode 100644 index 000000000..4cc0efd2d --- /dev/null +++ b/src/holosoma/holosoma/agents/callbacks/grid_eval_velocity.py @@ -0,0 +1,138 @@ +"""Grid evaluation velocity callback for locomotion policies. + +Registers velocity sweep axes with the GridConditionManager and +overrides ``env.command_manager.commands`` each step so each condition env +walks at its assigned velocity. + +Each non-trivial axis is swept independently (other axes default to 0). +E.g. lin_vel_x=[0.25, 0.5, 0.75, 1.0], ang_vel_yaw=[0.3, 0.5, 0.7] +produces 4 + 3 = 7 conditions (not 12). + +Warmup: the first ``warmup_s`` seconds use zero velocity on all envs. + +Usage example (4 velocity conditions): + + --grid-velocity.config.enabled + --grid-velocity.config.lin-vel-x 0.25 0.5 0.75 1.0 + --training.num-envs 4 +""" + +from __future__ import annotations + +from typing import Any + +from loguru import logger + +from holosoma.agents.callbacks.base_callback import RLEvalCallback +from holosoma.config_types.eval_callback import GridEvalVelocityConfig +from holosoma.utils.safe_torch_import import torch + + +class GridEvalVelocityCallback(RLEvalCallback): + """Assigns per-env velocity commands from a grid. + + Only handles velocity command injection. Recording is done by + EvalRecordingCallback. + """ + + def __init__(self, config: GridEvalVelocityConfig, training_loop: Any = None): + super().__init__(config, training_loop) + + self._command_tensor: torch.Tensor | None = None + self._zero_command_tensor: torch.Tensor | None = None + self._num_conditions: int = 0 + self._warmup_steps: int = 0 + self._step_count: int = 0 + self._recording_cb: Any = None + + def on_pre_evaluate_policy(self) -> None: + self._recording_cb = self._require_recording_cb() + + conditions = self._build_velocity_conditions(self.config) + cm = self._recording_cb.condition_manager + + cm.add_axis( + ["lin_vel_x", "lin_vel_y", "ang_vel_yaw"], + [(c["lin_vel_x"], c["lin_vel_y"], c["ang_vel_yaw"]) for c in conditions], + group="velocity", + ) + + logger.info(f"GridEvalVelocityCallback: {len(conditions)} velocity conditions") + + def _deferred_setup(self) -> None: + """Build command tensors after condition manager is finalized.""" + self._recording_cb.ensure_finalized() + + env = self._get_env() + cm = self._recording_cb.condition_manager + conditions = cm.conditions + self._num_conditions = cm.num_conditions + + self._command_tensor = self._build_command_tensor(conditions, env.device) + self._zero_command_tensor = torch.zeros_like(self._command_tensor) + + dt = float(env.dt) + self._warmup_steps = int(self.config.warmup_s / dt) if self.config.warmup_s > 0 else 0 + cm.warmup_steps = self._warmup_steps + + self._recording_cb.set_metadata("warmup_s", self.config.warmup_s) + self._recording_cb.set_metadata("warmup_steps", self._warmup_steps) + + logger.info( + f"GridEvalVelocityCallback: {self._num_conditions} total conditions, warmup={self._warmup_steps} steps" + ) + + def on_pre_eval_env_step(self, actor_state: dict) -> dict: + if self._step_count == 0: + self._deferred_setup() + + env = self._get_env() + commands = env.command_manager.commands + num_c = self._num_conditions + + if self._step_count < self._warmup_steps: + commands[:num_c] = self._zero_command_tensor + else: + commands[:num_c] = self._command_tensor + + self._step_count += 1 + return actor_state + + @staticmethod + def _build_velocity_conditions(config: GridEvalVelocityConfig) -> list[dict[str, float]]: + """Build independent velocity conditions from config. + + Each non-trivial axis is swept separately; other axes default to 0. + Deduplication is handled by GridConditionManager.finalize(). + """ + zeros = {"lin_vel_x": 0.0, "lin_vel_y": 0.0, "ang_vel_yaw": 0.0} + axes = { + "lin_vel_x": list(config.lin_vel_x), + "lin_vel_y": list(config.lin_vel_y), + "ang_vel_yaw": list(config.ang_vel_yaw), + } + + conditions: list[dict[str, float]] = [] + for axis_name, values in axes.items(): + if values == [0.0]: + continue + conditions.extend({**zeros, axis_name: val} for val in values) + return conditions or [zeros.copy()] + + def _build_command_tensor( + self, + conditions: list[dict[str, Any]], + device: torch.device, + ) -> torch.Tensor: + """Convert conditions to [num_conditions, 3] command tensor. + + Extracts velocity keys from conditions which may also contain + non-velocity keys from other sweep dimensions. + """ + num_c = len(conditions) + commands = torch.zeros(num_c, 3, dtype=torch.float32, device=device) + for i, cond in enumerate(conditions): + commands[i, 0] = float(cond.get("lin_vel_x", 0.0)) + commands[i, 1] = float(cond.get("lin_vel_y", 0.0)) + commands[i, 2] = float(cond.get("ang_vel_yaw", 0.0)) + return commands diff --git a/src/holosoma/holosoma/agents/callbacks/payload.py b/src/holosoma/holosoma/agents/callbacks/payload.py deleted file mode 100644 index 1a50af66c..000000000 --- a/src/holosoma/holosoma/agents/callbacks/payload.py +++ /dev/null @@ -1,172 +0,0 @@ -"""Eval callback that applies constant downward forces on wrists to simulate a held payload. - -Applies F = mass * g / num_bodies on each resolved wrist/elbow link -as a sustained downward (-Z) force throughout the evaluation episode. Uses the same -IsaacLab set_external_force_and_torque API as the push callback, with substep-level -force application via monkey-patching. -""" - -from __future__ import annotations - -from typing import Any - -import numpy as np -from loguru import logger - -from holosoma.agents.callbacks.base_callback import RLEvalCallback -from holosoma.config_types.eval_callback import PayloadConfig -from holosoma.utils.safe_torch_import import torch - -GRAVITY = 9.81 - - -class EvalPayloadCallback(RLEvalCallback): - """Applies constant downward forces on wrist links to simulate holding a payload. - - The total payload weight (mass * g) is split evenly across all resolved - wrist bodies. Forces are applied at every physics substep via monkey-patch. - - Recorded arrays (per env step): - payload_force_per_body [T] force magnitude per body in N - payload_body_pos_w [T, N, 3] world positions of payload bodies - """ - - def __init__( - self, - config: PayloadConfig, - training_loop: Any = None, - ): - super().__init__(config, training_loop) - self.env_id = config.env_id - self.candidate_body_names = [s.strip() for s in config.body_names.split(",")] - self._record_buffers: dict[str, list[np.ndarray]] | None = None - self._record_metadata: dict[str, Any] | None = None - - # Resolved at on_pre_evaluate_policy - self._resolved_bodies: list[tuple[str, int, int]] = [] - self._force_per_body: float = 0.0 - self._sim: Any = None - self._original_apply_force: Any = None - self._step_count: int = 0 - - def _get_env(self): - return self.training_loop._unwrap_env() - - def _find_recording_callback(self): - from holosoma.agents.callbacks.recording import EvalRecordingCallback - - for cb in self.training_loop.eval_callbacks: - if isinstance(cb, EvalRecordingCallback): - return cb - return None - - def on_pre_evaluate_policy(self) -> None: - # Discover recording callback for shared buffer/metadata access - recording_cb = self._find_recording_callback() - if recording_cb is not None: - self._record_buffers = recording_cb._buffers - self._record_metadata = recording_cb._metadata - - env = self._get_env() - self._sim = env.simulator - if self._sim.simulator_config.name != "isaacsim": - raise RuntimeError(f"EvalPayloadCallback requires IsaacSim, got '{self._sim.simulator_config.name}'") - - # Resolve body names - self._resolved_bodies = [] - available = list(getattr(self._sim, "body_names", [])) - for name in self.candidate_body_names: - body_names_idx = self._sim.find_rigid_body_indice(name) - if isinstance(body_names_idx, int) and body_names_idx >= 0: - isaac_body_id = self._sim.body_ids[body_names_idx] - self._resolved_bodies.append((name, body_names_idx, isaac_body_id)) - else: - raise ValueError(f"EvalPayloadCallback: body '{name}' not found. Available: {available}") - - total_force = self.config.mass_kg * GRAVITY - self._force_per_body = total_force / len(self._resolved_bodies) - - resolved_names = [name for name, _, _ in self._resolved_bodies] - logger.info( - f"EvalPayloadCallback: {len(self._resolved_bodies)} payload bodies: {resolved_names}, " - f"mass={self.config.mass_kg}kg, total_force={total_force:.1f}N, " - f"force_per_body={self._force_per_body:.1f}N" - ) - - # Store config in recording metadata - if self._record_metadata is not None: - self._record_metadata["payload_config"] = { - "body_names": resolved_names, - "mass_kg": self.config.mass_kg, - "total_force_n": total_force, - "force_per_body_n": self._force_per_body, - } - - # Initialize recording buffers - buffers = self._get_buffers() - if buffers is not None: - buffers["payload_force_per_body"] = [] - buffers["payload_body_pos_w"] = [] - - # Monkey-patch substep force application - self._original_apply_force = env._apply_force_in_physics_step - - def _patched_apply_force(): - self._original_apply_force() - self._apply_payload_forces() - - env._apply_force_in_physics_step = _patched_apply_force - - def on_post_eval_env_step(self, actor_state: dict) -> dict: - buffers = self._get_buffers() - if buffers is not None: - eid = self.env_id - body_positions = [] - for _name, _body_names_idx, isaac_body_id in self._resolved_bodies: - pos = self._sim._robot.data.body_pos_w[eid, isaac_body_id].detach().cpu().numpy().copy() - body_positions.append(pos) - - buffers["payload_force_per_body"].append(np.array(self._force_per_body, dtype=np.float32)) - buffers["payload_body_pos_w"].append(np.stack(body_positions, axis=0).astype(np.float32)) - - self._step_count += 1 - return actor_state - - def on_post_evaluate_policy(self) -> None: - self._get_env()._apply_force_in_physics_step = self._original_apply_force - logger.info( - f"EvalPayloadCallback: completed {self._step_count} steps, " - f"payload={self.config.mass_kg}kg on {len(self._resolved_bodies)} bodies" - ) - - def _apply_payload_forces(self) -> None: - """Apply downward force on all resolved wrist bodies (called each substep).""" - from isaaclab.utils.math import quat_apply_inverse - - eid = self.env_id - device = self._sim.sim_device - - # Force in world frame: straight down (-Z) - force_world = torch.tensor( - [0.0, 0.0, -self._force_per_body], - dtype=torch.float32, - device=device, - ) - - for _name, _body_names_idx, isaac_body_id in self._resolved_bodies: - # Transform world-frame force to body-local frame - body_quat_w = self._sim._robot.data.body_quat_w[eid, isaac_body_id] - force_body = quat_apply_inverse(body_quat_w, force_world) - - forces = force_body.unsqueeze(0).unsqueeze(0) # [1, 1, 3] - torques = torch.zeros_like(forces) - - self._sim._robot.set_external_force_and_torque( - forces=forces, - torques=torques, - env_ids=torch.tensor([eid], device=device), - body_ids=torch.tensor([isaac_body_id], device=device), - ) - - def _get_buffers(self) -> dict[str, list[np.ndarray]] | None: - return self._record_buffers diff --git a/src/holosoma/holosoma/agents/callbacks/push.py b/src/holosoma/holosoma/agents/callbacks/push.py deleted file mode 100644 index 1913c3c59..000000000 --- a/src/holosoma/holosoma/agents/callbacks/push.py +++ /dev/null @@ -1,263 +0,0 @@ -"""Eval callback that applies random force pulses to test policy robustness. - -Periodically pushes the robot at random body locations with configurable -force magnitude, duration, and interval. Uses IsaacLab's -set_external_force_and_torque API to apply forces in the physics simulation. - -Push events are optionally recorded for visualization in viser by sharing -buffer/metadata dicts with EvalRecordingCallback. -""" - -from __future__ import annotations - -import math -import random -from typing import Any - -import numpy as np -from loguru import logger - -from holosoma.agents.callbacks.base_callback import RLEvalCallback -from holosoma.config_types.eval_callback import PushConfig -from holosoma.utils.safe_torch_import import torch - - -class EvalPushCallback(RLEvalCallback): - """Applies random force pulses during evaluation and optionally records them. - - Forces are applied at the physics substep level by patching the env's - _apply_force_in_physics_step method, ensuring the force is active for the - full duration across all substeps (control decimation). - - Recorded arrays (per env step): - push_active [T] 1.0 when pushing, 0.0 otherwise - push_force_w [T, 3] force vector in world frame (zeros when inactive) - push_body_pos_w[T, 3] world position of pushed body (zeros when inactive) - push_body_idx [T] index into body_names (-1 when inactive) - """ - - def __init__( - self, - config: PushConfig, - training_loop: Any = None, - ): - super().__init__(config, training_loop) - self.env_id = config.env_id - self.candidate_body_names = [s.strip() for s in config.body_names.split(",")] - self._record_buffers: dict[str, list[np.ndarray]] | None = None - self._record_metadata: dict[str, Any] | None = None - - # Resolved at on_pre_evaluate_policy - self._resolved_bodies: list[tuple[str, int, int]] = [] - self._dt: float = 0.0 - self._sim: Any = None - self._original_apply_force: Any = None - - # Push state machine - self._push_active: bool = False - self._push_steps_remaining: int = 0 - self._idle_steps_remaining: int = 0 - self._current_force_w: torch.Tensor | None = None - self._current_body_names_idx: int = -1 - self._current_isaac_body_id: int = -1 - - self._step_count: int = 0 - - def _get_env(self): - return self.training_loop._unwrap_env() - - def _find_recording_callback(self): - from holosoma.agents.callbacks.recording import EvalRecordingCallback - - for cb in self.training_loop.eval_callbacks: - if isinstance(cb, EvalRecordingCallback): - return cb - return None - - def on_pre_evaluate_policy(self) -> None: - # Discover recording callback for shared buffer/metadata access - recording_cb = self._find_recording_callback() - if recording_cb is not None: - self._record_buffers = recording_cb._buffers - self._record_metadata = recording_cb._metadata - - env = self._get_env() - self._sim = env.simulator - if self._sim.simulator_config.name != "isaacsim": - raise RuntimeError(f"EvalPushCallback requires IsaacSim, got '{self._sim.simulator_config.name}'") - self._dt = float(env.dt) - - # Resolve body names to simulator indices - self._resolved_bodies = [] - available = list(getattr(self._sim, "body_names", [])) - for name in self.candidate_body_names: - body_names_idx = self._sim.find_rigid_body_indice(name) - if isinstance(body_names_idx, int) and body_names_idx >= 0: - isaac_body_id = self._sim.body_ids[body_names_idx] - self._resolved_bodies.append((name, body_names_idx, isaac_body_id)) - else: - raise ValueError(f"EvalPushCallback: body '{name}' not found. Available: {available}") - - resolved_names = [name for name, _, _ in self._resolved_bodies] - logger.info( - f"EvalPushCallback: {len(self._resolved_bodies)} push bodies: {resolved_names}, " - f"force={self.config.force_range}N, duration={self.config.duration_s}s, interval={self.config.interval_s}s" - ) - - # Store push config in recording metadata - if self._record_metadata is not None: - self._record_metadata["push_config"] = { - "body_names": resolved_names, - "force_range": list(self.config.force_range), - "duration_s": list(self.config.duration_s), - "interval_s": list(self.config.interval_s), - } - - # Initialize recording buffers - buffers = self._get_buffers() - if buffers is not None: - for key in ("push_active", "push_force_w", "push_body_pos_w", "push_body_idx"): - buffers[key] = [] - - # Start with a random idle period - self._idle_steps_remaining = self._sample_interval_steps() - self._push_active = False - - # Patch _apply_force_in_physics_step to inject push forces at every substep - self._original_apply_force = env._apply_force_in_physics_step - - def _patched_apply_force(): - self._original_apply_force() - if self._push_active: - self._apply_push_force() - - env._apply_force_in_physics_step = _patched_apply_force - - def on_post_eval_env_step(self, actor_state: dict) -> dict: - # Manage push schedule - if self._push_active: - self._push_steps_remaining -= 1 - if self._push_steps_remaining <= 0: - self._deactivate_push() - self._idle_steps_remaining = self._sample_interval_steps() - else: - self._idle_steps_remaining -= 1 - if self._idle_steps_remaining <= 0: - self._activate_push() - - buffers = self._get_buffers() - eid = self.env_id - - if self._push_active: - assert self._current_force_w is not None - force_w = self._current_force_w.detach().cpu().numpy().copy() - body_pos = self._sim._robot.data.body_pos_w[eid, self._current_isaac_body_id].detach().cpu().numpy().copy() - body_idx = self._current_body_names_idx - active = 1.0 - else: - force_w = np.zeros(3, dtype=np.float32) - body_pos = np.zeros(3, dtype=np.float32) - body_idx = -1 - active = 0.0 - - if buffers is not None: - buffers["push_active"].append(np.array(active, dtype=np.float32)) - buffers["push_force_w"].append(force_w.astype(np.float32)) - buffers["push_body_pos_w"].append(body_pos.astype(np.float32)) - buffers["push_body_idx"].append(np.array(body_idx, dtype=np.int32)) - - self._step_count += 1 - return actor_state - - def on_post_evaluate_policy(self) -> None: - if self._push_active: - self._deactivate_push() - self._get_env()._apply_force_in_physics_step = self._original_apply_force - logger.info(f"EvalPushCallback: completed {self._step_count} steps") - - # ------------------------------------------------------------------ - # Push lifecycle - # ------------------------------------------------------------------ - - def _activate_push(self) -> None: - name, body_names_idx, isaac_body_id = random.choice(self._resolved_bodies) - self._current_body_names_idx = body_names_idx - self._current_isaac_body_id = isaac_body_id - - # Random force magnitude - mag = random.uniform(*self.config.force_range) - - # Random horizontal direction (XY plane) - angle = random.uniform(0, 2 * math.pi) - direction = torch.tensor( - [math.cos(angle), math.sin(angle), 0.0], - dtype=torch.float32, - device=self._sim.sim_device, - ) - self._current_force_w = direction * mag - - # Duration in env steps - dur_s = random.uniform(*self.config.duration_s) - self._push_steps_remaining = max(1, round(dur_s / self._dt)) - - self._push_active = True - logger.info( - f"PUSH START body='{name}' force={mag:.0f}N " - f"dir=[{direction[0].item():.2f},{direction[1].item():.2f}] " - f"duration={self._push_steps_remaining} steps ({dur_s:.2f}s)" - ) - - def _deactivate_push(self) -> None: - self._clear_push_force() - self._push_active = False - self._current_force_w = None - self._current_body_names_idx = -1 - self._current_isaac_body_id = -1 - logger.info("PUSH END") - - # ------------------------------------------------------------------ - # Force application (called each physics substep via monkey-patch) - # ------------------------------------------------------------------ - - def _apply_push_force(self) -> None: - from isaaclab.utils.math import quat_apply_inverse - - eid = self.env_id - isaac_body_id = self._current_isaac_body_id - - # Transform world-frame force to body-local frame - # (IsaacLab 2.1 hardcodes is_global=False) - body_quat_w = self._sim._robot.data.body_quat_w[eid, isaac_body_id] - force_body = quat_apply_inverse(body_quat_w, self._current_force_w) - - forces = force_body.unsqueeze(0).unsqueeze(0) # [1, 1, 3] - torques = torch.zeros_like(forces) # [1, 1, 3] - - self._sim._robot.set_external_force_and_torque( - forces=forces, - torques=torques, - env_ids=torch.tensor([eid], device=self._sim.sim_device), - body_ids=torch.tensor([isaac_body_id], device=self._sim.sim_device), - ) - - def _clear_push_force(self) -> None: - if self._current_isaac_body_id < 0: - return - zero = torch.zeros(1, 1, 3, device=self._sim.sim_device) - self._sim._robot.set_external_force_and_torque( - forces=zero, - torques=zero, - env_ids=torch.tensor([self.env_id], device=self._sim.sim_device), - body_ids=torch.tensor([self._current_isaac_body_id], device=self._sim.sim_device), - ) - - # ------------------------------------------------------------------ - # Helpers - # ------------------------------------------------------------------ - - def _sample_interval_steps(self) -> int: - s = random.uniform(*self.config.interval_s) - return max(1, round(s / self._dt)) - - def _get_buffers(self) -> dict[str, list[np.ndarray]] | None: - return self._record_buffers diff --git a/src/holosoma/holosoma/agents/callbacks/recording.py b/src/holosoma/holosoma/agents/callbacks/recording.py index ad8b6820b..5c35659fa 100644 --- a/src/holosoma/holosoma/agents/callbacks/recording.py +++ b/src/holosoma/holosoma/agents/callbacks/recording.py @@ -1,7 +1,15 @@ -"""Eval callback that records per-step trajectory data to an NPZ file. - -Records joint positions, velocities, torques, body poses, and root state -for later visualization with viser_eval_viewer.py. +"""Single recorder for all eval callback data. + +Records all condition envs to NPZ with shape [T, num_conditions, ...]. +When no sweep axes are registered, num_conditions defaults to 1 +(equivalent to old single-env mode). + +Other callbacks interact via the public API: + - ``condition_manager`` — register sweep axes + - ``ensure_finalized()`` — trigger deferred setup (idempotent) + - ``register_buffer_key(name)`` — reserve a recording channel + - ``append_buffer(name, data)`` — append one step of data + - ``set_metadata(key, value)`` — add metadata to the NPZ """ from __future__ import annotations @@ -14,12 +22,16 @@ from loguru import logger from holosoma.agents.callbacks.base_callback import RLEvalCallback +from holosoma.agents.callbacks.grid_conditions import GridConditionManager from holosoma.config_types.eval_callback import RecordingConfig from holosoma.utils.safe_torch_import import torch class EvalRecordingCallback(RLEvalCallback): - """Records per-step data during evaluation and saves to .npz on completion.""" + """Records per-step data during evaluation and saves to .npz on completion. + + Owns buffers, metadata, condition manager, and NPZ output. + """ def __init__( self, @@ -27,7 +39,6 @@ def __init__( training_loop: Any = None, ): super().__init__(config, training_loop) - self.env_id = config.env_id output_path = config.output_path if not output_path.endswith(".npz"): @@ -40,63 +51,41 @@ def __init__( self._metadata: dict[str, Any] = {} self._step_count = 0 - def _get_env(self): - """Get the unwrapped BaseTask environment.""" - return self.training_loop._unwrap_env() + self.condition_manager = GridConditionManager() + self._setup_done = False - def _save(self) -> None: - """Save recorded data to NPZ.""" - if self._step_count == 0: - return + self._original_check_termination: Any = None - arrays: dict[str, np.ndarray] = {} - for name, values in self._buffers.items(): - if values: - arrays[name] = np.stack(values, axis=0) + @property + def num_conditions(self) -> int: + return self.condition_manager.num_conditions - arrays["_metadata_json"] = np.array(json.dumps(self._metadata)) + @staticmethod + def _to_np(t: torch.Tensor) -> np.ndarray: + return t.detach().cpu().numpy().copy() - path = Path(self.output_path) - path.parent.mkdir(parents=True, exist_ok=True) - np.savez_compressed(str(path), **arrays) + def register_buffer_key(self, name: str) -> None: + """Reserve a recording channel.""" + if name not in self._buffers: + self._buffers[name] = [] - channel_summary = ", ".join( - f"{name}{list(arr.shape)}" for name, arr in arrays.items() if name != "_metadata_json" - ) - logger.info(f"EvalRecordingCallback: saved {self._step_count} steps to {path}\n Channels: {channel_summary}") + def append_buffer(self, name: str, data: np.ndarray) -> None: + """Append one step of data to a named channel.""" + self._buffers[name].append(data) + + def set_metadata(self, key: str, value: Any) -> None: + """Set a metadata entry in the NPZ.""" + self._metadata[key] = value def on_pre_evaluate_policy(self) -> None: env = self._get_env() - sim = env.simulator + self._metadata.update(self._collect_sim_metadata(env)) - self._metadata["dt"] = float(env.dt) - self._metadata["fps"] = round(1.0 / float(env.dt)) - self._metadata["sim_dt"] = float(env.sim_dt) - self._metadata["sim_fps"] = round(1.0 / float(env.sim_dt)) - self._metadata["control_decimation"] = env.simulator.simulator_config.sim.control_decimation - self._metadata["env_id"] = self.env_id - if hasattr(sim, "dof_names"): - self._metadata["dof_names"] = list(sim.dof_names) - if hasattr(sim, "body_names"): - self._metadata["body_names"] = list(sim.body_names) - - # Static robot properties - robot_cfg = env.robot_config - self._metadata["effort_limits"] = list(robot_cfg.dof_effort_limit_list) - self._metadata["dof_pos_lower_limits"] = list(robot_cfg.dof_pos_lower_limit_list) - self._metadata["dof_pos_upper_limits"] = list(robot_cfg.dof_pos_upper_limit_list) - self._metadata["velocity_limits"] = list(robot_cfg.dof_vel_limit_list) - asset_cfg = robot_cfg.asset - self._metadata["urdf_path"] = str(Path(asset_cfg.asset_root) / asset_cfg.urdf_file) - - channel_names = [ + for name in [ "dof_pos_target", "dof_pos", "dof_vel", "torques", - "torques_substep", - "dof_pos_substep", - "dof_vel_substep", "actions", "root_pos", "root_quat_xyzw", @@ -105,88 +94,146 @@ def on_pre_evaluate_policy(self) -> None: "body_pos_w", "body_quat_xyzw", "commanded_velocity", - ] - for name in channel_names: - self._buffers[name] = [] + ]: + self.register_buffer_key(name) + + logger.info(f"EvalRecordingCallback: output={self.output_path}") + + def ensure_finalized(self) -> None: + """Ensure the condition manager is finalized and recording is ready. + + Other callbacks must call this at the start of their own deferred + setup, before reading ``condition_manager.conditions``. Safe to + call multiple times — only the first call does work. + """ + if not self._setup_done: + self._deferred_setup() + + def _deferred_setup(self) -> None: + """Finalize grid conditions on the first env step. + + Called after all callbacks have registered their sweep axes. + When no axes are registered, defaults to 1 condition. + """ + env = self._get_env() + self.condition_manager.finalize(env.num_envs) + self._metadata.update(self.condition_manager.get_metadata()) - logger.info(f"EvalRecordingCallback: recording env_id={self.env_id}, output={self.output_path}") + # --- Suppress termination/resets so fallen robots stay down --- + self._original_check_termination = env._check_termination + env._check_termination = lambda: None + + for name in ["torques_substep", "dof_pos_substep", "dof_vel_substep"]: + self.register_buffer_key(name) + + self._setup_done = True + logger.info(f"EvalRecordingCallback: {self.condition_manager.num_conditions} conditions, disabled env resets") + + def on_pre_eval_env_step(self, actor_state: dict) -> dict: + self.ensure_finalized() + return actor_state def on_post_eval_env_step(self, actor_state: dict) -> dict: env = self._get_env() sim = env.simulator - eid = self.env_id + _to_np = self._to_np + num_c = self.condition_manager.num_conditions + s = slice(num_c) - def _to_np(t: torch.Tensor) -> np.ndarray: - return t.detach().cpu().numpy().copy() + self._buffers["dof_pos"].append(_to_np(sim.dof_pos[s])) + self._buffers["dof_vel"].append(_to_np(sim.dof_vel[s])) - self._buffers["dof_pos"].append(_to_np(sim.dof_pos[eid])) # post_eval_env_step, so after 4 decimation - self._buffers["dof_vel"].append(_to_np(sim.dof_vel[eid])) - self._buffers["torques"].append( - _to_np(self._extract_torques(env, eid)) - ) # pre_eval_env_step, so the torques is the last decimation + term = env.action_manager.get_term("joint_control") + self._buffers["torques"].append(_to_np(term.torques[s])) - # robot_root_states: [num_envs, 13] = pos(3), quat_xyzw(4), lin_vel(3), ang_vel(3) - root = sim.robot_root_states[eid] - self._buffers["root_pos"].append(_to_np(root[:3])) - self._buffers["root_quat_xyzw"].append(_to_np(root[3:7])) - self._buffers["root_lin_vel"].append(_to_np(root[7:10])) - self._buffers["root_ang_vel"].append(_to_np(root[10:13])) + root = sim.robot_root_states[s] + self._buffers["root_pos"].append(_to_np(root[:, :3])) + self._buffers["root_quat_xyzw"].append(_to_np(root[:, 3:7])) + self._buffers["root_lin_vel"].append(_to_np(root[:, 7:10])) + self._buffers["root_ang_vel"].append(_to_np(root[:, 10:13])) - self._buffers["body_pos_w"].append(_to_np(sim._rigid_body_pos[eid])) - self._buffers["body_quat_xyzw"].append(_to_np(sim._rigid_body_rot[eid])) + self._buffers["body_pos_w"].append(_to_np(sim._rigid_body_pos[s])) + self._buffers["body_quat_xyzw"].append(_to_np(sim._rigid_body_rot[s])) - # substep tensors: [decimation, num_dof] — one row per physics sub-step - torques_substep, dof_pos_substep, dof_vel_substep = self._extract_substep_data(env, eid) - self._buffers["torques_substep"].append(_to_np(torques_substep)) - self._buffers["dof_pos_substep"].append(_to_np(dof_pos_substep)) - self._buffers["dof_vel_substep"].append(_to_np(dof_vel_substep)) + self._buffers["torques_substep"].append(_to_np(term.torques_substep[s])) + self._buffers["dof_pos_substep"].append(_to_np(term.dof_pos_substep[s])) + self._buffers["dof_vel_substep"].append(_to_np(term.dof_vel_substep[s])) if "actions" in actor_state and actor_state["actions"] is not None: - self._buffers["actions"].append(_to_np(actor_state["actions"][eid])) + self._buffers["actions"].append(_to_np(actor_state["actions"][s])) - # Record desired target joint positions (PD setpoint) - self._buffers["dof_pos_target"].append(_to_np(self._extract_dof_pos_target(env, eid))) + self._buffers["dof_pos_target"].append( + _to_np(term._actions_after_delay[s] * term.action_scales + env.default_dof_pos[s]) + ) - # Record commanded velocity [lin_vel_x, lin_vel_y, ang_vel_yaw] if hasattr(env, "command_manager") and env.command_manager is not None: try: - self._buffers["commanded_velocity"].append(_to_np(env.command_manager.commands[eid])) + self._buffers["commanded_velocity"].append(_to_np(env.command_manager.commands[s])) except (AttributeError, IndexError): pass self._step_count += 1 return actor_state - def _extract_dof_pos_target(self, env: Any, env_id: int) -> torch.Tensor: - """Extract desired target joint positions from the action manager's joint control term. - - The PD target is: actions_after_delay * action_scales + default_dof_pos. - Returns shape [num_dof]. - """ - for _term_name, term in env.action_manager.iter_terms(): - if hasattr(term, "_actions_after_delay") and hasattr(term, "action_scales"): - return term._actions_after_delay[env_id] * term.action_scales + env.default_dof_pos[env_id] - raise RuntimeError("No action term with _actions_after_delay found") - - def _extract_torques(self, env: Any, env_id: int) -> torch.Tensor: - """Extract torques from the action manager's joint control term. - - Returns torques, shape [num_dof]. - """ - for _term_name, term in env.action_manager.iter_terms(): - if hasattr(term, "torques"): - return term.torques[env_id] - raise RuntimeError("No action term with torques found") - - def _extract_substep_data(self, env: Any, env_id: int) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - """Extract sub-step torques, dof_pos, and dof_vel from the action manager's joint control term. + def on_post_evaluate_policy(self) -> None: + if self._original_check_termination is not None: + env = self._get_env() + env._check_termination = self._original_check_termination + self._original_check_termination = None - Returns (torques_substep, dof_pos_substep, dof_vel_substep), each shape [decimation, num_dof]. - """ - for _term_name, term in env.action_manager.iter_terms(): - if hasattr(term, "torques_substep"): - return term.torques_substep[env_id], term.dof_pos_substep[env_id], term.dof_vel_substep[env_id] - raise RuntimeError("No action term with torques_substep found") + log_prefix = ( + f"EvalRecordingCallback: {self._step_count} steps x {self.condition_manager.num_conditions} conditions" + ) + self._save_npz( + self._buffers, + self._metadata, + self._step_count, + self.output_path, + log_prefix, + ) - def on_post_evaluate_policy(self) -> None: - self._save() + @staticmethod + def _collect_sim_metadata(env: Any) -> dict[str, Any]: + """Collect standard simulator metadata (timing, DOF names, limits, URDF path).""" + sim = env.simulator + meta: dict[str, Any] = { + "dt": float(env.dt), + "fps": round(1.0 / float(env.dt)), + "sim_dt": float(env.sim_dt), + "sim_fps": round(1.0 / float(env.sim_dt)), + "control_decimation": sim.simulator_config.sim.control_decimation, + } + if hasattr(sim, "dof_names"): + meta["dof_names"] = list(sim.dof_names) + if hasattr(sim, "body_names"): + meta["body_names"] = list(sim.body_names) + robot_cfg = env.robot_config + meta["effort_limits"] = list(robot_cfg.dof_effort_limit_list) + meta["dof_pos_lower_limits"] = list(robot_cfg.dof_pos_lower_limit_list) + meta["dof_pos_upper_limits"] = list(robot_cfg.dof_pos_upper_limit_list) + meta["velocity_limits"] = list(robot_cfg.dof_vel_limit_list) + asset_cfg = robot_cfg.asset + meta["urdf_path"] = str(Path(asset_cfg.asset_root) / asset_cfg.urdf_file) + return meta + + @staticmethod + def _save_npz( + buffers: dict[str, list[np.ndarray]], + metadata: dict[str, Any], + step_count: int, + output_path: str, + log_prefix: str, + ) -> None: + """Stack buffers and write compressed NPZ.""" + if step_count == 0: + return + arrays: dict[str, np.ndarray] = {} + for name, values in buffers.items(): + if values: + arrays[name] = np.stack(values, axis=0) + arrays["_metadata_json"] = np.array(json.dumps(metadata)) + path = Path(output_path) + path.parent.mkdir(parents=True, exist_ok=True) + np.savez_compressed(str(path), **arrays) + summary = ", ".join(f"{k}{list(v.shape)}" for k, v in arrays.items() if k != "_metadata_json") + logger.info(f"{log_prefix}: saved {step_count} steps to {path}\n Channels: {summary}") diff --git a/src/holosoma/holosoma/agents/callbacks/sim_utils.py b/src/holosoma/holosoma/agents/callbacks/sim_utils.py new file mode 100644 index 000000000..4921546ad --- /dev/null +++ b/src/holosoma/holosoma/agents/callbacks/sim_utils.py @@ -0,0 +1,78 @@ +"""Utilities for interfacing with IsaacSim's external force API. + +Provides body resolution and force application helpers used by eval callbacks. +""" + +from __future__ import annotations + +from typing import Any + +from holosoma.utils.safe_torch_import import torch + + +def resolve_body(sim: Any, body_name: str) -> tuple[str, int]: + """Resolve body name to (name, isaac_body_id). + + ``find_rigid_body_indice`` returns a holosoma-ordered index (BFS order + used consistently across simulators, for contact sensors and other + holosoma-level operations). ``sim.body_ids[idx]`` maps that to the raw + IsaacLab body ID used by ``_robot.data.body_pos_w``, + ``_robot.data.body_quat_w``, and ``set_external_force_and_torque``. + + Raises ValueError if not found. + """ + body_names_idx = sim.find_rigid_body_indice(body_name) + if isinstance(body_names_idx, int) and body_names_idx >= 0: + return (body_name, sim.body_ids[body_names_idx]) + available = list(getattr(sim, "body_names", [])) + raise ValueError(f"Body '{body_name}' not found. Available: {available}") + + +def apply_body_force_world( + sim: Any, + env_ids: torch.Tensor, + isaac_body_id: int, + force_world: torch.Tensor, +) -> None: + """Apply world-frame force on a body across envs. + + Converts from world frame to body-local frame (required by + ``set_external_force_and_torque``) using the body's current orientation. + + Args: + sim: Simulator instance. + env_ids: [N] env indices. + isaac_body_id: Body to apply force to. + force_world: [N, 3] or [3] force vector in world frame. + """ + from isaaclab.utils.math import quat_apply_inverse + + if force_world.dim() == 1: + force_world = force_world.unsqueeze(0).expand(len(env_ids), -1) + body_quats = sim._robot.data.body_quat_w[env_ids, isaac_body_id] + force_body = quat_apply_inverse(body_quats, force_world) + forces = force_body.unsqueeze(1) # [N, 1, 3] + torques = torch.zeros_like(forces) + body_ids = torch.tensor([isaac_body_id], device=env_ids.device) + sim._robot.set_external_force_and_torque( + forces=forces, + torques=torques, + env_ids=env_ids, + body_ids=body_ids, + ) + + +def clear_body_force( + sim: Any, + env_ids: torch.Tensor, + isaac_body_id: int, +) -> None: + """Clear external forces on a body for given envs.""" + zero = torch.zeros(len(env_ids), 1, 3, device=env_ids.device) + body_ids = torch.tensor([isaac_body_id], device=env_ids.device) + sim._robot.set_external_force_and_torque( + forces=zero, + torques=zero, + env_ids=env_ids, + body_ids=body_ids, + ) diff --git a/src/holosoma/holosoma/config_types/eval_callback.py b/src/holosoma/holosoma/config_types/eval_callback.py index 4e8408997..8c59df343 100644 --- a/src/holosoma/holosoma/config_types/eval_callback.py +++ b/src/holosoma/holosoma/config_types/eval_callback.py @@ -4,6 +4,7 @@ import dataclasses +from pydantic import model_validator from pydantic.dataclasses import dataclass @@ -17,9 +18,6 @@ class RecordingConfig: output_path: str = "eval_recording.npz" """Path to save NPZ recording.""" - env_id: int = 0 - """Environment ID to record.""" - @dataclass(frozen=True) class RecordingCallbackConfig: @@ -33,69 +31,243 @@ class RecordingCallbackConfig: @dataclass(frozen=True) -class PushConfig: - """Settings for push perturbation during evaluation.""" +class GridEvalVelocityConfig: + """Settings for velocity sweep in grid evaluation. - enabled: bool = False - """Enable push perturbations.""" + Each non-trivial axis is swept independently (other axes default to 0). + E.g. lin_vel_x=[0.25,0.5], lin_vel_y=[0.25,0.5] -> 4 conditions. + """ - force_range: tuple[float, float] = (50.0, 200.0) - """Min and max force magnitude in Newtons.""" + enabled: bool = False + """Whether to enable grid velocity sweep.""" - duration_s: tuple[float, float] = (0.1, 0.3) - """Min and max push duration in seconds.""" + lin_vel_x: tuple[float, ...] = (0.0,) + """Linear velocity X values to sweep (m/s).""" - interval_s: tuple[float, float] = (3.0, 8.0) - """Min and max interval between pushes in seconds.""" + lin_vel_y: tuple[float, ...] = (0.0,) + """Linear velocity Y values to sweep (m/s).""" - body_names: str = "torso_link,pelvis" - """Comma-separated body names to push.""" + ang_vel_yaw: tuple[float, ...] = (0.0,) + """Angular velocity yaw values to sweep (rad/s).""" - env_id: int = 0 - """Environment ID to apply pushes.""" + warmup_s: float = 1.0 + """Duration of warmup period in seconds. During warmup all velocity commands are + zero. After warmup, grid velocities are applied. The warmup period is still + recorded (analysis scripts skip it using the warmup_steps metadata field).""" @dataclass(frozen=True) -class PushCallbackConfig: - """Instantiation config for EvalPushCallback.""" +class GridEvalVelocityCallbackConfig: + """Instantiation config for GridEvalVelocityCallback.""" - _target_: str = "holosoma.agents.callbacks.push.EvalPushCallback" + _target_: str = "holosoma.agents.callbacks.grid_eval_velocity.GridEvalVelocityCallback" """Class to instantiate.""" - config: PushConfig = PushConfig() - """Push perturbation settings.""" + config: GridEvalVelocityConfig = GridEvalVelocityConfig() + """Grid velocity sweep settings.""" @dataclass(frozen=True) -class PayloadConfig: - """Settings for wrist payload simulation during evaluation. +class GridEvalPayloadConfig: + """Settings for payload sweep in grid evaluation. + + Sweeps payload across body-group x mass dimensions, cross-producted with + the velocity conditions from GridEvalVelocityCallback. - Applies a constant downward force on wrist/elbow links to simulate - the robot holding a payload (force = mass * 9.81). + Each entry in ``body_groups`` is a comma-separated group of body names that + receive the force together (force is split evenly across bodies in the group). + ``body_labels`` provides human-readable names for each group. + + Example: + body_groups=("pelvis", "left_wrist_yaw_link,right_wrist_yaw_link") + body_labels=("torso", "wrists") + mass_kg=(0.0, 1.0, 2.0) + + This creates 2 body_groups x 3 masses = 6 payload sub-conditions, each + cross-producted with the velocity grid. """ enabled: bool = False - """Enable wrist payload forces.""" + """Whether to enable grid payload sweep.""" + + body_groups: tuple[str, ...] = () + """Body groups for payload forces. Each entry is a comma-separated list of + body names that receive force together. Creates one sweep dimension.""" + + body_labels: tuple[str, ...] = () + """Human-readable labels for each body group (must match length of body_groups).""" + + mass_kg: tuple[float, ...] = (0.0,) + """Payload masses to sweep in kg (0 = no payload).""" + + @model_validator(mode="after") + def _validate(self) -> GridEvalPayloadConfig: + if not self.enabled: + return self + if not self.body_groups: + raise ValueError("GridEvalPayloadConfig: body_groups must not be empty when enabled") + if self.body_labels and len(self.body_labels) != len(self.body_groups): + raise ValueError( + f"GridEvalPayloadConfig: body_labels length ({len(self.body_labels)}) " + f"must match body_groups length ({len(self.body_groups)})" + ) + if not self.body_labels: + object.__setattr__(self, "body_labels", tuple(bg.replace(",", "+") for bg in self.body_groups)) + return self + + +@dataclass(frozen=True) +class GridEvalPayloadCallbackConfig: + """Instantiation config for GridEvalPayloadCallback.""" - mass_kg: float = 1.0 - """Payload mass in kg. Force is split evenly across resolved bodies.""" + _target_: str = "holosoma.agents.callbacks.grid_eval_payload.GridEvalPayloadCallback" + """Class to instantiate.""" - body_names: str = "left_wrist_yaw_link,right_wrist_yaw_link" - """Comma-separated wrist body names.""" + config: GridEvalPayloadConfig = GridEvalPayloadConfig() + """Grid payload sweep settings.""" - env_id: int = 0 - """Environment ID to apply payload forces.""" + +@dataclass(frozen=True) +class GridEvalPushConfig: + """Settings for push perturbation sweep in grid evaluation. + + Sweeps pushes across body x direction x gait_phase dimensions, + cross-producted with the velocity conditions from GridEvalVelocityCallback. + + Each condition receives exactly one deterministic push, triggered when the + target gait phase is first detected after a gait-analysis warmup period. + + **Directions** are in the robot's local frame (rotated by the robot's yaw + at push time): + forward = robot's heading direction + backward = opposite to heading + left = 90 deg left of heading + right = 90 deg right of heading + + **Gait phases** are detected from left-foot-height oscillations. After warmup, + the callback records foot heights for at least ``min_gait_cycles`` full gait + cycles (detected dynamically from zero-crossings), then monitors foot state + to trigger pushes at target phases: + swing_to_stance = left foot touchdown (transition from air to ground) + stance_to_swing = left foot liftoff (transition from ground to air) + mid_stance = left foot on ground, right foot at peak height + mid_swing = left foot at peak height (in air) + + Foot bodies are specified via ``left_foot_body`` / ``right_foot_body``. + + Example: + body_names=("torso_link", "pelvis") + body_labels=("torso", "pelvis") + directions=("forward", "backward", "left", "right") + gait_phases=("swing_to_stance", "stance_to_swing", "mid_stance", "mid_swing") + force_n=150.0 + duration_s=0.2 + + This creates 2 bodies x 4 directions x 4 phases = 32 push sub-conditions, + each cross-producted with the velocity grid. + """ + + enabled: bool = False + """Whether to enable grid push sweep.""" + + body_names: tuple[str, ...] = () + """Body names to push. Each entry is a single body name.""" + + body_labels: tuple[str, ...] = () + """Human-readable labels for each body (must match length of body_names).""" + + directions: tuple[str, ...] = ("forward", "backward", "left", "right") + """Push directions in robot-local frame: forward, backward, left, right.""" + + gait_phases: tuple[str, ...] = ("swing_to_stance", "stance_to_swing", "mid_stance", "mid_swing") + """Gait phases at which to trigger pushes. Based on left foot state: + swing_to_stance = touchdown, stance_to_swing = liftoff, + mid_stance = right foot at peak, mid_swing = left foot at peak.""" + + min_gait_cycles: int = 3 + """Minimum number of full gait cycles to observe before finalizing gait analysis. + Analysis continues beyond gait_analysis_s if needed to reach this count.""" + + gait_analysis_s: float = 2.0 + """Minimum duration (seconds) after warmup for gait analysis. Actual analysis + may run longer if min_gait_cycles haven't been observed yet.""" + + max_gait_analysis_s: float = 8.0 + """Maximum duration (seconds) for gait analysis. If min_gait_cycles aren't + observed within this window, analysis finalizes anyway with a warning.""" + + left_foot_body: str = "left_foot_contact_point" + """Body name for left foot height detection.""" + + right_foot_body: str = "right_foot_contact_point" + """Body name for right foot height detection.""" + + foot_contact_threshold: float = 0.5 + """Fraction of foot height range below which the foot is considered 'on ground'. + E.g. 0.5 means the foot is on the ground when its height is in the lower 50% + of its observed swing range.""" + + force_n: float = 150.0 + """Push force magnitude in Newtons (used when force_magnitudes is empty).""" + + force_magnitudes: tuple[float, ...] = () + """Force magnitudes to sweep (Newtons). When non-empty, overrides force_n + and creates an additional sweep dimension. E.g. (50, 100, 200, 300).""" + + duration_s: float = 0.2 + """Push duration in seconds.""" + + DIRECTION_VECTORS: dict[str, tuple[float, float, float]] = dataclasses.field( + default_factory=lambda: { + "forward": (1.0, 0.0, 0.0), + "backward": (-1.0, 0.0, 0.0), + "left": (0.0, 1.0, 0.0), + "right": (0.0, -1.0, 0.0), + }, + repr=False, + ) + """Mapping from direction name to robot-local unit vector.""" + + _VALID_GAIT_PHASES: tuple[str, ...] = ( + "swing_to_stance", + "stance_to_swing", + "mid_stance", + "mid_swing", + ) + + @model_validator(mode="after") + def _validate(self) -> GridEvalPushConfig: + if not self.enabled: + return self + if not self.body_names: + raise ValueError("GridEvalPushConfig: body_names must not be empty when enabled") + if self.body_labels and len(self.body_labels) != len(self.body_names): + raise ValueError( + f"GridEvalPushConfig: body_labels length ({len(self.body_labels)}) " + f"must match body_names length ({len(self.body_names)})" + ) + if not self.body_labels: + object.__setattr__(self, "body_labels", self.body_names) + for d in self.directions: + if d not in self.DIRECTION_VECTORS: + raise ValueError(f"GridEvalPushConfig: unknown direction '{d}'. Valid: {list(self.DIRECTION_VECTORS)}") + for p in self.gait_phases: + if p not in self._VALID_GAIT_PHASES: + raise ValueError( + f"GridEvalPushConfig: unknown gait phase '{p}'. Valid: {list(self._VALID_GAIT_PHASES)}" + ) + return self @dataclass(frozen=True) -class PayloadCallbackConfig: - """Instantiation config for EvalPayloadCallback.""" +class GridEvalPushCallbackConfig: + """Instantiation config for GridEvalPushCallback.""" - _target_: str = "holosoma.agents.callbacks.payload.EvalPayloadCallback" + _target_: str = "holosoma.agents.callbacks.grid_eval_push.GridEvalPushCallback" """Class to instantiate.""" - config: PayloadConfig = PayloadConfig() - """Payload simulation settings.""" + config: GridEvalPushConfig = GridEvalPushConfig() + """Grid push sweep settings.""" @dataclass(frozen=True) @@ -109,11 +281,23 @@ class EvalCallbacksConfig: recording: RecordingCallbackConfig = RecordingCallbackConfig() """Trajectory recording callback.""" - push: PushCallbackConfig = PushCallbackConfig() - """Push perturbation callback.""" + grid_velocity: GridEvalVelocityCallbackConfig = GridEvalVelocityCallbackConfig() + """Grid-based velocity sweep callback.""" + + grid_payload: GridEvalPayloadCallbackConfig = GridEvalPayloadCallbackConfig() + """Grid-based payload sweep callback.""" + + grid_push: GridEvalPushCallbackConfig = GridEvalPushCallbackConfig() + """Grid-based push sweep callback.""" - payload: PayloadCallbackConfig = PayloadCallbackConfig() - """Wrist payload simulation callback.""" + @model_validator(mode="after") + def _validate(self) -> EvalCallbacksConfig: + if self.grid_payload.config.enabled and self.grid_push.config.enabled: + raise ValueError( + "grid_payload and grid_push cannot both be enabled: " + "both inject external forces via IsaacSim and would overwrite each other" + ) + return self def collect_active_callbacks(self) -> dict: """Collect callback configs where config.enabled is True.""" diff --git a/src/holosoma/holosoma/config_values/loco/g1/reward.py b/src/holosoma/holosoma/config_values/loco/g1/reward.py index fdfb61dee..a3c9b5687 100644 --- a/src/holosoma/holosoma/config_values/loco/g1/reward.py +++ b/src/holosoma/holosoma/config_values/loco/g1/reward.py @@ -1,93 +1,100 @@ """Locomotion reward presets for the G1 robot.""" +from __future__ import annotations + from holosoma.config_types.reward import RewardManagerCfg, RewardTermCfg +# Shared reward terms for G1 locomotion (used by both PPO and FastSAC). +_g1_29dof_loco_common_terms: dict[str, RewardTermCfg] = { + "tracking_lin_vel": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:tracking_lin_vel", + weight=2.0, + params={"tracking_sigma": 0.25}, + ), + "tracking_ang_vel": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:tracking_ang_vel", + weight=1.5, + params={"tracking_sigma": 0.25}, + ), + "penalty_ang_vel_xy": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:penalty_ang_vel_xy", + weight=-1.0, + params={}, + tags=["penalty_curriculum"], + ), + "penalty_orientation": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:penalty_orientation", + weight=-10.0, + params={}, + tags=["penalty_curriculum"], + ), + "penalty_action_rate": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:penalty_action_rate", + weight=-2.0, + params={}, + tags=["penalty_curriculum"], + ), + "feet_phase": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:feet_phase", + weight=5.0, + params={"swing_height": 0.09, "tracking_sigma": 0.008}, + ), + "pose": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:pose", + weight=-0.5, + params={ + "pose_weights": [ + 0.01, + 1.0, + 5.0, # left hip: pitch, roll, yaw + 0.01, + 5.0, + 5.0, # left: knee, ankle_pitch, ankle_roll + 0.01, + 1.0, + 5.0, # right hip: pitch, roll, yaw + 0.01, + 5.0, + 5.0, # right: knee, ankle_pitch, ankle_roll + 50.0, + 50.0, + 50.0, # waist: yaw, roll, pitch + 50.0, + 50.0, + 50.0, + 50.0, + 50.0, + 50.0, + 50.0, # left arm + 50.0, + 50.0, + 50.0, + 50.0, + 50.0, + 50.0, + 50.0, # right arm + ], + }, + tags=["penalty_curriculum"], + ), + "penalty_close_feet_xy": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:penalty_close_feet_xy", + weight=-10.0, + params={"close_feet_threshold": 0.15}, + tags=["penalty_curriculum"], + ), + "penalty_feet_ori": RewardTermCfg( + func="holosoma.managers.reward.terms.locomotion:penalty_feet_ori", + weight=-5.0, + params={}, + tags=["penalty_curriculum"], + ), +} + g1_29dof_loco = RewardManagerCfg( only_positive_rewards=False, terms={ - "tracking_lin_vel": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:tracking_lin_vel", - weight=2.0, - params={"tracking_sigma": 0.25}, - ), - "tracking_ang_vel": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:tracking_ang_vel", - weight=1.5, - params={"tracking_sigma": 0.25}, - ), - "penalty_ang_vel_xy": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_ang_vel_xy", - weight=-1.0, - params={}, - tags=["penalty_curriculum"], - ), - "penalty_orientation": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_orientation", - weight=-10.0, - params={}, - tags=["penalty_curriculum"], - ), - "penalty_action_rate": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_action_rate", - weight=-2.0, - params={}, - tags=["penalty_curriculum"], - ), - "feet_phase": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:feet_phase", - weight=5.0, - params={"swing_height": 0.09, "tracking_sigma": 0.008}, - ), - "pose": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:pose", - weight=-0.5, - params={ - "pose_weights": [ - 0.01, - 1.0, - 5.0, - 0.01, - 5.0, - 5.0, - 0.01, - 1.0, - 5.0, - 0.01, - 5.0, - 5.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - ], - }, - tags=["penalty_curriculum"], - ), - "penalty_close_feet_xy": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_close_feet_xy", - weight=-10.0, - params={"close_feet_threshold": 0.15}, - tags=["penalty_curriculum"], - ), - "penalty_feet_ori": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_feet_ori", - weight=-5.0, - params={}, - tags=["penalty_curriculum"], - ), + **_g1_29dof_loco_common_terms, "alive": RewardTermCfg( func="holosoma.managers.reward.terms.locomotion:alive", weight=1.0, @@ -99,89 +106,7 @@ g1_29dof_loco_fast_sac = RewardManagerCfg( only_positive_rewards=False, terms={ - "tracking_lin_vel": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:tracking_lin_vel", - weight=2.0, - params={"tracking_sigma": 0.25}, - ), - "tracking_ang_vel": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:tracking_ang_vel", - weight=1.5, - params={"tracking_sigma": 0.25}, - ), - "penalty_ang_vel_xy": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_ang_vel_xy", - weight=-1.0, - params={}, - tags=["penalty_curriculum"], - ), - "penalty_orientation": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_orientation", - weight=-10.0, - params={}, - tags=["penalty_curriculum"], - ), - "penalty_action_rate": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_action_rate", - weight=-2.0, - params={}, - tags=["penalty_curriculum"], - ), - "feet_phase": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:feet_phase", - weight=5.0, - params={"swing_height": 0.09, "tracking_sigma": 0.008}, - ), - "pose": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:pose", - weight=-0.5, - params={ - "pose_weights": [ - 0.01, - 1.0, - 5.0, - 0.01, - 5.0, - 5.0, - 0.01, - 1.0, - 5.0, - 0.01, - 5.0, - 5.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - ], - }, - tags=["penalty_curriculum"], - ), - "penalty_close_feet_xy": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_close_feet_xy", - weight=-10.0, - params={"close_feet_threshold": 0.15}, - tags=["penalty_curriculum"], - ), - "penalty_feet_ori": RewardTermCfg( - func="holosoma.managers.reward.terms.locomotion:penalty_feet_ori", - weight=-5.0, - params={}, - tags=["penalty_curriculum"], - ), + **_g1_29dof_loco_common_terms, "alive": RewardTermCfg( func="holosoma.managers.reward.terms.locomotion:alive", weight=10.0,