Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions source/isaaclab/isaaclab/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

"""Package containing the core framework."""

from enum import IntEnum
import os
import toml

Expand All @@ -17,3 +18,7 @@

# Configure the module-level variables
__version__ = ISAACLAB_METADATA["package"]["version"]

class Backend(IntEnum):
NEWTON = 0
PHYSX = 1
6 changes: 6 additions & 0 deletions source/isaaclab/isaaclab/actuators/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,12 @@
which defines the common interface for all actuator models. The actuator models are handled
and called by the :class:`isaaclab.assets.Articulation` class.
"""
from enum import IntEnum
class ControlMode(IntEnum):
"""Enum for the control mode of the actuator."""
NONE = 0
POSITION = 1
VELOCITY = 2

from .actuator_base import ActuatorBase
from .actuator_cfg import (
Expand Down
178 changes: 18 additions & 160 deletions source/isaaclab/isaaclab/actuators/actuator_base.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,8 @@
import torch
from abc import ABC, abstractmethod
from collections.abc import Sequence
from typing import TYPE_CHECKING, ClassVar, Literal
from typing import TYPE_CHECKING, ClassVar

import isaaclab.utils.string as string_utils
from isaaclab.utils.types import ArticulationActions

if TYPE_CHECKING:
Expand All @@ -36,68 +35,15 @@ class ActuatorBase(ABC):
To see how the class is used, check the :class:`isaaclab.assets.Articulation` class.
"""

__backend_name__: str = "base"
"""The name of the backend for the actuator."""

is_implicit_model: ClassVar[bool] = False
"""Flag indicating if the actuator is an implicit or explicit actuator model.

If a class inherits from :class:`ImplicitActuator`, then this flag should be set to :obj:`True`.
"""

computed_effort: torch.Tensor
"""The computed effort for the actuator group. Shape is (num_envs, num_joints)."""

applied_effort: torch.Tensor
"""The applied effort for the actuator group. Shape is (num_envs, num_joints).

This is the effort obtained after clipping the :attr:`computed_effort` based on the
actuator characteristics.
"""

effort_limit: torch.Tensor
"""The effort limit for the actuator group. Shape is (num_envs, num_joints).

For implicit actuators, the :attr:`effort_limit` and :attr:`effort_limit_sim` are the same.
"""

effort_limit_sim: torch.Tensor
"""The effort limit for the actuator group in the simulation. Shape is (num_envs, num_joints).

For implicit actuators, the :attr:`effort_limit` and :attr:`effort_limit_sim` are the same.
"""

velocity_limit: torch.Tensor
"""The velocity limit for the actuator group. Shape is (num_envs, num_joints).

For implicit actuators, the :attr:`velocity_limit` and :attr:`velocity_limit_sim` are the same.
"""

velocity_limit_sim: torch.Tensor
"""The velocity limit for the actuator group in the simulation. Shape is (num_envs, num_joints).

For implicit actuators, the :attr:`velocity_limit` and :attr:`velocity_limit_sim` are the same.
"""

control_mode: Literal["position", "velocity", "none"]
"""The control mode of the actuator.

The control mode can be one of the following:

* ``"position"``: Position control
* ``"velocity"``: Velocity control
* ``"none"``: No control (used for explicit actuators or direct effort control)
"""

stiffness: torch.Tensor
"""The stiffness (P gain) of the PD controller. Shape is (num_envs, num_joints)."""

damping: torch.Tensor
"""The damping (D gain) of the PD controller. Shape is (num_envs, num_joints)."""

armature: torch.Tensor
"""The armature of the actuator joints. Shape is (num_envs, num_joints)."""

friction: torch.Tensor
"""The joint friction of the actuator joints. Shape is (num_envs, num_joints)."""

_DEFAULT_MAX_EFFORT_SIM: ClassVar[float] = 1.0e9
"""The default maximum effort for the actuator joints in the simulation. Defaults to 1.0e9.

Expand All @@ -108,17 +54,6 @@ class ActuatorBase(ABC):
def __init__(
self,
cfg: ActuatorBaseCfg,
joint_names: list[str],
joint_ids: slice | torch.Tensor,
num_envs: int,
device: str,
control_mode: Literal["position", "velocity"] = "position",
stiffness: torch.Tensor | float = 0.0,
damping: torch.Tensor | float = 0.0,
armature: torch.Tensor | float = 0.0,
friction: torch.Tensor | float = 0.0,
effort_limit: torch.Tensor | float = torch.inf,
velocity_limit: torch.Tensor | float = torch.inf,
):
"""Initialize the actuator.

