TwoFootBot-isaaclab/src/TwoFootBot_isaaclab/tasks/manager_based/twofootbot/mdp/terminations.py

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