diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000000..51ec310d223 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/robot_model"] + path = source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/robot_model + url = https://github.com/ntnu-arl/robot_model.git + diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 0feaabb2c8f..e12718769f9 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -67,6 +67,7 @@ Guidelines for modifications: * Felix Yu * Gary Lvov * Giulio Romualdi +* Grzegorz Malczyk * Haoran Zhou * Harsh Patel * HoJin Jeon @@ -100,6 +101,7 @@ Guidelines for modifications: * Michael Noseworthy * Michael Lin * Miguel Alonso Jr +* Mihir Kulkarni * Mingyu Lee * Muhong Guo * Neel Anand Jawale @@ -135,6 +137,7 @@ Guidelines for modifications: * Virgilio Gómez Lambo * Vladimir Fokow * Wei Yang +* Welf Rehberg * Xavier Nal * Xinjie Yao * Xinpeng Liu diff --git a/docs/make.bat b/docs/make.bat index 941689ef03c..676a3abc67d 100644 --- a/docs/make.bat +++ b/docs/make.bat @@ -1,65 +1,65 @@ -@ECHO OFF - -pushd %~dp0 - -REM Command file to build Sphinx documentation - -set SOURCEDIR=. -set BUILDDIR=_build - -REM Check if a specific target was passed -if "%1" == "multi-docs" ( - REM Check if SPHINXBUILD is set, if not default to sphinx-multiversion - if "%SPHINXBUILD%" == "" ( - set SPHINXBUILD=sphinx-multiversion - ) - where %SPHINXBUILD% >NUL 2>NUL - if errorlevel 1 ( - echo. - echo.The 'sphinx-multiversion' command was not found. Make sure you have Sphinx - echo.installed, then set the SPHINXBUILD environment variable to point - echo.to the full path of the 'sphinx-multiversion' executable. Alternatively you - echo.may add the Sphinx directory to PATH. - echo. - echo.If you don't have Sphinx installed, grab it from - echo.http://sphinx-doc.org/ - exit /b 1 - ) - %SPHINXBUILD% %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% - - REM Copy the redirect index.html to the build directory - copy _redirect\index.html %BUILDDIR%\index.html - goto end -) - -if "%1" == "current-docs" ( - REM Check if SPHINXBUILD is set, if not default to sphinx-build - if "%SPHINXBUILD%" == "" ( - set SPHINXBUILD=sphinx-build - ) - where %SPHINXBUILD% >NUL 2>NUL - if errorlevel 1 ( - echo. - echo.The 'sphinx-build' command was not found. Make sure you have Sphinx - echo.installed, then set the SPHINXBUILD environment variable to point - echo.to the full path of the 'sphinx-build' executable. Alternatively you - echo.may add the Sphinx directory to PATH. - echo. - echo.If you don't have Sphinx installed, grab it from - echo.http://sphinx-doc.org/ - exit /b 1 - ) - if exist "%BUILDDIR%\current" rmdir /s /q "%BUILDDIR%\current" - %SPHINXBUILD% -W "%SOURCEDIR%" "%BUILDDIR%\current" %SPHINXOPTS% - goto end -) - -REM If no valid target is passed, show usage instructions -echo. -echo.Usage: -echo. make.bat multi-docs - To build the multi-version documentation. -echo. make.bat current-docs - To build the current documentation. -echo. - -:end -popd +@ECHO OFF + +pushd %~dp0 + +REM Command file to build Sphinx documentation + +set SOURCEDIR=. +set BUILDDIR=_build + +REM Check if a specific target was passed +if "%1" == "multi-docs" ( + REM Check if SPHINXBUILD is set, if not default to sphinx-multiversion + if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-multiversion + ) + where %SPHINXBUILD% >NUL 2>NUL + if errorlevel 1 ( + echo. + echo.The 'sphinx-multiversion' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-multiversion' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 + ) + %SPHINXBUILD% %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + + REM Copy the redirect index.html to the build directory + copy _redirect\index.html %BUILDDIR%\index.html + goto end +) + +if "%1" == "current-docs" ( + REM Check if SPHINXBUILD is set, if not default to sphinx-build + if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build + ) + where %SPHINXBUILD% >NUL 2>NUL + if errorlevel 1 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 + ) + if exist "%BUILDDIR%\current" rmdir /s /q "%BUILDDIR%\current" + %SPHINXBUILD% -W "%SOURCEDIR%" "%BUILDDIR%\current" %SPHINXOPTS% + goto end +) + +REM If no valid target is passed, show usage instructions +echo. +echo.Usage: +echo. make.bat multi-docs - To build the multi-version documentation. +echo. make.bat current-docs - To build the current documentation. +echo. + +:end +popd diff --git a/source/isaaclab/isaaclab/actuators/__init__.py b/source/isaaclab/isaaclab/actuators/__init__.py index 5ccf5d7b082..a8ef77e7fe1 100644 --- a/source/isaaclab/isaaclab/actuators/__init__.py +++ b/source/isaaclab/isaaclab/actuators/__init__.py @@ -32,6 +32,8 @@ IdealPDActuatorCfg, ImplicitActuatorCfg, RemotizedPDActuatorCfg, + ThrusterCfg, ) from .actuator_net import ActuatorNetLSTM, ActuatorNetMLP from .actuator_pd import DCMotor, DelayedPDActuator, IdealPDActuator, ImplicitActuator, RemotizedPDActuator +from .thruster import Thruster diff --git a/source/isaaclab/isaaclab/actuators/actuator_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_cfg.py index e5351e4fa63..54a525610b9 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_cfg.py +++ b/source/isaaclab/isaaclab/actuators/actuator_cfg.py @@ -11,6 +11,7 @@ from . import actuator_net, actuator_pd from .actuator_base import ActuatorBase +from .thruster import Thruster @configclass @@ -283,3 +284,50 @@ class RemotizedPDActuatorCfg(DelayedPDActuatorCfg): This tensor describes the relationship between the joint angle (rad), the transmission ratio (in/out), and the output torque (N*m). The table is used to interpolate the output torque based on the joint angle. """ + + +@configclass +class ThrusterCfg: + """Configuration for thruster actuator groups. + + This config defines per-actuator-group parameters used by the low-level + thruster/motor models (time-constants, thrust ranges, integration scheme, + and initial state specifications). Fields left as ``MISSING`` are required + and must be provided by the user configuration. + """ + + class_type: type[Thruster] = Thruster + """Concrete Python class that consumes this config.""" + + dt: float = MISSING + """Simulation/integration timestep used by the thruster update [s].""" + + thrust_range: tuple[float, float] = MISSING + """Per-motor thrust clamp range [N]: values are clipped to this interval.""" + + max_thrust_rate: float = 100000.0 + """Per-motor thrust slew-rate limit applied inside the first-order model [N/s].""" + + thrust_const_range: tuple[float, float] = MISSING + """Range for thrust coefficient :math:`k_f` [N/(rps²)].""" + + tau_inc_range: tuple[float, float] = MISSING + """Range of time constants when commanded output is **increasing** (rise dynamics) [s].""" + + tau_dec_range: tuple[float, float] = MISSING + """Range of time constants when commanded output is **decreasing** (fall dynamics) [s].""" + + torque_to_thrust_ratio: float = MISSING + """Yaw-moment coefficient converting thrust to motor torque about +Z [N·m per N]. + Used as ``tau_z = torque_to_thrust_ratio * thrust_z * direction``. + """ + + use_discrete_approximation: bool = True + """If ``True``, use discrete mixing factor ``1/(dt + tau)``; if ``False``, use continuous ``1/tau``.""" + + integration_scheme: Literal["rk4", "euler"] = "rk4" + """Numerical integrator for the first-order model. Choose ``"euler"`` or ``"rk4"``.""" + + thruster_names_expr: list[str] = MISSING + + """Articulation's joint names that are part of the group.""" diff --git a/source/isaaclab/isaaclab/actuators/thruster.py b/source/isaaclab/isaaclab/actuators/thruster.py new file mode 100644 index 00000000000..fad660eb9f5 --- /dev/null +++ b/source/isaaclab/isaaclab/actuators/thruster.py @@ -0,0 +1,227 @@ +# Copyright (c) 2022-2025, 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 + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.utils.types import ArticulationThrustActions + +if TYPE_CHECKING: + from .actuator_cfg import ThrusterCfg + + +class Thruster: + """Low-level motor/thruster dynamics with separate rise/fall time constants. + + Integration scheme is Euler or RK4. All internal buffers are shaped (num_envs, num_motors). + Units: thrust [N], rates [N/s], time [s]. + """ + + computed_thrust: torch.Tensor + """The computed thrust for the actuator group. Shape is (num_envs, num_thrusters).""" + + applied_thrust: torch.Tensor + """The applied thrust for the actuator group. Shape is (num_envs, num_thrusters). + + This is the thrust obtained after clipping the :attr:`computed_thrust` based on the + actuator characteristics. + """ + + cfg: ThrusterCfg + + def __init__( + self, + cfg: ThrusterCfg, + thruster_names: list[str], + thruster_ids: slice | torch.Tensor, + num_envs: int, + device: str, + init_thruster_rps: torch.Tensor, + ): + """Construct buffers and sample per-motor parameters. + + Args: + cfg: Thruster configuration. + thruster_names: List of thruster names belonging to this group. + thruster_ids: Slice or tensor of indices into the articulation thruster array. + num_envs: Number of parallel/vectorized environments. + device: PyTorch device string or device identifier. + init_thruster_rps: Initial per-thruster rotations-per-second tensor used when + the configuration uses RPM-based thrust modelling. + """ + self.cfg = cfg + self._num_envs = num_envs + self._device = device + self._thruster_names = thruster_names + self._thruster_indices = thruster_ids + self._init_thruster_rps = init_thruster_rps + + # Range tensors, shaped (num_envs, 2, num_motors); [:,0,:]=min, [:,1,:]=max + self.num_motors = len(thruster_names) + self.thrust_r = torch.tensor(cfg.thrust_range).to(self._device) + self.tau_inc_r = torch.tensor(cfg.tau_inc_range).to(self._device) + self.tau_dec_r = torch.tensor(cfg.tau_dec_range).to(self._device) + + self.max_rate = torch.tensor(cfg.max_thrust_rate).expand(self._num_envs, self.num_motors).to(self._device) + + self.max_thrust = self.cfg.thrust_range[1] + self.min_thrust = self.cfg.thrust_range[0] + + # State & randomized per-motor parameters + self.tau_inc_s = math_utils.sample_uniform(*self.tau_inc_r, (self._num_envs, self.num_motors), self._device) + self.tau_dec_s = math_utils.sample_uniform(*self.tau_dec_r, (self._num_envs, self.num_motors), self._device) + + self.thrust_const_r = torch.tensor(cfg.thrust_const_range).to(device) + self.thrust_const = math_utils.sample_uniform( + *self.thrust_const_r, (self._num_envs, self.num_motors), self._device + ) + + self.curr_thrust = ( + torch.ones(self._num_envs, self.num_motors, device=self._device, dtype=torch.float32) + * self.thrust_const + * self._init_thruster_rps**2 + ) + + # Mixing factor (discrete vs continuous form) + if self.cfg.use_discrete_approximation: + self.mixing_factor_function = self.discrete_mixing_factor + else: + self.mixing_factor_function = self.continuous_mixing_factor + + # Choose stepping kernel once (avoids per-step branching) + if self.cfg.integration_scheme == "euler": + self._step_thrust = self.compute_thrust_with_rpm_time_constant + elif self.cfg.integration_scheme == "rk4": + self._step_thrust = self.compute_thrust_with_rpm_time_constant_rk4 + else: + raise ValueError("integration scheme unknown") + + @property + def num_thrusters(self) -> int: + """Number of actuators in the group.""" + return len(self._thruster_names) + + @property + def thruster_names(self) -> list[str]: + """Articulation's thruster names that are part of the group.""" + return self._thruster_names + + @property + def thruster_indices(self) -> slice | torch.Tensor: + """Articulation's thruster indices that are part of the group. + + Note: + If :obj:`slice(None)` is returned, then the group contains all the thrusters in the articulation. + We do this to avoid unnecessary indexing of the thrusters for performance reasons. + """ + return self._thruster_indices + + def compute(self, control_action: ArticulationThrustActions) -> ArticulationThrustActions: + """Advance the thruster state one step. + + Applies saturation, chooses rise/fall tau per motor, computes mixing factor, + and integrates with the selected kernel. + + Args: + control_action: (num_envs, num_motors) commanded per-motor thrust [N]. + + Returns: + (num_envs, num_motors) updated thrust state [N]. + + """ + des_thrust = control_action.thrusts + des_thrust = torch.clamp(des_thrust, *self.thrust_r) + + thrust_decrease_mask = torch.sign(self.curr_thrust) * torch.sign(des_thrust - self.curr_thrust) + motor_tau = torch.where(thrust_decrease_mask < 0, self.tau_dec_s, self.tau_inc_s) + mixing = self.mixing_factor_function(motor_tau) + + self.curr_thrust[:] = self._step_thrust(des_thrust, self.curr_thrust, mixing) + + self.computed_thrust = self.curr_thrust + self.applied_thrust = torch.clamp(self.computed_thrust, self.min_thrust, self.max_thrust) + + control_action.thrusts = self.applied_thrust + + return control_action + + def reset_idx(self, env_ids=None) -> None: + """Re-sample parameters and reinitialize state. + + Args: + env_ids: Env indices to reset. If ``None``, resets all envs. + """ + if env_ids is None: + env_ids = slice(None) + + if isinstance(env_ids, slice): + num_resets = self._num_envs + else: + num_resets = len(env_ids) + + self.tau_inc_s[env_ids] = math_utils.sample_uniform( + *self.tau_inc_r, + (num_resets, self.num_motors), + self._device, + ) + self.tau_dec_s[env_ids] = math_utils.sample_uniform( + *self.tau_dec_r, + (num_resets, self.num_motors), + self._device, + ) + self.thrust_const[env_ids] = math_utils.sample_uniform( + *self.thrust_const_r, + (num_resets, self.num_motors), + self._device, + ) + self.curr_thrust[env_ids] = self.thrust_const[env_ids] * self._init_thruster_rps[env_ids] ** 2 + + def reset(self, env_ids: Sequence[int]) -> None: + """Reset all envs.""" + self.reset_idx(env_ids) + + def motor_model_rate(self, error: torch.Tensor, mixing_factor: torch.Tensor): + return torch.clamp(mixing_factor * (error), -self.max_rate, self.max_rate) + + def rk4_integration(self, error: torch.Tensor, mixing_factor: torch.Tensor): + k1 = self.motor_model_rate(error, mixing_factor) + k2 = self.motor_model_rate(error + 0.5 * self.cfg.dt * k1, mixing_factor) + k3 = self.motor_model_rate(error + 0.5 * self.cfg.dt * k2, mixing_factor) + k4 = self.motor_model_rate(error + self.cfg.dt * k3, mixing_factor) + return (self.cfg.dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4) + + def discrete_mixing_factor(self, time_constant: torch.Tensor): + return 1.0 / (self.cfg.dt + time_constant) + + def continuous_mixing_factor(self, time_constant: torch.Tensor): + return 1.0 / time_constant + + def compute_thrust_with_rpm_time_constant( + self, + des_thrust: torch.Tensor, + curr_thrust: torch.Tensor, + mixing_factor: torch.Tensor, + ): + current_rpm = torch.sqrt(curr_thrust / self.thrust_const) + desired_rpm = torch.sqrt(des_thrust / self.thrust_const) + rpm_error = desired_rpm - current_rpm + current_rpm += self.motor_model_rate(rpm_error, mixing_factor) * self.cfg.dt + return self.thrust_const * current_rpm**2 + + def compute_thrust_with_rpm_time_constant_rk4( + self, + des_thrust: torch.Tensor, + curr_thrust: torch.Tensor, + mixing_factor: torch.Tensor, + ) -> torch.Tensor: + current_rpm = torch.sqrt(curr_thrust / self.thrust_const) + desired_rpm = torch.sqrt(des_thrust / self.thrust_const) + rpm_error = desired_rpm - current_rpm + current_rpm += self.rk4_integration(rpm_error, mixing_factor) + return self.thrust_const * current_rpm**2 diff --git a/source/isaaclab/isaaclab/assets/__init__.py b/source/isaaclab/isaaclab/assets/__init__.py index 206e5dd9c5c..a5597a9c0b1 100644 --- a/source/isaaclab/isaaclab/assets/__init__.py +++ b/source/isaaclab/isaaclab/assets/__init__.py @@ -38,7 +38,7 @@ the corresponding actuator torques. """ -from .articulation import Articulation, ArticulationCfg, ArticulationData +from .articulation import Articulation, ArticulationCfg, ArticulationData, Multirotor, MultirotorCfg, MultirotorData from .asset_base import AssetBase from .asset_base_cfg import AssetBaseCfg from .deformable_object import DeformableObject, DeformableObjectCfg, DeformableObjectData diff --git a/source/isaaclab/isaaclab/assets/articulation/__init__.py b/source/isaaclab/isaaclab/assets/articulation/__init__.py index 9429f0fec31..d0caee0693e 100644 --- a/source/isaaclab/isaaclab/assets/articulation/__init__.py +++ b/source/isaaclab/isaaclab/assets/articulation/__init__.py @@ -8,3 +8,6 @@ from .articulation import Articulation from .articulation_cfg import ArticulationCfg from .articulation_data import ArticulationData +from .multirotor import Multirotor +from .multirotor_cfg import MultirotorCfg +from .multirotor_data import MultirotorData diff --git a/source/isaaclab/isaaclab/assets/articulation/multirotor.py b/source/isaaclab/isaaclab/assets/articulation/multirotor.py new file mode 100644 index 00000000000..eaaeaae3a12 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/articulation/multirotor.py @@ -0,0 +1,382 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Flag for pyright to ignore type errors in this file. +# pyright: reportPrivateUsage=false + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.utils.string as string_utils +from isaaclab.actuators import Thruster +from isaaclab.assets.articulation.multirotor_data import MultirotorData +from isaaclab.utils.types import ArticulationThrustActions + +from .articulation import Articulation + +if TYPE_CHECKING: + from .multirotor_cfg import MultirotorCfg + + +class Multirotor(Articulation): + """A multirotor articulation asset class. + + This class extends the base articulation class to support multirotor vehicles + with thruster actuators that can be applied at specific body locations. + """ + + cfg: MultirotorCfg + """Configuration instance for the multirotor.""" + + actuators: dict[str, Thruster] + """Dictionary of thruster actuator instances for the multirotor. + + The keys are the actuator names and the values are the actuator instances. The actuator instances + are initialized based on the actuator configurations specified in the :attr:`MultirotorCfg.actuators` + attribute. They are used to compute the thruster commands during the :meth:`write_data_to_sim` function. + """ + + def __init__(self, cfg: MultirotorCfg): + """Initialize the multirotor articulation. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + """ + Properties + """ + + @property + def thruster_names(self) -> list[str]: + """Ordered names of thrusters in the multirotor.""" + if not hasattr(self, "actuators") or not self.actuators: + return [] + + thruster_names = [] + for actuator in self.actuators.values(): + if hasattr(actuator, "thruster_names"): + thruster_names.extend(actuator.thruster_names) + else: + raise ValueError("Non thruster actuator found in multirotor actuators. Not supported at the moment.") + + return thruster_names + + @property + def num_thrusters(self) -> int: + """Number of thrusters in the multirotor.""" + return len(self.thruster_names) + + @property + def allocation_matrix(self) -> torch.Tensor: + """Allocation matrix for control allocation.""" + return torch.tensor(self.cfg.allocation_matrix, device=self.device, dtype=torch.float32) + + """ + Operations + """ + + def set_thrust_target( + self, + target: torch.Tensor, + thruster_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set target thrust values for thrusters. + + Args: + target: Target thrust values. Shape is (num_envs, num_thrusters) or (num_envs,). + thruster_ids: Indices of thrusters to set. Defaults to None (all thrusters). + env_ids: Environment indices to set. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if thruster_ids is None: + thruster_ids = slice(None) + + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and thruster_ids != slice(None): + env_ids = env_ids[:, None] + + # set targets + self._data.thrust_target[env_ids, thruster_ids] = target + + def reset(self, env_ids: Sequence[int] | None = None): + """Reset the multirotor to default state. + + Args: + env_ids: Environment indices to reset. Defaults to None (all environments). + """ + # call parent reset + super().reset(env_ids) + + # reset multirotor-specific data + if env_ids is None: + env_ids = self._ALL_INDICES + elif not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, dtype=torch.long, device=self.device) + + # reset thruster targets to default values + if self._data.thrust_target is not None and self._data.default_thruster_rps is not None: + self._data.thrust_target[env_ids] = self._data.default_thruster_rps[env_ids] + + def write_data_to_sim(self): + """Write thrust and torque commands to the simulation.""" + self._apply_actuator_model() + # apply thruster forces at individual locations + self._apply_combined_wrench() + + def _initialize_impl(self): + """Initialize the multirotor implementation.""" + # call parent initialization + super()._initialize_impl() + + # Replace data container with MultirotorData + self._data = MultirotorData(self.root_physx_view, self.device) + + # Create thruster buffers with correct size (SINGLE PHASE) + self._create_thruster_buffers() + + # Process thruster configuration + self._process_thruster_cfg() + + # Process configuration + self._process_cfg() + + # Update the robot data + self.update(0.0) + + # Log multirotor information + self._log_multirotor_info() + + def _count_thrusters_from_config(self) -> int: + """Count total number of thrusters from actuator configuration. + + Returns: + Total number of thrusters across all actuator groups. + """ + total_thrusters = 0 + + for actuator_name, actuator_cfg in self.cfg.actuators.items(): + if not hasattr(actuator_cfg, "thruster_names_expr"): + continue + + # Use find_bodies to count thrusters for this actuator + body_indices, thruster_names = self.find_bodies(actuator_cfg.thruster_names_expr) + total_thrusters += len(body_indices) + + if total_thrusters == 0: + raise ValueError( + "No thrusters found in actuator configuration. " + "Please check thruster_names_expr in your MultirotorCfg.actuators." + ) + + return total_thrusters + + def _create_thruster_buffers(self): + """Create thruster buffers with correct size.""" + num_instances = self.num_instances + num_thrusters = self._count_thrusters_from_config() + + # Create thruster data tensors with correct size + self._data.default_thruster_rps = torch.zeros(num_instances, num_thrusters, device=self.device) + # thrust after controller and allocation is applied + self._data.thrust_target = torch.zeros(num_instances, num_thrusters, device=self.device) + self._data.computed_thrust = torch.zeros(num_instances, num_thrusters, device=self.device) + self._data.applied_thrust = torch.zeros(num_instances, num_thrusters, device=self.device) + + # Combined wrench buffers + self._thrust_target_sim = torch.zeros_like(self._data.thrust_target) # thrust after actuator model is applied + # wrench target for combined mode + self._internal_wrench_target_sim = torch.zeros(num_instances, 6, device=self.device) + # internal force/torque targets per body for combined mode + self._internal_force_target_sim = torch.zeros(num_instances, self.num_bodies, 3, device=self.device) + self._internal_torque_target_sim = torch.zeros(num_instances, self.num_bodies, 3, device=self.device) + + # Placeholder thruster names (will be filled during actuator creation) + self._data.thruster_names = [f"thruster_{i}" for i in range(num_thrusters)] + + def _process_actuators_cfg(self): + """Override parent method to do nothing - we handle thrusters separately.""" + # Do nothing - we handle thruster processing in _process_thruster_cfg() otherwise this + # gives issues with joint name expressions + pass + + def _process_cfg(self): + """Post processing of multirotor configuration parameters.""" + # Handle root state (like parent does) + default_root_state = ( + tuple(self.cfg.init_state.pos) + + tuple(self.cfg.init_state.rot) + + tuple(self.cfg.init_state.lin_vel) + + tuple(self.cfg.init_state.ang_vel) + ) + default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device) + self._data.default_root_state = default_root_state.repeat(self.num_instances, 1) + + # Handle thruster-specific initial state + if hasattr(self._data, "default_thruster_rps") and hasattr(self.cfg.init_state, "rps"): + # Match against thruster names + indices_list, _, values_list = string_utils.resolve_matching_names_values( + self.cfg.init_state.rps, self.thruster_names + ) + if indices_list: + rps_values = torch.tensor(values_list, device=self.device) + self._data.default_thruster_rps[:, indices_list] = rps_values + self._data.thrust_target[:, indices_list] = rps_values + + def _process_thruster_cfg(self): + """Process and apply multirotor thruster properties.""" + # create actuators + self.actuators = dict() + self._has_implicit_actuators = False + + # Check for mixed configurations (same as before) + has_thrusters = False + has_joints = False + + for actuator_name, actuator_cfg in self.cfg.actuators.items(): + if hasattr(actuator_cfg, "thruster_names_expr"): + has_thrusters = True + elif hasattr(actuator_cfg, "joint_names_expr"): + has_joints = True + + if has_thrusters and has_joints: + raise ValueError("Mixed configurations with both thrusters and regular joints are not supported.") + + if has_joints: + raise ValueError("Regular joint actuators are not supported in Multirotor class.") + + # Store the body-to-thruster mapping + self._thruster_body_mapping = {} + + # Track thruster names as we create actuators + all_thruster_names = [] + + for actuator_name, actuator_cfg in self.cfg.actuators.items(): + body_indices, thruster_names = self.find_bodies(actuator_cfg.thruster_names_expr) + + # Create 0-based thruster array indices starting from current count + start_idx = len(all_thruster_names) + thruster_array_indices = list(range(start_idx, start_idx + len(body_indices))) + + # Track all thruster names + all_thruster_names.extend(thruster_names) + + # Store the mapping + self._thruster_body_mapping[actuator_name] = { + "body_indices": body_indices, + "array_indices": thruster_array_indices, + "thruster_names": thruster_names, + } + + # Create thruster actuator + actuator: Thruster = actuator_cfg.class_type( + cfg=actuator_cfg, + thruster_names=thruster_names, + thruster_ids=thruster_array_indices, + num_envs=self.num_instances, + device=self.device, + init_thruster_rps=self._data.default_thruster_rps[:, thruster_array_indices], + ) + + # Store actuator + self.actuators[actuator_name] = actuator + + # Log information + omni.log.info( + f"Thruster actuator: {actuator_name} with model '{actuator_cfg.class_type.__name__}'" + f" (thruster names: {thruster_names} [{body_indices}])." + ) + + # Update thruster names in data container + self._data.thruster_names = all_thruster_names + + # Log summary + omni.log.info(f"Initialized {len(self.actuators)} thruster actuator(s) for multirotor.") + + def _apply_actuator_model(self): + """Processes thruster commands for the multirotor by forwarding them to the actuators. + + The actions are first processed using actuator models. The thruster actuator models + compute the thruster level simulation commands and sets them into the PhysX buffers. + """ + + # process thruster actions per group + for actuator in self.actuators.values(): + if not isinstance(actuator, Thruster): + continue + + # prepare input for actuator model based on cached data + control_action = ArticulationThrustActions( + thrusts=self._data.thrust_target[:, actuator.thruster_indices], + thruster_indices=actuator.thruster_indices, + ) + + # compute thruster command from the actuator model + control_action = actuator.compute(control_action) + + # update targets (these are set into the simulation) + if control_action.thrusts is not None: + self._thrust_target_sim[:, actuator.thruster_indices] = control_action.thrusts + + # update state of the actuator model + self._data.computed_thrust[:, actuator.thruster_indices] = actuator.computed_thrust + self._data.applied_thrust[:, actuator.thruster_indices] = actuator.applied_thrust + + def _apply_combined_wrench(self): + """Apply combined wrench to the base link (like articulation_with_thrusters.py).""" + # Combine individual thrusts into a wrench vector + self._combine_thrusts() + + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._internal_force_target_sim.view(-1, 3), # Shape: (num_envs * num_bodies, 3) + torque_data=self._internal_torque_target_sim.view(-1, 3), # Shape: (num_envs * num_bodies, 3) + position_data=None, # Apply at center of mass + indices=self._ALL_INDICES, + is_global=False, # Forces are in local frame + ) + + def _combine_thrusts(self): + """Combine individual thrusts into a wrench vector.""" + thrusts = self._thrust_target_sim + self._internal_wrench_target_sim = (self.allocation_matrix @ thrusts.T).T + # Apply forces to base link (body index 0) only + self._internal_force_target_sim[:, 0, :] = self._internal_wrench_target_sim[:, :3] + self._internal_torque_target_sim[:, 0, :] = self._internal_wrench_target_sim[:, 3:] + + def _validate_cfg(self): + """Validate the multirotor configuration after processing. + + Note: + This function should be called only after the configuration has been processed and the buffers have been + created. Otherwise, some settings that are altered during processing may not be validated. + """ + # Only validate if actuators have been created + if hasattr(self, "actuators") and self.actuators: + # Validate thruster-specific configuration + for actuator_name in self.actuators: + if isinstance(self.actuators[actuator_name], Thruster): + initial_thrust = self.actuators[actuator_name].curr_thrust + # check that the initial thrust is within the limits + thrust_limits = self.actuators[actuator_name].cfg.thrust_range + if torch.any(initial_thrust < thrust_limits[0]) or torch.any(initial_thrust > thrust_limits[1]): + raise ValueError( + f"Initial thrust for actuator '{actuator_name}' is out of bounds: " + f"{initial_thrust} not in {thrust_limits}" + ) + + def _log_multirotor_info(self): + """Log multirotor-specific information.""" + omni.log.info(f"Multirotor initialized with {self.num_thrusters} thrusters") + omni.log.info(f"Thruster names: {self.thruster_names}") + omni.log.info(f"Thruster force direction: {self.cfg.thruster_force_direction}") diff --git a/source/isaaclab/isaaclab/assets/articulation/multirotor_cfg.py b/source/isaaclab/isaaclab/assets/articulation/multirotor_cfg.py new file mode 100644 index 00000000000..f0e022187d6 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/articulation/multirotor_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, 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 dataclasses import MISSING + +from isaaclab.actuators import ThrusterCfg +from isaaclab.utils import configclass + +from .articulation_cfg import ArticulationCfg +from .multirotor import Multirotor + + +@configclass +class MultirotorCfg(ArticulationCfg): + """Configuration parameters for a multirotor articulation. + + This extends the base articulation configuration to support multirotor-specific + settings. + """ + + class_type: type = Multirotor + + @configclass + class InitialStateCfg(ArticulationCfg.InitialStateCfg): + """Initial state of the multirotor articulation.""" + + # multirotor-specific initial state + rps: dict[str, float] = {".*": 100.0} + """RPS of the thrusters. Defaults to 100.0 for all thrusters.""" + + # multirotor-specific configuration + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the multirotor object.""" + + actuators: dict[str, ThrusterCfg] = MISSING + """Thruster actuators for the multirotor with corresponding thruster names.""" + + # multirotor force application settings + thruster_force_direction: tuple[float, float, float] = (0.0, 0.0, 1.0) + """Default force direction in body-local frame for thrusters. Defaults to Z-axis (upward).""" + + allocation_matrix: list[list[float]] | None = None + """allocation matrix for control allocation""" + + rotor_directions: list[int] | None = None + """List of rotor directions, -1 for clockwise, 1 for counter-clockwise.""" diff --git a/source/isaaclab/isaaclab/assets/articulation/multirotor_data.py b/source/isaaclab/isaaclab/assets/articulation/multirotor_data.py new file mode 100644 index 00000000000..b9443bf4fcb --- /dev/null +++ b/source/isaaclab/isaaclab/assets/articulation/multirotor_data.py @@ -0,0 +1,50 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch + +from isaaclab.assets.articulation.articulation_data import ArticulationData + + +class MultirotorData(ArticulationData): + """Data container for a multirotor articulation. + + This class extends the base articulation data container to include multirotor-specific + data such as thruster states and forces. + """ + + thruster_names: list[str] = None + """List of thruster names in the multirotor.""" + + default_thruster_rps: torch.Tensor = None + """Default thruster RPS state of all thrusters. Shape is (num_instances, num_thrusters). + + This quantity is configured through the :attr:`isaaclab.assets.MultirotorCfg.init_state` parameter. + """ + + thrust_target: torch.Tensor = None + """Thrust targets commanded by the user. Shape is (num_instances, num_thrusters). + + This quantity contains the target thrust values set by the user through the + :meth:`isaaclab.assets.Multirotor.set_thrust_target` method. + """ + + ## + # Thruster commands + ## + + computed_thrust: torch.Tensor = None + """Computed thrust from the actuator model (before clipping). Shape is (num_instances, num_thrusters). + + This quantity contains the thrust values computed by the thruster actuator models + before any clipping or saturation is applied. + """ + + applied_thrust: torch.Tensor = None + """Applied thrust from the actuator model (after clipping). Shape is (num_instances, num_thrusters). + + This quantity contains the final thrust values that are applied to the simulation + after all actuator model processing, including clipping and saturation. + """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py index 19a84846a52..616141fbaa3 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/actions_cfg.py @@ -16,6 +16,7 @@ non_holonomic_actions, surface_gripper_actions, task_space_actions, + thrust_actions, ) ## @@ -374,3 +375,38 @@ class SurfaceGripperBinaryActionCfg(ActionTermCfg): """The command value to close the gripper. Defaults to 1.0.""" class_type: type[ActionTerm] = surface_gripper_actions.SurfaceGripperBinaryAction + + +## +# Drone actions. +## + + +@configclass +class ThrustActionCfg(ActionTermCfg): + """Configuration for the thrust action term. + + See :class:`ThrustAction` for more details. + """ + + class_type: type[ActionTerm] = thrust_actions.ThrustAction + + asset_name: str = MISSING + """Name or regex expression of the asset that the action will be mapped to.""" + + scale: float | dict[str, float] = 1.0 + """Scale factor for the action (float or dict of regex expressions). Defaults to 1.0.""" + + offset: float | dict[str, float] = 0.0 + """Offset factor for the action (float or dict of regex expressions). Defaults to 0.0.""" + + preserve_order: bool = False + """Whether to preserve the order of the asset names in the action output. Defaults to False.""" + + use_default_offset: bool = True + """Whether to use default thrust (e.g. hover thrust) configured in the articulation asset as offset. + Defaults to True. + + If True, this flag results in overwriting the values of :attr:`offset` to the default thrust values + from the articulation asset. + """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/thrust_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/thrust_actions.py new file mode 100644 index 00000000000..e0edac00d2e --- /dev/null +++ b/source/isaaclab/isaaclab/envs/mdp/actions/thrust_actions.py @@ -0,0 +1,159 @@ +# Copyright (c) 2022-2025, 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 + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Multirotor +from isaaclab.managers.action_manager import ActionTerm + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor + + from . import actions_cfg + + +class ThrustAction(ActionTerm): + """Thrust action term that applies the processed actions as thrust commands.""" + + cfg: actions_cfg.ThrustActionCfg + """The configuration of the action term.""" + _asset: Multirotor + """The articulation asset on which the action term is applied.""" + _scale: torch.Tensor | float + """The scaling factor applied to the input action.""" + _offset: torch.Tensor | float + """The offset applied to the input action.""" + _clip: torch.Tensor + """The clip applied to the input action.""" + + def __init__(self, cfg: actions_cfg.ThrustActionCfg, env: ManagerBasedEnv) -> None: + # initialize the action term + super().__init__(cfg, env) + + thruster_names_expr = self._asset.actuators["thrusters"].cfg.thruster_names_expr + + # resolve the thrusters over which the action term is applied + self._thruster_ids, self._thruster_names = self._asset.find_bodies( + thruster_names_expr, preserve_order=self.cfg.preserve_order + ) + self._num_thrusters = len(self._thruster_ids) + # log the resolved thruster names for debugging + omni.log.info( + f"Resolved thruster names for the action term {self.__class__.__name__}:" + f" {self._thruster_names} [{self._thruster_ids}]" + ) + + # Avoid indexing across all thrusters for efficiency + if self._num_thrusters == self._asset.num_thrusters and not self.cfg.preserve_order: + self._thruster_ids = slice(None) + + # create tensors for raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self.raw_actions) + + # parse scale + if isinstance(cfg.scale, (float, int)): + self._scale = float(cfg.scale) + elif isinstance(cfg.scale, dict): + self._scale = torch.ones(self.num_envs, self.action_dim, device=self.device) + # resolve the dictionary config + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.scale, self._thruster_names) + self._scale[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported scale type: {type(cfg.scale)}. Supported types are float and dict.") + + # parse offset + if isinstance(cfg.offset, (float, int)): + self._offset = float(cfg.offset) + elif isinstance(cfg.offset, dict): + self._offset = torch.zeros_like(self._raw_actions) + # resolve the dictionary config + index_list, _, value_list = string_utils.resolve_matching_names_values( + self.cfg.offset, self._thruster_names + ) + self._offset[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported offset type: {type(cfg.offset)}. Supported types are float and dict.") + + # parse clip + if cfg.clip is not None: + if isinstance(cfg.clip, dict): + self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( + self.num_envs, self.action_dim, 1 + ) + index_list, _, value_list = string_utils.resolve_matching_names_values( + self.cfg.clip, self._thruster_names + ) + self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + + # Handle use_default_offset + if cfg.use_default_offset: + # Use default thruster RPS as offset + self._offset = self._asset.data.default_thruster_rps[:, self._thruster_ids].clone() + + @property + def action_dim(self) -> int: + return self._num_thrusters + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term.""" + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "ThrustAction" + self._IO_descriptor.thruster_names = self._thruster_names + self._IO_descriptor.scale = self._scale + if isinstance(self._offset, torch.Tensor): + self._IO_descriptor.offset = self._offset[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.offset = self._offset + if self.cfg.clip is not None: + if isinstance(self._clip, torch.Tensor): + self._IO_descriptor.clip = self._clip[0].detach().cpu().numpy().tolist() + else: + self._IO_descriptor.clip = self._clip + else: + self._IO_descriptor.clip = None + return self._IO_descriptor + + def process_actions(self, actions: torch.Tensor): + """Process actions by applying scaling, offset, and clipping.""" + # store the raw actions + self._raw_actions[:] = actions + # apply the affine transformations + self._processed_actions = self._raw_actions * self._scale + self._offset + # clip actions + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + + def apply_actions(self): + """Apply the processed actions as thrust commands.""" + # Set thrust targets using thruster IDs + self._asset.set_thrust_target(self.processed_actions, thruster_ids=self._thruster_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Reset the action term.""" + self._raw_actions[env_ids] = 0.0 diff --git a/source/isaaclab/isaaclab/utils/types.py b/source/isaaclab/isaaclab/utils/types.py index aa6f1fbfd45..8d67059b6a6 100644 --- a/source/isaaclab/isaaclab/utils/types.py +++ b/source/isaaclab/isaaclab/utils/types.py @@ -37,3 +37,24 @@ class ArticulationActions: If the joint indices are a slice, this indicates that the indices are continuous and correspond to all the joints of the articulation. We use a slice to make the indexing more efficient. """ + + +@dataclass +class ArticulationThrustActions: + """Data container to store articulation's thruster actions. + + This class is used to store the actions of the thrusters of an articulation. + It is used to store the thrust values and indices. + + If the actions are not provided, the values are set to None. + """ + + thrusts: torch.Tensor | None = None + """The thrusts of the articulation. Defaults to None.""" + + thruster_indices: torch.Tensor | Sequence[int] | slice | None = None + """The thruster indices of the articulation. Defaults to None. + + If the thruster indices are a slice, this indicates that the indices are continuous and correspond + to all the thrusters of the articulation. We use a slice to make the indexing more efficient. + """ diff --git a/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py b/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py new file mode 100644 index 00000000000..7e4e87810e5 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the ARL robots. + +The following configuration parameters are available: + +* :obj:`ARL_ROBOT_1_CFG`: The ARL_Robot_1 with (TODO add motor propeller combination) +""" + +import isaaclab.sim as sim_utils +from isaaclab import ISAACLAB_EXT_DIR +from isaaclab.actuators import ThrusterCfg +from isaaclab.assets.articulation import MultirotorCfg + +## +# Configuration - Actuators. +## + +ARL_ROBOT_1_THRUSTER = ThrusterCfg( + thrust_range=(0.1, 10.0), + thrust_const_range=(9.26312e-06, 1.826312e-05), + tau_inc_range=(0.05, 0.08), + tau_dec_range=(0.005, 0.005), + torque_to_thrust_ratio=0.07, + thruster_names_expr=["back_left_prop", "back_right_prop", "front_left_prop", "front_right_prop"], +) + +## +# Configuration - Articulation. +## + +ARL_ROBOT_1_CFG = MultirotorCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_EXT_DIR}/../isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/robot_model/arl_robot_1/arl_robot_1.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + ), + init_state=MultirotorCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.0), + lin_vel=(0.0, 0.0, 0.0), + ang_vel=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + rps={ + "back_left_prop": 200.0, + "back_right_prop": 200.0, + "front_left_prop": 200.0, + "front_right_prop": 200.0, + }, + ), + actuators={"thrusters": ARL_ROBOT_1_THRUSTER}, + rotor_directions=[1, -1, 1, -1], + allocation_matrix=[ + [0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0], + [1.0, 1.0, 1.0, 1.0], + [-0.13, -0.13, 0.13, 0.13], + [-0.13, 0.13, 0.13, -0.13], + [-0.07, 0.07, -0.07, 0.07], + ], +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/__init__.py new file mode 100644 index 00000000000..ee75c13ca1f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Drone ARL environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.py new file mode 100644 index 00000000000..f39ee61f652 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the drone ARL environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .commands import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/__init__.py new file mode 100644 index 00000000000..f47a4f597d1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Various command terms that can be used in the environment.""" + +from .commands_cfg import DroneUniformPoseCommandCfg +from .drone_pose_command import DroneUniformPoseCommand diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/commands_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/commands_cfg.py new file mode 100644 index 00000000000..5de9fab3eea --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/commands_cfg.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, 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 isaaclab.envs.mdp.commands.commands_cfg import UniformPoseCommandCfg +from isaaclab.utils import configclass + +from .drone_pose_command import DroneUniformPoseCommand + + +@configclass +class DroneUniformPoseCommandCfg(UniformPoseCommandCfg): + """Configuration for uniform drone pose command generator.""" + + class_type: type = DroneUniformPoseCommand diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py new file mode 100644 index 00000000000..67103a8d37b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py @@ -0,0 +1,67 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command generators for pose tracking.""" + +from __future__ import annotations + +import torch + +from isaaclab.envs.mdp.commands.pose_command import UniformPoseCommand +from isaaclab.utils.math import combine_frame_transforms, compute_pose_error + + +class DroneUniformPoseCommand(UniformPoseCommand): + """Drone-specific UniformPoseCommand extensions. + + This class customizes the generic :class:`UniformPoseCommand` for drone (multirotor) + use-cases. Main differences and additions: + + - Transforms pose commands from the drone's base frame to the world frame before use. + - Accounts for per-environment origin offsets (``scene.env_origins``) when computing + position errors so tasks running on shifted/sub-terrain environments compute + meaningful errors. + - Computes and exposes simple metrics used by higher-level code: ``position_error`` + and ``orientation_error`` (stored in ``self.metrics``). + - Provides a debug visualization callback that renders the goal pose (with + sub-terrain shift) and current body pose using the existing visualizers. + + The implementation overrides :meth:`_update_metrics` and :meth:`_debug_vis_callback` + from the base class to implement these drone-specific behaviors. + """ + + def _update_metrics(self): + # transform command from base frame to simulation world frame + self.pose_command_w[:, :3], self.pose_command_w[:, 3:] = combine_frame_transforms( + self.robot.data.root_pos_w, + self.robot.data.root_quat_w, + self.pose_command_b[:, :3], + self.pose_command_b[:, 3:], + ) + # compute the error + pos_error, rot_error = compute_pose_error( + # Sub-terrain shift for correct position error calculation @grzemal + self.pose_command_b[:, :3] + self._env.scene.env_origins, + self.pose_command_w[:, 3:], + self.robot.data.body_pos_w[:, self.body_idx], + self.robot.data.body_quat_w[:, self.body_idx], + ) + self.metrics["position_error"] = torch.norm(pos_error, dim=-1) + self.metrics["orientation_error"] = torch.norm(rot_error, dim=-1) + + def _debug_vis_callback(self, event): + # check if robot is initialized + # note: this is needed in-case the robot is de-initialized. we can't access the data + if not self.robot.is_initialized: + return + # update the markers + # -- goal pose + # Sub-terrain shift for visualization purposes @grzemal + self.goal_pose_visualizer.visualize( + self.pose_command_b[:, :3] + self._env.scene.env_origins, self.pose_command_b[:, 3:] + ) + # -- current body pose + body_link_pose_w = self.robot.data.body_link_pose_w[:, self.body_idx] + self.current_pose_visualizer.visualize(body_link_pose_w[:, :3], body_link_pose_w[:, 3:7]) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py new file mode 100644 index 00000000000..ad467d6caf6 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to create drone observation terms. + +The functions can be passed to the :class:`isaaclab.managers.ObservationTermCfg` object to enable +the observation introduced by the function. +""" + +from __future__ import annotations + +import torch +import torch.jit +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, Multirotor +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv + +from isaaclab.envs.utils.io_descriptors import generic_io_descriptor, record_shape + +""" +State. +""" + + +def base_roll_pitch(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Roll and pitch of the base in the simulation world frame.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # extract euler angles (in world frame) + roll, pitch, _ = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) + # normalize angle to [-pi, pi] + roll = math_utils.wrap_to_pi(roll) + pitch = math_utils.wrap_to_pi(pitch) + + return torch.cat((roll.unsqueeze(-1), pitch.unsqueeze(-1)), dim=-1) + + +""" +Commands. +""" + + +@generic_io_descriptor(dtype=torch.float32, observation_type="Command", on_inspect=[record_shape]) +def generated_drone_commands( + env: ManagerBasedRLEnv, command_name: str | None = None, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """The generated command from command term in the command manager with the given name.""" + asset: Multirotor = env.scene[asset_cfg.name] + current_position_w = asset.data.root_pos_w - env.scene.env_origins + command = env.command_manager.get_command(command_name) + current_position_b = math_utils.quat_apply_inverse(asset.data.root_link_quat_w, command[:, :3] - current_position_w) + current_position_b_dir = current_position_b / (torch.norm(current_position_b, dim=-1, keepdim=True) + 1e-8) + current_position_b_mag = torch.norm(current_position_b, dim=-1, keepdim=True) + return torch.cat((current_position_b_dir, current_position_b_mag), dim=-1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py new file mode 100644 index 00000000000..1a489feabf8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py @@ -0,0 +1,146 @@ +# Copyright (c) 2022-2025, 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 + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +""" +Drone control rewards. +""" + + +def distance_to_goal_exp( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + std: float = 1.0, + command_name: str = "target_pose", +) -> torch.Tensor: + """Reward the distance to a goal position using an exponential kernel. + + This reward computes an exponential falloff of the squared Euclidean distance + between the commanded target position and the asset (robot) root position. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel; larger values + produce a gentler falloff. + command_name: Name of the command to read the target pose from the + environment's command manager. The function expects the command + tensor to contain positions in its first three columns. + + Returns: + A 1-D tensor of shape (num_envs,) containing the per-environment reward + values in [0, 1], with 1.0 when the position error is zero. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + + target_position_w = command[:, :3].clone() + current_position = asset.data.root_pos_w - env.scene.env_origins + + # compute the error + position_error_square = torch.sum(torch.square(target_position_w - current_position), dim=1) + return torch.exp(-position_error_square / std**2) + + +def ang_vel_xyz_exp( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 1.0 +) -> torch.Tensor: + """Penalize angular velocity magnitude with an exponential kernel. + + This reward computes exp(-||omega||^2 / std^2) where omega is the body-frame + angular velocity of the asset. It is useful for encouraging low rotational + rates. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel; controls + sensitivity to angular velocity magnitude. + + Returns: + A 1-D tensor of shape (num_envs,) with values in (0, 1], where 1 indicates + zero angular velocity. + """ + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # compute squared magnitude of angular velocity (all axes) + ang_vel_squared = torch.sum(torch.square(asset.data.root_ang_vel_b), dim=1) + + return torch.exp(-ang_vel_squared / std**2) + + +def lin_vel_xyz_exp( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 1.0 +) -> torch.Tensor: + """Penalize linear velocity magnitude with an exponential kernel. + + Computes exp(-||v||^2 / std^2) where v is the asset's linear velocity in + world frame. Useful for encouraging the agent to reduce translational speed. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel. + + Returns: + A 1-D tensor of shape (num_envs,) with values in (0, 1], where 1 indicates + zero linear velocity. + """ + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # compute squared magnitude of linear velocity (all axes) + lin_vel_squared = torch.sum(torch.square(asset.data.root_lin_vel_w), dim=1) + + return torch.exp(-lin_vel_squared / std**2) + + +def yaw_aligned( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 0.5 +) -> torch.Tensor: + """Reward alignment of the vehicle's yaw to zero using an exponential kernel. + + The function extracts the yaw (rotation about Z) from the world-frame root + quaternion and computes exp(-yaw^2 / std^2). This encourages heading + alignment to a zero-yaw reference. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel; smaller values + make the reward more sensitive to yaw deviations. + + Returns: + A 1-D tensor of shape (num_envs,) with values in (0, 1], where 1 indicates + perfect yaw alignment (yaw == 0). + """ + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # extract yaw from current orientation + _, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) + + # normalize yaw to [-pi, pi] (target is 0) + yaw = math_utils.wrap_to_pi(yaw) + + # return exponential reward (1 when yaw=0, approaching 0 when rotated) + return torch.exp(-(yaw**2) / std**2) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/robot_model b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/robot_model new file mode 160000 index 00000000000..f53d8e81450 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/robot_model @@ -0,0 +1 @@ +Subproject commit f53d8e814503b44c379ba72ff5e1f691b48a4c28 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/__init__.py new file mode 100644 index 00000000000..9a37793ea66 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Drone ARL state-based control environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/__init__.py new file mode 100644 index 00000000000..601fefecac2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for state-based control environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/__init__.py new file mode 100644 index 00000000000..1657ec3061f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/__init__.py @@ -0,0 +1,36 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-StateBasedControl-ARL-robot-1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.empty_env_cfg:EmptyEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:StateBasedControlEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-StateBasedControl-ARL-robot-1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.empty_env_cfg:EmptyEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:StateBasedControlEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/__init__.py new file mode 100644 index 00000000000..2e924fbf1b1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..b23c75e2816 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,87 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256,128,64] + d2rl: False + activation: elu + initializer: + name: default + scale: 2 + rnn: + name: gru + units: 64 + layers: 1 + # before_mlp: False + # layer_norm: True + config: + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + env_config: + num_envs: 8192 + + name: arl_robot_1_state_based_control + reward_shaper: + # min_val: -1 + scale_value: 0.1 + + normalize_advantage: True + gamma: 0.98 + tau: 0.95 + ppo: True + learning_rate: 1e-4 + lr_schedule: adaptive + kl_threshold: 0.016 + save_best_after: 10 + score_to_win: 100000 + grad_norm: 1.0 + entropy_coef: 0 + truncate_grads: True + e_clip: 0.2 + clip_value: False + num_actors: 1024 + horizon_length: 32 + minibatch_size: 2048 + mini_epochs: 4 + critic_coef: 2 + normalize_input: True + bounds_loss_coef: 0.0001 + max_epochs: 1500 + normalize_value: True + use_diagnostics: True + value_bootstrap: True + #weight_decay: 0.0001 + use_smooth_clamp: False + + player: + render: True + deterministic: True + games_num: 100000 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..b4fab7d138a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2025, 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 isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class StateBasedControlEnvPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "arl_robot_1_state_based_control" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=0.5, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.001, + num_learning_epochs=4, + num_mini_batches=4, + learning_rate=4.0e-4, + schedule="adaptive", + gamma=0.98, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..1c4b131d0c5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,95 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: mlp + input: STATES + layers: [256, 128, 64] + activations: elu + - name: gru + input: mlp + type: GRU + layers: [64] + num_layers: 1 + output: ACTIONS + value: + class: DeterministicMixin + clip_actions: False + network: + - name: mlp + input: STATES + layers: [256, 128, 64] + activations: elu + - name: gru + input: mlp + type: GRU + layers: [64] + num_layers: 1 + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "arl_robot_1_state_based_control" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/empty_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/empty_env_cfg.py new file mode 100644 index 00000000000..e9cb2a98218 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/empty_env_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2025, 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 isaaclab_assets.robots.arl_robot_1 import ARL_ROBOT_1_CFG + +from isaaclab.utils import configclass + +from .state_based_control_env_cfg import StateBasedControlEmptyEnvCfg + +## +# Pre-defined configs +## + + +@configclass +class EmptyEnvCfg(StateBasedControlEmptyEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # switch robot to arl_robot_1 + self.scene.robot = ARL_ROBOT_1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.actuators["thrusters"].dt = self.sim.dt + + +@configclass +class EmptyEnvCfg_PLAY(EmptyEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/state_based_control_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/state_based_control_env_cfg.py new file mode 100644 index 00000000000..cfc6a8d0bde --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/state_based_control/config/arl_robot_1/state_based_control_env_cfg.py @@ -0,0 +1,229 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg, MultirotorCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg + +# from isaaclab.sim import PinholeCameraCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.drone_arl.mdp as mdp + + +## +# Scene definition +## +@configclass +class MySceneCfg(InteractiveSceneCfg): + """Configuration for the terrain scene with a flying robot.""" + + # robots + robot: MultirotorCfg = MISSING + + # lights + sky_light = AssetBaseCfg( + prim_path="/World/skyLight", + spawn=sim_utils.DomeLightCfg( + intensity=750.0, + texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr", + ), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command specifications for the MDP.""" + + target_pose = mdp.DroneUniformPoseCommandCfg( + asset_name="robot", + body_name="base_link", + resampling_time_range=(10.0, 10.0), + debug_vis=True, + ranges=mdp.DroneUniformPoseCommandCfg.Ranges( + pos_x=(-0.0, 0.0), + pos_y=(-0.0, 0.0), + pos_z=(-0.0, 0.0), + roll=(-0.0, 0.0), + pitch=(-0.0, 0.0), + yaw=(-0.0, 0.0), + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + thrust_command = mdp.ThrustActionCfg( + asset_name="robot", + scale=3.0, + offset=3.0, + preserve_order=False, + use_default_offset=False, + clip={ + "back_left_prop": (0.0, 6.0), + "back_right_prop": (0.0, 6.0), + "front_left_prop": (0.0, 6.0), + "front_right_prop": (0.0, 6.0), + }, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + base_link_position = ObsTerm(func=mdp.root_pos_w, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_orientation = ObsTerm(func=mdp.root_quat_w, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) + last_action = ObsTerm(func=mdp.last_action, noise=Unoise(n_min=-0.0, n_max=0.0)) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + # reset + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": (-1.0, 1.0), + "y": (-1.0, 1.0), + "z": (-1.0, 1.0), + "yaw": (-math.pi / 6.0, math.pi / 6.0), + "roll": (-math.pi / 6.0, math.pi / 6.0), + "pitch": (-math.pi / 6.0, math.pi / 6.0), + }, + "velocity_range": { + "x": (-0.2, 0.2), + "y": (-0.2, 0.2), + "z": (-0.2, 0.2), + "roll": (-0.2, 0.2), + "pitch": (-0.2, 0.2), + "yaw": (-0.2, 0.2), + }, + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + distance_to_goal_exp = RewTerm( + func=mdp.distance_to_goal_exp, + weight=25.0, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "std": 1.5, + "command_name": "target_pose", + }, + ) + flat_orientation_l2 = RewTerm( + func=mdp.flat_orientation_l2, + weight=1.0, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + yaw_aligned = RewTerm( + func=mdp.yaw_aligned, + weight=2.0, + params={"asset_cfg": SceneEntityCfg("robot"), "std": 1.0}, + ) + lin_vel_xyz_exp = RewTerm( + func=mdp.lin_vel_xyz_exp, + weight=2.5, + params={"asset_cfg": SceneEntityCfg("robot"), "std": 2.0}, + ) + ang_vel_xyz_exp = RewTerm( + func=mdp.ang_vel_xyz_exp, + weight=10.0, + params={"asset_cfg": SceneEntityCfg("robot"), "std": 10.0}, + ) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.05) + action_magnitude_l2 = RewTerm(func=mdp.action_l2, weight=-0.05) + + termination_penalty = RewTerm( + func=mdp.is_terminated, + weight=-5.0, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + crash = DoneTerm(func=mdp.root_height_below_minimum, params={"minimum_height": -3.0}) + + +## +# Environment configuration +## + + +@configclass +class StateBasedControlEmptyEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the state-based drone pose-control environment.""" + + # Scene settings + scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 10 + self.episode_length_s = 5.0 + # simulation settings + self.sim.dt = 0.01 + self.sim.render_interval = self.decimation + self.sim.physics_material = sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ) + self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15