# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause from __future__ import annotations from typing import TYPE_CHECKING import torch from isaaclab.managers import SceneEntityCfg if TYPE_CHECKING: from isaaclab.envs import ManagerBasedRLEnv def root_low_speed_for_duration( env: ManagerBasedRLEnv, threshold: float, duration: float, asset_cfg: SceneEntityCfg, ) -> torch.Tensor: """Terminate environments whose robot stays below a planar speed threshold for a duration.""" if threshold < 0.0: raise ValueError(f"Speed threshold must be non-negative, got {threshold}.") if duration <= 0.0: raise ValueError(f"Duration must be positive, got {duration}.") asset = env.scene[asset_cfg.name] speed = torch.linalg.vector_norm(asset.data.root_lin_vel_w[:, :2], dim=1) episode_length = env.episode_length_buf elapsed = getattr(env, "_root_low_speed_elapsed", None) last_episode_length = getattr(env, "_root_low_speed_last_episode_length", None) if elapsed is None: elapsed = torch.zeros_like(speed) last_episode_length = episode_length.clone() setattr(env, "_root_low_speed_elapsed", elapsed) elif not isinstance(last_episode_length, torch.Tensor): raise RuntimeError("Low-speed termination episode state is missing or invalid.") if elapsed.shape != speed.shape or elapsed.device != speed.device: raise RuntimeError("Low-speed termination state does not match the environment.") if last_episode_length.shape != episode_length.shape or last_episode_length.device != episode_length.device: raise RuntimeError("Low-speed termination episode state does not match the environment.") elapsed[episode_length <= last_episode_length] = 0.0 below_threshold = speed < threshold elapsed[below_threshold] += env.step_dt elapsed[~below_threshold] = 0.0 terminated = elapsed > duration elapsed[terminated] = 0.0 setattr(env, "_root_low_speed_last_episode_length", episode_length.clone()) return terminated