From 0dfbf47f22a4cd644a02a9e1dbe5963d7ccaf5f2 Mon Sep 17 00:00:00 2001 From: Brian McCann Date: Mon, 10 Nov 2025 15:12:38 -0500 Subject: [PATCH 1/2] Refactor Actuator Configs to avoid circular import. --- CONTRIBUTORS.md | 1 + source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 9 + .../isaaclab/isaaclab/actuators/__init__.py | 11 +- .../isaaclab/actuators/actuator_base.py | 2 +- .../isaaclab/actuators/actuator_base_cfg.py | 165 ++++++++++++++++++ .../isaaclab/actuators/actuator_net.py | 2 +- .../isaaclab/actuators/actuator_net_cfg.py | 64 +++++++ .../isaaclab/actuators/actuator_pd.py | 36 +--- .../isaaclab/actuators/actuator_pd_cfg.py | 81 +++++++++ .../isaaclab_assets/robots/allegro.py | 2 +- .../isaaclab_assets/robots/kuka_allegro.py | 2 +- .../isaaclab_assets/robots/shadow_hand.py | 2 +- .../direct/automate/assembly_env_cfg.py | 2 +- .../direct/automate/disassembly_env_cfg.py | 2 +- .../direct/factory/factory_env_cfg.py | 2 +- .../franka_cabinet/franka_cabinet_env.py | 2 +- .../manipulation/cabinet/cabinet_env_cfg.py | 2 +- 18 files changed, 342 insertions(+), 47 deletions(-) create mode 100644 source/isaaclab/isaaclab/actuators/actuator_base_cfg.py create mode 100644 source/isaaclab/isaaclab/actuators/actuator_net_cfg.py create mode 100644 source/isaaclab/isaaclab/actuators/actuator_pd_cfg.py diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 8491f2647a4..3fb26cde1dd 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -35,6 +35,7 @@ Guidelines for modifications: * Pascal Roth * Sheikh Dawood * Ossama Ahmed +* Brian McCann ## Contributors diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index f33f3f354b6..0f69b167d74 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.48.0" +version = "0.48.1" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 28dc76731f8..043c1776187 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,15 @@ Changelog --------- +0.48.1 (2025-11-10) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Refactored modules related to the actuator configs in order to remediate a circular import necessary to support future + actuator drive model improvements. + 0.48.0 (2025-11-03) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/actuators/__init__.py b/source/isaaclab/isaaclab/actuators/__init__.py index 5ccf5d7b082..2e9f5e05c71 100644 --- a/source/isaaclab/isaaclab/actuators/__init__.py +++ b/source/isaaclab/isaaclab/actuators/__init__.py @@ -23,15 +23,14 @@ """ from .actuator_base import ActuatorBase -from .actuator_cfg import ( - ActuatorBaseCfg, - ActuatorNetLSTMCfg, - ActuatorNetMLPCfg, +from .actuator_base_cfg import ActuatorBaseCfg +from .actuator_net import ActuatorNetLSTM, ActuatorNetMLP +from .actuator_net_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg +from .actuator_pd import DCMotor, DelayedPDActuator, IdealPDActuator, ImplicitActuator, RemotizedPDActuator +from .actuator_pd_cfg import ( DCMotorCfg, DelayedPDActuatorCfg, IdealPDActuatorCfg, ImplicitActuatorCfg, RemotizedPDActuatorCfg, ) -from .actuator_net import ActuatorNetLSTM, ActuatorNetMLP -from .actuator_pd import DCMotor, DelayedPDActuator, IdealPDActuator, ImplicitActuator, RemotizedPDActuator diff --git a/source/isaaclab/isaaclab/actuators/actuator_base.py b/source/isaaclab/isaaclab/actuators/actuator_base.py index 3a2333bff77..b0517fc4ef6 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_base.py +++ b/source/isaaclab/isaaclab/actuators/actuator_base.py @@ -14,7 +14,7 @@ from isaaclab.utils.types import ArticulationActions if TYPE_CHECKING: - from .actuator_cfg import ActuatorBaseCfg + from .actuator_base_cfg import ActuatorBaseCfg class ActuatorBase(ABC): diff --git a/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py new file mode 100644 index 00000000000..fb4697e4025 --- /dev/null +++ b/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py @@ -0,0 +1,165 @@ +# 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.utils import configclass + + +@configclass +class ActuatorBaseCfg: + """Configuration for default actuators in an articulation.""" + + class_type: type = MISSING + """The associated actuator class. + + The class should inherit from :class:`isaaclab.actuators.ActuatorBase`. + """ + + joint_names_expr: list[str] = MISSING + """Articulation's joint names that are part of the group. + + Note: + This can be a list of joint names or a list of regex expressions (e.g. ".*"). + """ + + effort_limit: dict[str, float] | float | None = None + """Force/Torque limit of the joints in the group. Defaults to None. + + This limit is used to clip the computed torque sent to the simulation. If None, the + limit is set to the value specified in the USD joint prim. + + .. attention:: + + The :attr:`effort_limit_sim` attribute should be used to set the effort limit for + the simulation physics solver. + + The :attr:`effort_limit` attribute is used for clipping the effort output of the + actuator model **only** in the case of explicit actuators, such as the + :class:`~isaaclab.actuators.IdealPDActuator`. + + .. note:: + + For implicit actuators, the attributes :attr:`effort_limit` and :attr:`effort_limit_sim` + are equivalent. However, we suggest using the :attr:`effort_limit_sim` attribute because + it is more intuitive. + + """ + + velocity_limit: dict[str, float] | float | None = None + """Velocity limit of the joints in the group. Defaults to None. + + This limit is used by the actuator model. If None, the limit is set to the value specified + in the USD joint prim. + + .. attention:: + + The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for + the simulation physics solver. + + The :attr:`velocity_limit` attribute is used for clipping the effort output of the + actuator model **only** in the case of explicit actuators, such as the + :class:`~isaaclab.actuators.IdealPDActuator`. + + .. note:: + + For implicit actuators, the attribute :attr:`velocity_limit` is not used. This is to stay + backwards compatible with previous versions of the Isaac Lab, where this parameter was + unused since PhysX did not support setting the velocity limit for the joints using the + PhysX Tensor API. + """ + + effort_limit_sim: dict[str, float] | float | None = None + """Effort limit of the joints in the group applied to the simulation physics solver. Defaults to None. + + The effort limit is used to constrain the computed joint efforts in the physics engine. If the + computed effort exceeds this limit, the physics engine will clip the effort to this value. + + Since explicit actuators (e.g. DC motor), compute and clip the effort in the actuator model, this + limit is by default set to a large value to prevent the physics engine from any additional clipping. + However, at times, it may be necessary to set this limit to a smaller value as a safety measure. + + If None, the limit is resolved based on the type of actuator model: + + * For implicit actuators, the limit is set to the value specified in the USD joint prim. + * For explicit actuators, the limit is set to 1.0e9. + + """ + + velocity_limit_sim: dict[str, float] | float | None = None + """Velocity limit of the joints in the group applied to the simulation physics solver. Defaults to None. + + The velocity limit is used to constrain the joint velocities in the physics engine. The joint will only + be able to reach this velocity if the joint's effort limit is sufficiently large. If the joint is moving + faster than this velocity, the physics engine will actually try to brake the joint to reach this velocity. + + If None, the limit is set to the value specified in the USD joint prim for both implicit and explicit actuators. + + .. tip:: + If the velocity limit is too tight, the physics engine may have trouble converging to a solution. + In such cases, we recommend either keeping this value sufficiently large or tuning the stiffness and + damping parameters of the joint to ensure the limits are not violated. + + """ + + stiffness: dict[str, float] | float | None = MISSING + """Stiffness gains (also known as p-gain) of the joints in the group. + + The behavior of the stiffness is different for implicit and explicit actuators. For implicit actuators, + the stiffness gets set into the physics engine directly. For explicit actuators, the stiffness is used + by the actuator model to compute the joint efforts. + + If None, the stiffness is set to the value from the USD joint prim. + """ + + damping: dict[str, float] | float | None = MISSING + """Damping gains (also known as d-gain) of the joints in the group. + + The behavior of the damping is different for implicit and explicit actuators. For implicit actuators, + the damping gets set into the physics engine directly. For explicit actuators, the damping gain is used + by the actuator model to compute the joint efforts. + + If None, the damping is set to the value from the USD joint prim. + """ + + armature: dict[str, float] | float | None = None + """Armature of the joints in the group. Defaults to None. + + The armature is directly added to the corresponding joint-space inertia. It helps improve the + simulation stability by reducing the joint velocities. + + It is a physics engine solver parameter that gets set into the simulation. + + If None, the armature is set to the value from the USD joint prim. + """ + + friction: dict[str, float] | float | None = None + r"""The static friction coefficient of the joints in the group. Defaults to None. + + The joint static friction is a unitless quantity. It relates the magnitude of the spatial force transmitted + from the parent body to the child body to the maximal static friction force that may be applied by the solver + to resist the joint motion. + + Mathematically, this means that: :math:`F_{resist} \leq \mu F_{spatial}`, where :math:`F_{resist}` + is the resisting force applied by the solver and :math:`F_{spatial}` is the spatial force + transmitted from the parent body to the child body. The simulated static friction effect is therefore + similar to static and Coulomb static friction. + + If None, the joint static friction is set to the value from the USD joint prim. + + Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + dynamic_friction: dict[str, float] | float | None = None + """The dynamic friction coefficient of the joints in the group. Defaults to None. + + Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, + it is modeled as an effort (torque or force). + """ + + viscous_friction: dict[str, float] | float | None = None + """The viscous friction coefficient of the joints in the group. Defaults to None. + """ diff --git a/source/isaaclab/isaaclab/actuators/actuator_net.py b/source/isaaclab/isaaclab/actuators/actuator_net.py index 9b4b1641f2c..fc40261c774 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_net.py +++ b/source/isaaclab/isaaclab/actuators/actuator_net.py @@ -24,7 +24,7 @@ from .actuator_pd import DCMotor if TYPE_CHECKING: - from .actuator_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg + from .actuator_net_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg class ActuatorNetLSTM(DCMotor): diff --git a/source/isaaclab/isaaclab/actuators/actuator_net_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_net_cfg.py new file mode 100644 index 00000000000..9035cf8ab78 --- /dev/null +++ b/source/isaaclab/isaaclab/actuators/actuator_net_cfg.py @@ -0,0 +1,64 @@ +# 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 collections.abc import Iterable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from . import actuator_net +from .actuator_pd_cfg import DCMotorCfg + + +@configclass +class ActuatorNetLSTMCfg(DCMotorCfg): + """Configuration for LSTM-based actuator model.""" + + class_type: type = actuator_net.ActuatorNetLSTM + # we don't use stiffness and damping for actuator net + stiffness = None + damping = None + + network_file: str = MISSING + """Path to the file containing network weights.""" + + +@configclass +class ActuatorNetMLPCfg(DCMotorCfg): + """Configuration for MLP-based actuator model.""" + + class_type: type = actuator_net.ActuatorNetMLP + # we don't use stiffness and damping for actuator net + + stiffness = None + damping = None + + network_file: str = MISSING + """Path to the file containing network weights.""" + + pos_scale: float = MISSING + """Scaling of the joint position errors input to the network.""" + vel_scale: float = MISSING + """Scaling of the joint velocities input to the network.""" + torque_scale: float = MISSING + """Scaling of the joint efforts output from the network.""" + + input_order: Literal["pos_vel", "vel_pos"] = MISSING + """Order of the inputs to the network. + + The order can be one of the following: + + * ``"pos_vel"``: joint position errors followed by joint velocities + * ``"vel_pos"``: joint velocities followed by joint position errors + """ + + input_idx: Iterable[int] = MISSING + """ + Indices of the actuator history buffer passed as inputs to the network. + + The index *0* corresponds to current time-step, while *n* corresponds to n-th + time-step in the past. The allocated history length is `max(input_idx) + 1`. + """ diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd.py b/source/isaaclab/isaaclab/actuators/actuator_pd.py index 162005dfd17..cd774c3abde 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_pd.py +++ b/source/isaaclab/isaaclab/actuators/actuator_pd.py @@ -17,7 +17,7 @@ from .actuator_base import ActuatorBase if TYPE_CHECKING: - from .actuator_cfg import ( + from .actuator_pd_cfg import ( DCMotorCfg, DelayedPDActuatorCfg, IdealPDActuatorCfg, @@ -377,41 +377,17 @@ class RemotizedPDActuator(DelayedPDActuator): def __init__( self, cfg: RemotizedPDActuatorCfg, - joint_names: list[str], - joint_ids: Sequence[int], - num_envs: int, - device: str, - stiffness: torch.Tensor | float = 0.0, - damping: torch.Tensor | float = 0.0, - armature: torch.Tensor | float = 0.0, - friction: torch.Tensor | float = 0.0, - dynamic_friction: torch.Tensor | float = 0.0, - viscous_friction: torch.Tensor | float = 0.0, - effort_limit: torch.Tensor | float = torch.inf, - velocity_limit: torch.Tensor | float = torch.inf, + *args, + **kwargs, ): # remove effort and velocity box constraints from the base class cfg.effort_limit = torch.inf cfg.velocity_limit = torch.inf # call the base method and set default effort_limit and velocity_limit to inf - super().__init__( - cfg, - joint_names, - joint_ids, - num_envs, - device, - stiffness, - damping, - armature, - friction, - dynamic_friction, - viscous_friction, - effort_limit, - velocity_limit, - ) - self._joint_parameter_lookup = torch.tensor(cfg.joint_parameter_lookup, device=device) + super().__init__(cfg, *args, **kwargs) + self._joint_parameter_lookup = torch.tensor(cfg.joint_parameter_lookup, device=self._device) # define remotized joint torque limit - self._torque_limit = LinearInterpolation(self.angle_samples, self.max_torque_samples, device=device) + self._torque_limit = LinearInterpolation(self.angle_samples, self.max_torque_samples, device=self._device) """ Properties. diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_pd_cfg.py new file mode 100644 index 00000000000..35d40278e2c --- /dev/null +++ b/source/isaaclab/isaaclab/actuators/actuator_pd_cfg.py @@ -0,0 +1,81 @@ +# 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.utils import configclass + +from . import actuator_pd +from .actuator_base_cfg import ActuatorBaseCfg + +""" +Implicit Actuator Models. +""" + + +@configclass +class ImplicitActuatorCfg(ActuatorBaseCfg): + """Configuration for an implicit actuator. + + Note: + The PD control is handled implicitly by the simulation. + """ + + class_type: type = actuator_pd.ImplicitActuator + + +""" +Explicit Actuator Models. +""" + + +@configclass +class IdealPDActuatorCfg(ActuatorBaseCfg): + """Configuration for an ideal PD actuator.""" + + class_type: type = actuator_pd.IdealPDActuator + + +@configclass +class DCMotorCfg(IdealPDActuatorCfg): + """Configuration for direct control (DC) motor actuator model.""" + + class_type: type = actuator_pd.DCMotor + + saturation_effort: float = MISSING + """Peak motor force/torque of the electric DC motor (in N-m).""" + + +@configclass +class DelayedPDActuatorCfg(IdealPDActuatorCfg): + """Configuration for a delayed PD actuator.""" + + class_type: type = actuator_pd.DelayedPDActuator + + min_delay: int = 0 + """Minimum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" + + max_delay: int = 0 + """Maximum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" + + +@configclass +class RemotizedPDActuatorCfg(DelayedPDActuatorCfg): + """Configuration for a remotized PD actuator. + + Note: + The torque output limits for this actuator is derived from a linear interpolation of a lookup table + in :attr:`joint_parameter_lookup`. This table describes the relationship between joint angles and + the output torques. + """ + + class_type: type = actuator_pd.RemotizedPDActuator + + joint_parameter_lookup: list[list[float]] = MISSING + """Joint parameter lookup table. Shape is (num_lookup_points, 3). + + 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. + """ diff --git a/source/isaaclab_assets/isaaclab_assets/robots/allegro.py b/source/isaaclab_assets/isaaclab_assets/robots/allegro.py index abc601aa283..26bec4c5fd0 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/allegro.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/allegro.py @@ -19,7 +19,7 @@ import math import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR diff --git a/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py b/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py index d6c86bb3f15..2ca955cd1e8 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py @@ -17,7 +17,7 @@ """ import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR diff --git a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py index 79696aab091..47e3f3d5840 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py @@ -17,7 +17,7 @@ import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py index 78cb3d3da6a..68d28ace75d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py @@ -4,7 +4,7 @@ # SPDX-License-Identifier: BSD-3-Clause import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg from isaaclab.scene import InteractiveSceneCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py index 6f6630a3eb8..64b09ea81c5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py @@ -4,7 +4,7 @@ # SPDX-License-Identifier: BSD-3-Clause import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg from isaaclab.scene import InteractiveSceneCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py index 735efa306d6..72a08b06941 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -4,7 +4,7 @@ # SPDX-License-Identifier: BSD-3-Clause import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg from isaaclab.scene import InteractiveSceneCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py index 8e0aab5b0c7..53dc7cf8ffb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py @@ -12,7 +12,7 @@ from pxr import UsdGeom import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import Articulation, ArticulationCfg from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg from isaaclab.scene import InteractiveSceneCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index b6a3702eab8..ba2e9ac946e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -7,7 +7,7 @@ from dataclasses import MISSING import isaaclab.sim as sim_utils -from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import EventTermCfg as EventTerm From d1932bf1dd5f8ac9ef2dbcf4e90146dd8a78320c Mon Sep 17 00:00:00 2001 From: Brian McCann Date: Mon, 10 Nov 2025 15:59:24 -0500 Subject: [PATCH 2/2] parent 7e8ebe67ef3e3c280eb3fda39443d643317fdeca author Brian McCann 1762808364 -0500 committer Brian McCann 1762808383 -0500 This change implements the necessary changes to the articulation and actuator classes in order to configure the new actuator drive model including velocity and effort dependent constraints on motor actuation. For details see https://nvidia-omniverse.github.io/PhysX/physx/5.6.1/docs/Articulations.html#articulation-drive-stability and https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.ArticulationView.set_dof_drive_model_properties --- docs/source/api/lab/isaaclab.actuators.rst | 3 + scripts/get_omni_version.py | 15 + source/isaaclab/config/extension.toml | 3 +- source/isaaclab/docs/CHANGELOG.rst | 31 +- .../isaaclab/actuators/actuator_base.py | 135 ++++++-- .../isaaclab/actuators/actuator_base_cfg.py | 20 ++ .../isaaclab/actuators/actuator_cfg.py | 291 ------------------ .../assets/articulation/articulation.py | 73 ++++- .../assets/articulation/articulation_data.py | 7 + .../actuators/test_drive_model_properties.py | 69 +++++ .../isaaclab/test/assets/test_articulation.py | 85 ++++- 11 files changed, 412 insertions(+), 320 deletions(-) create mode 100644 scripts/get_omni_version.py delete mode 100644 source/isaaclab/isaaclab/actuators/actuator_cfg.py create mode 100644 source/isaaclab/test/actuators/test_drive_model_properties.py diff --git a/docs/source/api/lab/isaaclab.actuators.rst b/docs/source/api/lab/isaaclab.actuators.rst index 5ab005de5b3..199ed77b811 100644 --- a/docs/source/api/lab/isaaclab.actuators.rst +++ b/docs/source/api/lab/isaaclab.actuators.rst @@ -36,6 +36,9 @@ Actuator Base :inherited-members: :exclude-members: __init__, class_type +.. autoclass:: isaaclab.actuators.actuator_base_cfg.ActuatorBaseCfg.DriveModelCfg + :members: + Implicit Actuator ----------------- diff --git a/scripts/get_omni_version.py b/scripts/get_omni_version.py new file mode 100644 index 00000000000..7ff22c77ffc --- /dev/null +++ b/scripts/get_omni_version.py @@ -0,0 +1,15 @@ +# 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 omni.kit.app + +from isaaclab.app import AppLauncher + +app_launcher = AppLauncher(headless=True, enable_cameras=True) +simulation_app = app_launcher.app + +app = omni.kit.app.get_app() +kit_version = app.get_kit_version() +print(kit_version) diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 0f69b167d74..9883d9fbb25 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,8 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.48.1" + +version = "0.48.2" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 043c1776187..fbff2d885ec 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,14 +1,25 @@ Changelog --------- -0.48.1 (2025-11-10) +0.48.2 (2025-11-10) ~~~~~~~~~~~~~~~~~~~ -Changed -^^^^^^^ +Added +^^^^^ + +* Implemented drive model improvements for implicit actuators allowing them to configure a new feature within physx to apply + constraints on actuator effort dependent on the torque and velocity on the articulation. +* Introduced a NamedTuple config classes as a way to organize related parameters, and extended the configuration parsing to + work with related (mutually dependent) parameters in the configurations. -* Refactored modules related to the actuator configs in order to remediate a circular import necessary to support future - actuator drive model improvements. +0.48.1 (2025-11-10) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Refactored modules related to the actuator configs in order to remediate a circular import necessary to support future + actuator drive model improvements. 0.48.0 (2025-11-03) ~~~~~~~~~~~~~~~~~~~ @@ -71,6 +82,16 @@ Added * Added parameter :attr:`~isaaclab.terrains.TerrainImporterCfg.use_terrain_origins` to allow generated sub terrains with grid origins. +0.48.2 (2025-11-10) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Implemented drive model improvements for implicit actuators allowing them to configure a new feature within physx to apply + constraints on actuator effort dependent on the torque and velocity on the articulation. +* Introduced a NamedTuple config classes as a way to organize related parameters, and extended the configuration parsing to + work with related (mutually dependent) parameters in the configurations. 0.47.7 (2025-10-31) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/actuators/actuator_base.py b/source/isaaclab/isaaclab/actuators/actuator_base.py index b0517fc4ef6..816903be9f2 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_base.py +++ b/source/isaaclab/isaaclab/actuators/actuator_base.py @@ -8,13 +8,17 @@ import torch from abc import ABC, abstractmethod from collections.abc import Sequence -from typing import TYPE_CHECKING, ClassVar +from typing import ClassVar import isaaclab.utils.string as string_utils from isaaclab.utils.types import ArticulationActions +<<<<<<< HEAD if TYPE_CHECKING: from .actuator_base_cfg import ActuatorBaseCfg +======= +from .actuator_base_cfg import ActuatorBaseCfg +>>>>>>> ed1544b8ca (parent 7e8ebe67ef3e3c280eb3fda39443d643317fdeca) class ActuatorBase(ABC): @@ -84,6 +88,18 @@ class ActuatorBase(ABC): For implicit actuators, the :attr:`velocity_limit` and :attr:`velocity_limit_sim` are the same. """ + drive_model: torch.Tensor + """Three parameters for each joint/env defining the: + (1) [:,:,0] speed_effort_gradient : float = 1 (default), + (2) [:,:,1] maximum_actuator_velocity : float = torch.inf (default), and + (3) [:,:,2] velocity_dependent_resistance : float = 1 (default) + + which define velocity and effort dependent constraints on the motor's performance. + + This feature is only implemented in IsaacSim v5.0. + + The shape is (num_envs, num_joints, 3).""" + stiffness: torch.Tensor """The stiffness (P gain) of the PD controller. Shape is (num_envs, num_joints).""" @@ -124,6 +140,7 @@ def __init__( viscous_friction: torch.Tensor | float = 0.0, effort_limit: torch.Tensor | float = torch.inf, velocity_limit: torch.Tensor | float = torch.inf, + drive_model: torch.Tensor | tuple[float, float, float] = ActuatorBaseCfg.DriveModelCfg(), ): """Initialize the actuator. @@ -160,6 +177,9 @@ def __init__( If a tensor, then the shape is (num_envs, num_joints). velocity_limit: The default velocity limit. Defaults to infinity. If a tensor, then the shape is (num_envs, num_joints). + drive_model: Drive model for the actuator including speed_effort_gradient, max_actuator_velocity, and + velocity_dependent_resistance in that order. Defaults to (0.0, torch.inf, 0.0). + If a tensor then the shape is (num_envs, num_joints, 3). """ # save parameters self.cfg = cfg @@ -187,19 +207,32 @@ def __init__( ("friction", friction), ("dynamic_friction", dynamic_friction), ("viscous_friction", viscous_friction), + ("drive_model", drive_model, 3), ] - for param_name, usd_val in to_check: + for param_name, usd_val, *tuple_len in to_check: + # check if the parameter requires a tuple or a single float + if len(tuple_len) > 0: + shape = (self._num_envs, self.num_joints, tuple_len[0]) + else: + shape = (self._num_envs, self.num_joints) + cfg_val = getattr(self.cfg, param_name) - setattr(self, param_name, self._parse_joint_parameter(cfg_val, usd_val)) + setattr(self, param_name, self._parse_joint_parameter(cfg_val, usd_val, shape, param_name=param_name)) new_val = getattr(self, param_name) allclose = ( - torch.all(new_val == usd_val) if isinstance(usd_val, (float, int)) else torch.allclose(new_val, usd_val) + torch.all(new_val == usd_val) + if isinstance(usd_val, (float, int)) + else ( + all([torch.all(new_val[:, :, i] == float(v)) for i, v in enumerate(usd_val)]) + if isinstance(usd_val, tuple) + else torch.allclose(new_val, usd_val) + ) ) if cfg_val is None or not allclose: self._record_actuator_resolution( cfg_val=getattr(self.cfg, param_name), - new_val=new_val[0], # new val always has the shape of (num_envs, num_joints) + new_val=new_val[0], usd_val=usd_val, joint_names=joint_names, joint_ids=joint_ids, @@ -303,13 +336,24 @@ def _record_actuator_resolution(self, cfg_val, new_val, usd_val, joint_names, jo ids = joint_ids if isinstance(joint_ids, torch.Tensor) else list(range(len(joint_names))) for idx, name in enumerate(joint_names): - cfg_val_log = "Not Specified" if cfg_val is None else float(new_val[idx]) - default_usd_val = usd_val if isinstance(usd_val, (float, int)) else float(usd_val[0][idx]) - applied_val_log = default_usd_val if cfg_val is None else float(new_val[idx]) - table.append([name, int(ids[idx]), default_usd_val, cfg_val_log, applied_val_log]) + if len(new_val.shape) == 1: + cfg_val_log = "Not Specified" if cfg_val is None else float(new_val[idx]) + default_usd_val = usd_val if isinstance(usd_val, (float, int)) else float(usd_val[0][idx]) + applied_val_log = default_usd_val if cfg_val is None else float(new_val[idx]) + table.append([name, int(ids[idx]), default_usd_val, cfg_val_log, applied_val_log]) + else: + cfg_val_log = "Not Specified" if cfg_val is None else tuple(new_val[idx]) + default_usd_val = usd_val if isinstance(usd_val, (tuple)) else tuple(usd_val[0][idx][:]) + applied_val_log = default_usd_val if cfg_val is None else tuple(new_val[idx]) + table.append([name, int(ids[idx]), default_usd_val, cfg_val_log, applied_val_log]) def _parse_joint_parameter( - self, cfg_value: float | dict[str, float] | None, default_value: float | torch.Tensor | None + self, + cfg_value: tuple[float, ...] | dict[str, tuple[float, ...]] | float | dict[str, float] | None, + default_value: tuple[float, ...] | float | torch.Tensor | None, + expected_shape: tuple[int, ...] | None = None, + *, + param_name: str = "No name specified", ) -> torch.Tensor: """Parse the joint parameter from the configuration. @@ -317,6 +361,10 @@ def _parse_joint_parameter( cfg_value: The parameter value from the configuration. If None, then use the default value. default_value: The default value to use if the parameter is None. If it is also None, then an error is raised. + expected_shape: The expected shape for the tensor buffer. Usually defaults to (num_envs, num_joints). + + Kwargs: + param_name: a string with the parameter name. (Optional used only in exception messages). Returns: The parsed parameter value. @@ -325,38 +373,87 @@ def _parse_joint_parameter( TypeError: If the parameter value is not of the expected type. TypeError: If the default value is not of the expected type. ValueError: If the parameter value is None and no default value is provided. - ValueError: If the default value tensor is the wrong shape. + ValueError: If a tensor or tuple is the wrong shape. """ + if expected_shape is None: + expected_shape = (self._num_envs, self.num_joints) # create parameter buffer - param = torch.zeros(self._num_envs, self.num_joints, device=self._device) + param = torch.zeros(*expected_shape, device=self._device) + # parse the parameter if cfg_value is not None: if isinstance(cfg_value, (float, int)): # if float, then use the same value for all joints param[:] = float(cfg_value) + elif isinstance(cfg_value, tuple): + # if tuple, ensure we expect a tuple for this parameter + if len(expected_shape) < 3: + raise TypeError( + f"Invalid type for parameter value: {type(cfg_value)} for parameter {param_name}" + + f" actuator on joints {self.joint_names}. Expected float or dict, got tuple" + ) + # ensure the tuple is the correct length, and assign to the last tensor dimensions across all joints + if not len(cfg_value) == expected_shape[2]: + raise ValueError( + f"Invalid tuple length for parameter {param_name}, got {len(cfg_value)}, expected" + + f" {expected_shape[2]}" + ) + for i, v in enumerate(cfg_value): + param[:, :, i] = float(v) elif isinstance(cfg_value, dict): # if dict, then parse the regular expression - indices, _, values = string_utils.resolve_matching_names_values(cfg_value, self.joint_names) - # note: need to specify type to be safe (e.g. values are ints, but we want floats) - param[:, indices] = torch.tensor(values, dtype=torch.float, device=self._device) + indices, j, values = string_utils.resolve_matching_names_values(cfg_value, self.joint_names) + # if the expected shape has two dimensions, we expect floats + if len(expected_shape) < 3: + # note: need to specify type to be safe (e.g. values are ints, but we want floats) + param[:, indices] = torch.tensor(values, dtype=torch.float, device=self._device) + # otherwise, we expect tuples + else: + # We can't directly assign tuples to tensors, so iterate through them + for i, v in enumerate(values): + # Raise an exception if the tuple is the incorrect length + if len(v) != expected_shape[2]: + raise ValueError( + f"Invalid tuple length for parameter {param_name} on joint {j[i]} at index" + f" {indices[i]}, " + + f"expected {expected_shape[2]} got {len(v)}." + ) + # Otherwise iterate through the tuple, and assign the values in order. + for i2, v2 in enumerate(v): + param[:, indices[i], i2] = float(v2) else: raise TypeError( f"Invalid type for parameter value: {type(cfg_value)} for " - + f"actuator on joints {self.joint_names}. Expected float or dict." + + f"actuator on joints {self.joint_names}. Expected tuple, float or dict." ) elif default_value is not None: if isinstance(default_value, (float, int)): # if float, then use the same value for all joints param[:] = float(default_value) + elif isinstance(default_value, tuple): + # if tuple, ensure we expect a tuple for this parameter + if len(expected_shape) < 3: + raise TypeError( + f"Invalid default type for parameter value: {type(default_value)} for " + + f"actuator on joints {self.joint_names}. Expected float or dict, got tuple" + ) + # ensure the tuple is the correct length, and assign to the last tensor dimensions across all joints + if not len(default_value) == expected_shape[2]: + raise ValueError( + f"Invalid tuple length for parameter {param_name}, got {len(default_value)}, expected" + + f" {expected_shape[2]}" + ) + for i, v in enumerate(default_value): + param[:, :, i] = float(v) elif isinstance(default_value, torch.Tensor): # if tensor, then use the same tensor for all joints - if default_value.shape == (self._num_envs, self.num_joints): + if tuple(default_value.shape) == expected_shape: param = default_value.float() else: raise ValueError( "Invalid default value tensor shape.\n" - f"Got: {default_value.shape}\n" - f"Expected: {(self._num_envs, self.num_joints)}" + + f"Got: {tuple(default_value.shape)}\n" + + f"Expected: {expected_shape}" ) else: raise TypeError( diff --git a/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py index fb4697e4025..e1e908c866b 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py +++ b/source/isaaclab/isaaclab/actuators/actuator_base_cfg.py @@ -4,6 +4,11 @@ # SPDX-License-Identifier: BSD-3-Clause from dataclasses import MISSING +<<<<<<< HEAD +======= +from torch import inf +from typing import NamedTuple +>>>>>>> ed1544b8ca (parent 7e8ebe67ef3e3c280eb3fda39443d643317fdeca) from isaaclab.utils import configclass @@ -48,6 +53,21 @@ class ActuatorBaseCfg: """ +<<<<<<< HEAD +======= + """Optional (min v5) settings to the drive model capturing performance envelope velocity-effort dependence. + + See: https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/_downloads/f44e831b7f29e7c2ec8e3f2c54418430/drivePerformanceEnvelope.pdf + """ + + class DriveModelCfg(NamedTuple): + speed_effort_gradient: float = 0.0 + max_actuator_velocity: float = inf + velocity_dependent_resistance: float = 0.0 + + drive_model: dict[str, DriveModelCfg] | DriveModelCfg | None = None + +>>>>>>> ed1544b8ca (parent 7e8ebe67ef3e3c280eb3fda39443d643317fdeca) velocity_limit: dict[str, float] | float | None = None """Velocity limit of the joints in the group. Defaults to None. diff --git a/source/isaaclab/isaaclab/actuators/actuator_cfg.py b/source/isaaclab/isaaclab/actuators/actuator_cfg.py deleted file mode 100644 index bc2e1a6667d..00000000000 --- a/source/isaaclab/isaaclab/actuators/actuator_cfg.py +++ /dev/null @@ -1,291 +0,0 @@ -# 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 collections.abc import Iterable -from dataclasses import MISSING -from typing import Literal - -from isaaclab.utils import configclass - -from . import actuator_net, actuator_pd -from .actuator_base import ActuatorBase - - -@configclass -class ActuatorBaseCfg: - """Configuration for default actuators in an articulation.""" - - class_type: type[ActuatorBase] = MISSING - """The associated actuator class. - - The class should inherit from :class:`isaaclab.actuators.ActuatorBase`. - """ - - joint_names_expr: list[str] = MISSING - """Articulation's joint names that are part of the group. - - Note: - This can be a list of joint names or a list of regex expressions (e.g. ".*"). - """ - - effort_limit: dict[str, float] | float | None = None - """Force/Torque limit of the joints in the group. Defaults to None. - - This limit is used to clip the computed torque sent to the simulation. If None, the - limit is set to the value specified in the USD joint prim. - - .. attention:: - - The :attr:`effort_limit_sim` attribute should be used to set the effort limit for - the simulation physics solver. - - The :attr:`effort_limit` attribute is used for clipping the effort output of the - actuator model **only** in the case of explicit actuators, such as the - :class:`~isaaclab.actuators.IdealPDActuator`. - - .. note:: - - For implicit actuators, the attributes :attr:`effort_limit` and :attr:`effort_limit_sim` - are equivalent. However, we suggest using the :attr:`effort_limit_sim` attribute because - it is more intuitive. - - """ - - velocity_limit: dict[str, float] | float | None = None - """Velocity limit of the joints in the group. Defaults to None. - - This limit is used by the actuator model. If None, the limit is set to the value specified - in the USD joint prim. - - .. attention:: - - The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for - the simulation physics solver. - - The :attr:`velocity_limit` attribute is used for clipping the effort output of the - actuator model **only** in the case of explicit actuators, such as the - :class:`~isaaclab.actuators.IdealPDActuator`. - - .. note:: - - For implicit actuators, the attribute :attr:`velocity_limit` is not used. This is to stay - backwards compatible with previous versions of the Isaac Lab, where this parameter was - unused since PhysX did not support setting the velocity limit for the joints using the - PhysX Tensor API. - """ - - effort_limit_sim: dict[str, float] | float | None = None - """Effort limit of the joints in the group applied to the simulation physics solver. Defaults to None. - - The effort limit is used to constrain the computed joint efforts in the physics engine. If the - computed effort exceeds this limit, the physics engine will clip the effort to this value. - - Since explicit actuators (e.g. DC motor), compute and clip the effort in the actuator model, this - limit is by default set to a large value to prevent the physics engine from any additional clipping. - However, at times, it may be necessary to set this limit to a smaller value as a safety measure. - - If None, the limit is resolved based on the type of actuator model: - - * For implicit actuators, the limit is set to the value specified in the USD joint prim. - * For explicit actuators, the limit is set to 1.0e9. - - """ - - velocity_limit_sim: dict[str, float] | float | None = None - """Velocity limit of the joints in the group applied to the simulation physics solver. Defaults to None. - - The velocity limit is used to constrain the joint velocities in the physics engine. The joint will only - be able to reach this velocity if the joint's effort limit is sufficiently large. If the joint is moving - faster than this velocity, the physics engine will actually try to brake the joint to reach this velocity. - - If None, the limit is set to the value specified in the USD joint prim for both implicit and explicit actuators. - - .. tip:: - If the velocity limit is too tight, the physics engine may have trouble converging to a solution. - In such cases, we recommend either keeping this value sufficiently large or tuning the stiffness and - damping parameters of the joint to ensure the limits are not violated. - - """ - - stiffness: dict[str, float] | float | None = MISSING - """Stiffness gains (also known as p-gain) of the joints in the group. - - The behavior of the stiffness is different for implicit and explicit actuators. For implicit actuators, - the stiffness gets set into the physics engine directly. For explicit actuators, the stiffness is used - by the actuator model to compute the joint efforts. - - If None, the stiffness is set to the value from the USD joint prim. - """ - - damping: dict[str, float] | float | None = MISSING - """Damping gains (also known as d-gain) of the joints in the group. - - The behavior of the damping is different for implicit and explicit actuators. For implicit actuators, - the damping gets set into the physics engine directly. For explicit actuators, the damping gain is used - by the actuator model to compute the joint efforts. - - If None, the damping is set to the value from the USD joint prim. - """ - - armature: dict[str, float] | float | None = None - """Armature of the joints in the group. Defaults to None. - - The armature is directly added to the corresponding joint-space inertia. It helps improve the - simulation stability by reducing the joint velocities. - - It is a physics engine solver parameter that gets set into the simulation. - - If None, the armature is set to the value from the USD joint prim. - """ - - friction: dict[str, float] | float | None = None - r"""The static friction coefficient of the joints in the group. Defaults to None. - - The joint static friction is a unitless quantity. It relates the magnitude of the spatial force transmitted - from the parent body to the child body to the maximal static friction force that may be applied by the solver - to resist the joint motion. - - Mathematically, this means that: :math:`F_{resist} \leq \mu F_{spatial}`, where :math:`F_{resist}` - is the resisting force applied by the solver and :math:`F_{spatial}` is the spatial force - transmitted from the parent body to the child body. The simulated static friction effect is therefore - similar to static and Coulomb static friction. - - If None, the joint static friction is set to the value from the USD joint prim. - - Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, - it is modeled as an effort (torque or force). - """ - - dynamic_friction: dict[str, float] | float | None = None - """The dynamic friction coefficient of the joints in the group. Defaults to None. - - Note: In Isaac Sim 4.5, this parameter is modeled as a coefficient. In Isaac Sim 5.0 and later, - it is modeled as an effort (torque or force). - """ - - viscous_friction: dict[str, float] | float | None = None - """The viscous friction coefficient of the joints in the group. Defaults to None. - """ - - -""" -Implicit Actuator Models. -""" - - -@configclass -class ImplicitActuatorCfg(ActuatorBaseCfg): - """Configuration for an implicit actuator. - - Note: - The PD control is handled implicitly by the simulation. - """ - - class_type: type = actuator_pd.ImplicitActuator - - -""" -Explicit Actuator Models. -""" - - -@configclass -class IdealPDActuatorCfg(ActuatorBaseCfg): - """Configuration for an ideal PD actuator.""" - - class_type: type = actuator_pd.IdealPDActuator - - -@configclass -class DCMotorCfg(IdealPDActuatorCfg): - """Configuration for direct control (DC) motor actuator model.""" - - class_type: type = actuator_pd.DCMotor - - saturation_effort: float = MISSING - """Peak motor force/torque of the electric DC motor (in N-m).""" - - -@configclass -class ActuatorNetLSTMCfg(DCMotorCfg): - """Configuration for LSTM-based actuator model.""" - - class_type: type = actuator_net.ActuatorNetLSTM - # we don't use stiffness and damping for actuator net - stiffness = None - damping = None - - network_file: str = MISSING - """Path to the file containing network weights.""" - - -@configclass -class ActuatorNetMLPCfg(DCMotorCfg): - """Configuration for MLP-based actuator model.""" - - class_type: type = actuator_net.ActuatorNetMLP - # we don't use stiffness and damping for actuator net - stiffness = None - damping = None - - network_file: str = MISSING - """Path to the file containing network weights.""" - - pos_scale: float = MISSING - """Scaling of the joint position errors input to the network.""" - vel_scale: float = MISSING - """Scaling of the joint velocities input to the network.""" - torque_scale: float = MISSING - """Scaling of the joint efforts output from the network.""" - - input_order: Literal["pos_vel", "vel_pos"] = MISSING - """Order of the inputs to the network. - - The order can be one of the following: - - * ``"pos_vel"``: joint position errors followed by joint velocities - * ``"vel_pos"``: joint velocities followed by joint position errors - """ - - input_idx: Iterable[int] = MISSING - """ - Indices of the actuator history buffer passed as inputs to the network. - - The index *0* corresponds to current time-step, while *n* corresponds to n-th - time-step in the past. The allocated history length is `max(input_idx) + 1`. - """ - - -@configclass -class DelayedPDActuatorCfg(IdealPDActuatorCfg): - """Configuration for a delayed PD actuator.""" - - class_type: type = actuator_pd.DelayedPDActuator - - min_delay: int = 0 - """Minimum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" - - max_delay: int = 0 - """Maximum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" - - -@configclass -class RemotizedPDActuatorCfg(DelayedPDActuatorCfg): - """Configuration for a remotized PD actuator. - - Note: - The torque output limits for this actuator is derived from a linear interpolation of a lookup table - in :attr:`joint_parameter_lookup`. This table describes the relationship between joint angles and - the output torques. - """ - - class_type: type = actuator_pd.RemotizedPDActuator - - joint_parameter_lookup: list[list[float]] = MISSING - """Joint parameter lookup table. Shape is (num_lookup_points, 3). - - 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. - """ diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index a76d5ae4444..ea5c46d2005 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -328,7 +328,6 @@ def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[in def write_root_com_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): """Set the root center of mass state over selected environment indices into the simulation. - The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear and angular velocity. All the quantities are in the simulation frame. @@ -793,6 +792,47 @@ def write_joint_effort_limit_to_sim( # set into simulation self.root_physx_view.set_dof_max_forces(self._data.joint_effort_limits.cpu(), indices=physx_env_ids.cpu()) + def write_joint_drive_model_to_sim( + self, + drive_params: torch.Tensor | tuple[float, float, float] | None = None, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write parameters (in additional to max forces) necessary to model motor drive performance envelopes. + + Additional parameters can be provided to improve sim-to-real transfer by more accurately modeling the motor drive's + performance envelope. Specifically, we define linear constraints on the torque-speed boundary and the speed-torque + boundary by defining a speed-effort grandient, maximum actuator velocity, and velocity dependent resistance for each + motor in the articulation. See: https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/_downloads/f44e831b7f29e7c2ec8e3f2c54418430/drivePerformanceEnvelope.pdf + + Args: + drive_params: The drive parameters. Shape is (len(env_ids), len(joint_ids), 3). The final three dimensions + reflect the speed-effort gradient, max actuator velocity, and velocity dependent resistance + respectively. + joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). + env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). + """ + if int(get_version()[2]) >= 5: + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # move tensor to cpu if needed + if isinstance(drive_params, torch.Tensor): + drive_params = drive_params.to(self.device) + # set into internal buffers + self._data.joint_drive_model_parameters[env_ids, joint_ids] = drive_params + # set into simulation + self.root_physx_view.set_dof_drive_model_properties( + self._data.joint_drive_model_parameters.cpu(), indices=physx_env_ids.cpu() + ) + def write_joint_armature_to_sim( self, armature: torch.Tensor | float, @@ -1594,6 +1634,17 @@ def _create_buffers(self): self._data.joint_pos_limits = self._data.default_joint_pos_limits.clone() self._data.joint_vel_limits = self.root_physx_view.get_dof_max_velocities().to(self.device).clone() self._data.joint_effort_limits = self.root_physx_view.get_dof_max_forces().to(self.device).clone() + if int(get_version()[2]) >= 5: + self._data.default_joint_drive_model_parameters = ( + self.root_physx_view.get_dof_drive_model_properties().to(self.device).clone() + ) + self._data.joint_drive_model_parameters = self._data.default_joint_drive_model_parameters.clone() + else: + # Initialize with zeros for v<5 compatibility + default_shape = (self.num_instances, self.num_joints, 3) + self._data.default_joint_drive_model_parameters = torch.zeros(default_shape, device=self.device) + self._data.joint_drive_model_parameters = self._data.default_joint_drive_model_parameters.clone() + self._data.joint_stiffness = self._data.default_joint_stiffness.clone() self._data.joint_damping = self._data.default_joint_damping.clone() self._data.joint_armature = self._data.default_joint_armature.clone() @@ -1701,6 +1752,7 @@ def _process_actuators_cfg(self): joint_ids = torch.tensor(joint_ids, device=self.device) # create actuator collection # note: for efficiency avoid indexing when over all indices + actuator: ActuatorBase = actuator_cfg.class_type( cfg=actuator_cfg, joint_names=joint_names, @@ -1715,6 +1767,11 @@ def _process_actuators_cfg(self): viscous_friction=self._data.default_joint_viscous_friction_coeff[:, joint_ids], effort_limit=self._data.joint_effort_limits[:, joint_ids].clone(), velocity_limit=self._data.joint_vel_limits[:, joint_ids], + drive_model=( + self._data.joint_drive_model_parameters[:, joint_ids] + if int(get_version()[2]) >= 5 + else ActuatorBaseCfg.DriveModelCfg() + ), ) # log information on actuator groups model_type = "implicit" if actuator.is_implicit_model else "explicit" @@ -1742,6 +1799,7 @@ def _process_actuators_cfg(self): self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices) self.write_joint_friction_coefficient_to_sim(actuator.friction, joint_ids=actuator.joint_indices) if int(get_version()[2]) >= 5: + self.write_joint_drive_model_to_sim(actuator.drive_model, joint_ids=actuator.joint_indices) self.write_joint_dynamic_friction_coefficient_to_sim( actuator.dynamic_friction, joint_ids=actuator.joint_indices ) @@ -1758,7 +1816,7 @@ def _process_actuators_cfg(self): if int(get_version()[2]) >= 5: self._data.default_joint_dynamic_friction_coeff[:, actuator.joint_indices] = actuator.dynamic_friction self._data.default_joint_viscous_friction_coeff[:, actuator.joint_indices] = actuator.viscous_friction - + self._data.default_joint_drive_model_parameters[:, actuator.joint_indices] = actuator.drive_model # perform some sanity checks to ensure actuators are prepared correctly total_act_joints = sum(actuator.num_joints for actuator in self.actuators.values()) if total_act_joints != (self.num_joints - self.num_fixed_tendons): @@ -1775,7 +1833,16 @@ def _process_actuators_cfg(self): for prop_idx, resolution_detail in enumerate(resolution_details): actuator_group_str = actuator_group if group_count == 0 else "" property_str = property if prop_idx == 0 else "" - fmt = [f"{v:.2e}" if isinstance(v, float) else str(v) for v in resolution_detail] + fmt = [ + ( + f"{v:.2e}" + if isinstance(v, float) + else ( + "(" + ", ".join([f"{vt:.2e}" for vt in v]) + ")" if isinstance(v, tuple) else str(v) + ) + ) + for v in resolution_detail + ] t.add_row([actuator_group_str, property_str, *fmt]) group_count += 1 omni.log.warn(f"\nActuatorCfg-USD Value Discrepancy Resolution (matching values are skipped): \n{t}") diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py index 6d974dd37d6..0fc8ed04cf5 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py @@ -286,6 +286,10 @@ def update(self, dt: float): This quantity is parsed from the USD schema at the time of initialization. """ + default_joint_drive_model_parameters: torch.Tensor = None + """Default joint drive model parameters. Shape is (num_instances, num_drive_models, 3). The last indices reflect + the speed_effort_gradient, the max_actuator_velocity, and the velocity_dependent_resistance respectively.""" + ## # Joint commands -- Set into simulation. ## @@ -381,6 +385,9 @@ def update(self, dt: float): joint_effort_limits: torch.Tensor = None """Joint maximum effort provided to the simulation. Shape is (num_instances, num_joints).""" + joint_drive_model_parameters = None + """Joint drive model parameters provided to the simulation. Shape is (num_instances, num_joints, 3).""" + ## # Joint properties - Custom. ## diff --git a/source/isaaclab/test/actuators/test_drive_model_properties.py b/source/isaaclab/test/actuators/test_drive_model_properties.py new file mode 100644 index 00000000000..7402544d910 --- /dev/null +++ b/source/isaaclab/test/actuators/test_drive_model_properties.py @@ -0,0 +1,69 @@ +# Copyright (c) 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.app import AppLauncher + +HEADLESS = True + +# if not AppLauncher.instance(): +simulation_app = AppLauncher(headless=HEADLESS).app + +"""Rest of imports follows""" + +import torch + +import pytest + +from isaaclab.actuators import IdealPDActuatorCfg + + +@pytest.mark.parametrize("num_envs", [1, 2]) +@pytest.mark.parametrize("num_joints", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_init_drive_model(num_envs, num_joints, device): + joint_names = [f"joint_{d}" for d in range(num_joints)] + joint_ids = [d for d in range(num_joints)] + stiffness = 200 + damping = 10 + effort_limit = 60.0 + velocity_limit = 50 + drive_model = IdealPDActuatorCfg.DriveModelCfg( + speed_effort_gradient=100.0, + max_actuator_velocity=200.0, + velocity_dependent_resistance=0.1, + ) + + actuator_cfg = IdealPDActuatorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + effort_limit=effort_limit, + velocity_limit=velocity_limit, + drive_model=drive_model, + ) + # assume Articulation class: + # - finds joints (names and ids) associate with the provided joint_names_expr + + actuator = actuator_cfg.class_type( + actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=num_envs, + device=device, + ) + + # check device and shape + torch.testing.assert_close( + actuator.drive_model[:, :, 2], + drive_model.velocity_dependent_resistance * torch.ones(num_envs, num_joints, device=device), + ) + torch.testing.assert_close( + actuator.drive_model[:, :, 1], + drive_model.max_actuator_velocity * torch.ones(num_envs, num_joints, device=device), + ) + torch.testing.assert_close( + actuator.drive_model[:, :, 0], + drive_model.speed_effort_gradient * torch.ones(num_envs, num_joints, device=device), + ) diff --git a/source/isaaclab/test/assets/test_articulation.py b/source/isaaclab/test/assets/test_articulation.py index 3dda2c89396..9b8a735487d 100644 --- a/source/isaaclab/test/assets/test_articulation.py +++ b/source/isaaclab/test/assets/test_articulation.py @@ -48,6 +48,7 @@ def generate_articulation_cfg( effort_limit: float | None = None, velocity_limit_sim: float | None = None, effort_limit_sim: float | None = None, + drive_model: tuple[float, float, float] | None = None, ) -> ArticulationCfg: """Generate an articulation configuration. @@ -67,11 +68,13 @@ def generate_articulation_cfg( Only currently used for "single_joint_implicit" and "single_joint_explicit". effort_limit_sim: Effort limit for the actuators (set into the simulation). Only currently used for "single_joint_implicit" and "single_joint_explicit". + drive_model: Drive model parameters for the actuators. + Only currently used for "single_joint_implicit" and "single_joint_explicit". Returns: The articulation configuration for the requested articulation type. - """ + if articulation_type == "humanoid": articulation_cfg = ArticulationCfg( spawn=sim_utils.UsdFileCfg( @@ -102,6 +105,7 @@ def generate_articulation_cfg( velocity_limit=velocity_limit, stiffness=2000.0, damping=100.0, + drive_model=drive_model, ), }, init_state=ArticulationCfg.InitialStateCfg( @@ -126,6 +130,7 @@ def generate_articulation_cfg( velocity_limit=velocity_limit, stiffness=0.0, damping=10.0, + drive_model=drive_model, ), }, ) @@ -1369,6 +1374,84 @@ def test_setting_velocity_limit_explicit(sim, num_articulations, device, vel_lim torch.testing.assert_close(physx_vel_limit, expected_vel_limit) +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("drive_model", [(100.0, 200.0, 0.1), None]) +@pytest.mark.skipif(int(get_version()[2]) < 5, reason="Simulator version must be 5 or greater") +@pytest.mark.isaacsim_ci +def test_setting_drive_model_explicit(sim, num_articulations, device, drive_model): + """Test setting of velocity limit for explicit actuators.""" + articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_explicit", drive_model=drive_model) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, + num_articulations=num_articulations, + device=device, + ) + # Play sim + sim.reset() + + # collect dm init values + physx_dm = articulation.root_physx_view.get_dof_drive_model_properties().to(device) + actuator_dm = articulation.actuators["joint"].drive_model + + # check data buffer for joint_ + torch.testing.assert_close(articulation.data.joint_drive_model_parameters, physx_dm) + + if drive_model is not None: + expected_actuator_dm = torch.zeros( + (articulation.num_instances, articulation.num_joints, 3), + device=articulation.device, + ) + expected_actuator_dm[:, :, 0] = drive_model[0] + expected_actuator_dm[:, :, 1] = drive_model[1] + expected_actuator_dm[:, :, 2] = drive_model[2] + + # check actuator is set + torch.testing.assert_close(actuator_dm, expected_actuator_dm) + else: + # check actuator velocity_limit is the same as the PhysX default + torch.testing.assert_close(actuator_dm, physx_dm) + + +@pytest.mark.parametrize("num_articulations", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("drive_model", [(100.0, 200.0, 0.1), None]) +@pytest.mark.skipif(int(get_version()[2]) < 5, reason="Simulator version must be 5 or greater") +@pytest.mark.isaacsim_ci +def test_setting_drive_model_implicit(sim, num_articulations, device, drive_model): + """Test setting of velocity limit for explicit actuators.""" + articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit", drive_model=drive_model) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, + num_articulations=num_articulations, + device=device, + ) + # Play sim + sim.reset() + + # collect dm init values + physx_dm = articulation.root_physx_view.get_dof_drive_model_properties().to(device) + actuator_dm = articulation.actuators["joint"].drive_model + + # check data buffer for joint_ + torch.testing.assert_close(articulation.data.joint_drive_model_parameters, physx_dm) + + if drive_model is not None: + expected_actuator_dm = torch.zeros( + (articulation.num_instances, articulation.num_joints, 3), + device=articulation.device, + ) + expected_actuator_dm[:, :, 0] = drive_model[0] + expected_actuator_dm[:, :, 1] = drive_model[1] + expected_actuator_dm[:, :, 2] = drive_model[2] + + # check actuator is set + torch.testing.assert_close(actuator_dm, expected_actuator_dm) + else: + # check actuator velocity_limit is the same as the PhysX default + torch.testing.assert_close(actuator_dm, physx_dm) + + @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("effort_limit_sim", [1e5, None])