57 lines
2.1 KiB
Python
57 lines
2.1 KiB
Python
# 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
|