Expand All @@ -131,56 +66,9 @@ def __init__(

Args:
cfg: The configuration of the actuator model.
joint_names: The joint names in the articulation.
joint_ids: The joint indices in the articulation. If :obj:`slice(None)`, then all
the joints in the articulation are part of the group.
num_envs: Number of articulations in the view.
device: Device used for processing.
control_mode: The default control mode. Defaults to "position".
stiffness: The default joint stiffness (P gain). Defaults to 0.0.
If a tensor, then the shape is (num_envs, num_joints).
damping: The default joint damping (D gain). Defaults to 0.0.
If a tensor, then the shape is (num_envs, num_joints).
armature: The default joint armature. Defaults to 0.0.
If a tensor, then the shape is (num_envs, num_joints).
friction: The default joint friction. Defaults to 0.0.
If a tensor, then the shape is (num_envs, num_joints).
effort_limit: The default effort limit. Defaults to infinity.
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).
"""
# save parameters
self.cfg = cfg
self._num_envs = num_envs
self._device = device
self._joint_names = joint_names
self._joint_indices = joint_ids

# For explicit models, we do not want to enforce the effort limit through the solver
# (unless it is explicitly set)
if not self.is_implicit_model and self.cfg.effort_limit_sim is None:
self.cfg.effort_limit_sim = self._DEFAULT_MAX_EFFORT_SIM

# parse control mode
self.control_mode = control_mode
# parse joint stiffness and damping
self.stiffness = self._parse_joint_parameter(self.cfg.stiffness, stiffness)
self.damping = self._parse_joint_parameter(self.cfg.damping, damping)
# parse joint armature and friction
self.armature = self._parse_joint_parameter(self.cfg.armature, armature)
self.friction = self._parse_joint_parameter(self.cfg.friction, friction)
# parse joint limits
# -- velocity
self.velocity_limit_sim = self._parse_joint_parameter(self.cfg.velocity_limit_sim, velocity_limit)
self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, self.velocity_limit_sim)
# -- effort
self.effort_limit_sim = self._parse_joint_parameter(self.cfg.effort_limit_sim, effort_limit)
self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, self.effort_limit_sim)

# create commands buffers for allocation
self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device)
self.applied_effort = torch.zeros_like(self.computed_effort)

def __str__(self) -> str:
"""Returns: A string representation of the actuator group."""
Expand All @@ -205,24 +93,27 @@ def __str__(self) -> str:
"""

@property
@abstractmethod
def num_joints(self) -> int:
"""Number of actuators in the group."""
return len(self._joint_names)
raise NotImplementedError

@property
@abstractmethod
def joint_names(self) -> list[str]:
"""Articulation's joint names that are part of the group."""
return self._joint_names
raise NotImplementedError

@property
@abstractmethod
def joint_indices(self) -> slice | torch.Tensor:
"""Articulation's joint indices that are part of the group.

Note:
If :obj:`slice(None)` is returned, then the group contains all the joints in the articulation.
We do this to avoid unnecessary indexing of the joints for performance reasons.
"""
return self._joint_indices
raise NotImplementedError

"""
Operations.
Expand Down Expand Up @@ -260,6 +151,11 @@ def compute(
Helper functions.
"""

@abstractmethod
def _record_actuator_resolution(self, cfg_val, new_val, usd_val, joint_names, joint_ids, actuator_param: str):
raise NotImplementedError

@abstractmethod
def _parse_joint_parameter(
self, cfg_value: float | dict[str, float] | None, default_value: float | torch.Tensor | None
) -> torch.Tensor:
Expand All @@ -279,47 +175,9 @@ def _parse_joint_parameter(
ValueError: If the parameter value is None and no default value is provided.
ValueError: If the default value tensor is the wrong shape.
"""
# create parameter buffer
param = torch.zeros(self._num_envs, self.num_joints, 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, 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)
else:
raise TypeError(
f"Invalid type for parameter value: {type(cfg_value)} for "
+ f"actuator on joints {self.joint_names}. Expected 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, torch.Tensor):
# if tensor, then use the same tensor for all joints
if default_value.shape == (self._num_envs, self.num_joints):
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)}"
)
else:
raise TypeError(
f"Invalid type for default value: {type(default_value)} for "
+ f"actuator on joints {self.joint_names}. Expected float or Tensor."
)
else:
raise ValueError("The parameter value is None and no default value is provided.")

return param
raise NotImplementedError

@abstractmethod
def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:
"""Clip the desired torques based on the motor limits.

Expand All @@ -329,4 +187,4 @@ def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:
Returns:
The clipped torques.
"""
return torch.clip(effort, min=-self.effort_limit, max=self.effort_limit)
raise NotImplementedError
44 changes: 26 additions & 18 deletions source/isaaclab/isaaclab/actuators/actuator_cfg.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,14 @@
from isaaclab.utils import configclass

from . import actuator_net, actuator_pd
from .actuator_base import ActuatorBase
from . import ControlMode


@configclass
class ActuatorBaseCfg:
"""Configuration for default actuators in an articulation."""

class_type: type[ActuatorBase] = MISSING
class_type: type = MISSING
"""The associated actuator class.

The class should inherit from :class:`isaaclab.actuators.ActuatorBase`.
Expand Down Expand Up @@ -109,16 +109,6 @@ class ActuatorBaseCfg:

"""

control_mode: Literal["position", "velocity", "none"] = "position"
"""Control mode of the actuator. Defaults to "position".

The control mode can be one of the following:

* ``"position"``: Position control
* ``"velocity"``: Velocity control
* ``"none"``: No control (used for explicit actuators or direct effort control)
"""

stiffness: dict[str, float] | float | None = MISSING
"""Stiffness gains (also known as p-gain) of the joints in the group.

Expand Down Expand Up @@ -151,18 +141,36 @@ class ActuatorBaseCfg:
"""

friction: dict[str, float] | float | None = None
r"""The friction coefficient of the joints in the group. Defaults to None.
r"""The static friction coefficient of the joints in the group. Defaults to None.

The joint 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 friction force that may be applied by the solver
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 friction effect is therefore
similar to static and Coulomb friction.
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.
"""

dynamic_friction: dict[str, float] | float | None = None
"""The dynamic friction coefficient of the joints in the group. Defaults to None.
"""

viscous_friction: dict[str, float] | float | None = None
"""The viscous friction coefficient of the joints in the group. Defaults to None.
"""

control_mode: ControlMode = ControlMode.POSITION
"""The control mode of the actuator. Defaults to :obj:`ControlMode.POSITION`.

The control mode can be one of the following:

If None, the joint friction is set to the value from the USD joint prim.
* ``"position"``: position control
* ``"velocity"``: velocity control
* ``"effort"``: effort control
"""


Expand Down
Loading