Skip to content

Commit

Permalink
Cleans up the buffer-related implementations (#562)
Browse files Browse the repository at this point in the history
* Adds a lot of missing docstrings related to buffers and new actuator
models
* Nitpicks clean ups to make it simpler to read
* Adds unit test for the circular buffer class

## Type of change

- Breaking change (fix or feature that would cause existing
functionality to not work as expected)
- This change requires a documentation update

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have run all the tests with `./isaaclab.sh --test` and they pass
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there
  • Loading branch information
Mayankm96 authored Jun 25, 2024
1 parent 0154535 commit e28eeb7
Show file tree
Hide file tree
Showing 19 changed files with 708 additions and 295 deletions.
33 changes: 32 additions & 1 deletion docs/source/api/lab/omni.isaac.lab.actuators.rst
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,10 @@
IdealPDActuatorCfg
DCMotor
DCMotorCfg
DelayedPDActuator
DelayedPDActuatorCfg
RemotizedPDActuator
RemotizedPDActuatorCfg
ActuatorNetMLP
ActuatorNetMLPCfg
ActuatorNetLSTM
Expand Down Expand Up @@ -74,6 +78,34 @@ DC Motor Actuator
:show-inheritance:
:exclude-members: __init__, class_type

Delayed PD Actuator
-------------------

.. autoclass:: DelayedPDActuator
:members:
:inherited-members:
:show-inheritance:

.. autoclass:: DelayedPDActuatorCfg
:members:
:inherited-members:
:show-inheritance:
:exclude-members: __init__, class_type

Remotized PD Actuator
---------------------

.. autoclass:: RemotizedPDActuator
:members:
:inherited-members:
:show-inheritance:

.. autoclass:: RemotizedPDActuatorCfg
:members:
:inherited-members:
:show-inheritance:
:exclude-members: __init__, class_type

MLP Network Actuator
---------------------

Expand All @@ -88,7 +120,6 @@ MLP Network Actuator
:show-inheritance:
:exclude-members: __init__, class_type


LSTM Network Actuator
---------------------

Expand Down
20 changes: 20 additions & 0 deletions docs/source/api/lab/omni.isaac.lab.utils.rst
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,9 @@
io
array
assets
buffers
dict
interpolation
math
noise
string
Expand Down Expand Up @@ -52,13 +54,31 @@ Asset operations
:members:
:show-inheritance:

Buffer operations
~~~~~~~~~~~~~~~~~

.. automodule:: omni.isaac.lab.utils.buffers
:members:
:imported-members:
:inherited-members:
:show-inheritance:

Dictionary operations
~~~~~~~~~~~~~~~~~~~~~

.. automodule:: omni.isaac.lab.utils.dict
:members:
:show-inheritance:

Interpolation operations
~~~~~~~~~~~~~~~~~~~~~~~~

.. automodule:: omni.isaac.lab.utils.interpolation
:members:
:imported-members:
:inherited-members:
:show-inheritance:

Math operations
~~~~~~~~~~~~~~~

Expand Down
2 changes: 1 addition & 1 deletion source/extensions/omni.isaac.lab/config/extension.toml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
[package]

# Note: Semantic Versioning is used: https://semver.org/
version = "0.18.2"
version = "0.18.3"

# Description
title = "Isaac Lab framework for Robot Learning"
Expand Down
25 changes: 25 additions & 0 deletions source/extensions/omni.isaac.lab/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,6 +1,31 @@
Changelog
---------

0.18.3 (2024-06-25)
~~~~~~~~~~~~~~~~~~~

Fixed
^^^^^

* Fixed the docstrings at multiple places related to the different buffer implementations inside the
:mod:`omni.isaac.lab.utils.buffers` module. The docstrings were not clear and did not provide enough
information about the classes and their methods.

Added
^^^^^

* Added the field for fixed tendom names in the :class:`omni.isaac.lab.assets.ArticulationData` class.
Earlier, this information was not exposed which was inconsistent with other name related information
such as joint or body names.

Changed
^^^^^^^

* Renamed the fields ``min_num_time_lags`` and ``max_num_time_lags`` to ``min_delay`` and
``max_delay`` in the :class:`omni.isaac.lab.actuators.DelayedPDActuatorCfg` class. This is to make
the naming simpler to understand.


0.18.2 (2024-06-25)
~~~~~~~~~~~~~~~~~~~

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -162,18 +162,11 @@ class DelayedPDActuatorCfg(IdealPDActuatorCfg):

class_type: type = actuator_pd.DelayedPDActuator

min_num_time_lags: int = 0
"""Minimum number of physics time-steps that the actuator command may be delayed."""
min_delay: int = 0
"""Minimum number of physics time-steps with which the actuator command may be delayed. Defaults to 0."""

max_num_time_lags: int = 0
"""Maximum number of physics time-steps that the actuator command may be delayed."""

num_time_lags: int = 0
"""The number of physics time-steps that the actuator command will be delayed.
Note:
This values cannot be greater than `max_num_time_lags`.
"""
max_delay: int = 0
"""Maximum number of physics time-steps with which the actuator command may be delayed. Defaults to 0."""


@configclass
Expand All @@ -182,14 +175,15 @@ class RemotizedPDActuatorCfg(DelayedPDActuatorCfg):
Note:
The torque output limits for this actuator is derived from a linear interpolation of a lookup table
in :attr:`joint_parameter_lookup` describing the relationship between joint angles and the output torques.
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: torch.Tensor = MISSING
"""Joint parameter lookup table. Shape is (num_lookup_points, 3).
The tensor describes relationship between the joint angle (rad), the transmission ratio (in/out),
and the output torque (N*m).
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.
"""
Original file line number Diff line number Diff line change
Expand Up @@ -132,8 +132,7 @@ def compute(


class DCMotor(IdealPDActuator):
r"""
Direct control (DC) motor actuator model with velocity-based saturation model.
r"""Direct control (DC) motor actuator model with velocity-based saturation model.
It uses the same model as the :class:`IdealActuator` for computing the torques from input commands.
However, it implements a saturation model defined by DC motor characteristics.
Expand Down Expand Up @@ -221,65 +220,49 @@ def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:


class DelayedPDActuator(IdealPDActuator):
"""Ideal PD actuator with delayed data.
"""Ideal PD actuator with delayed command application.
The DelayedPDActuator has configurable minimum and maximum time lag values, which are used to initialize a
DelayBuffer to hold a queue of pending actuator commands. On reset, a value time_lags will be randomly sampled
from the min and max time lag bounds. At every physics step, the most recent actuation value is pushed to the
DelayBuffer, but the final actuation value applied to simulation will be `time_lags` physics steps in the past.
This class extends the :class:`IdealPDActuator` class by adding a delay to the actuator commands. The delay
is implemented using a circular buffer that stores the actuator commands for a certain number of physics steps.
The most recent actuation value is pushed to the buffer at every physics step, but the final actuation value
applied to the simulation is lagged by a certain number of physics steps.
The amount of time lag is configurable and can be set to a random value between the minimum and maximum time
lag bounds at every reset. The minimum and maximum time lag values are set in the configuration instance passed
to the class.
"""

def __init__(
self,
cfg: DelayedPDActuatorCfg,
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,
effort_limit: torch.Tensor | float = torch.inf,
velocity_limit: torch.Tensor | float = torch.inf,
):
super().__init__(
cfg,
joint_names,
joint_ids,
num_envs,
device,
stiffness,
damping,
armature,
friction,
effort_limit,
velocity_limit,
)
cfg: DelayedPDActuatorCfg
"""The configuration for the actuator model."""

def __init__(self, cfg: DelayedPDActuatorCfg, *args, **kwargs):
super().__init__(cfg, *args, **kwargs)
# instantiate the delay buffers
self.positions_delay_buffer = DelayBuffer(cfg.max_num_time_lags, num_envs=num_envs, device=device)
self.velocities_delay_buffer = DelayBuffer(cfg.max_num_time_lags, num_envs=num_envs, device=device)
self.efforts_delay_buffer = DelayBuffer(cfg.max_num_time_lags, num_envs=num_envs, device=device)
self.positions_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device)
self.velocities_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device)
self.efforts_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device)
# all of the envs
self._ALL_INDICES = torch.arange(num_envs, dtype=torch.long, device=device)
self._ALL_INDICES = torch.arange(self._num_envs, dtype=torch.long, device=self._device)

def reset(self, env_ids: Sequence[int]):
super().reset(env_ids)
# number of environments (since env_ids can be a slice)
env_size = self._ALL_INDICES[env_ids].size()
if env_ids is None or env_ids == slice(None):
num_envs = self._num_envs
else:
num_envs = len(env_ids)
# set a new random delay for environments in env_ids
time_lags = self.positions_delay_buffer.time_lags
time_lags[env_ids] = torch.randint(
low=self.cfg.min_num_time_lags,
high=self.cfg.max_num_time_lags + 1,
size=env_size,
device=self._device,
time_lags = torch.randint(
low=self.cfg.min_delay,
high=self.cfg.max_delay + 1,
size=(num_envs,),
dtype=torch.int,
device=self._device,
)
# set delays
self.positions_delay_buffer.set_time_lag(time_lags)
self.velocities_delay_buffer.set_time_lag(time_lags)
self.efforts_delay_buffer.set_time_lag(time_lags)
self.positions_delay_buffer.set_time_lag(time_lags, env_ids)
self.velocities_delay_buffer.set_time_lag(time_lags, env_ids)
self.efforts_delay_buffer.set_time_lag(time_lags, env_ids)
# reset buffers
self.positions_delay_buffer.reset(env_ids)
self.velocities_delay_buffer.reset(env_ids)
Expand All @@ -297,10 +280,13 @@ def compute(


class RemotizedPDActuator(DelayedPDActuator):
"""Ideal PD actuator with angle dependent torque limits.
"""Ideal PD actuator with angle-dependent torque limits.
This class extends the :class:`DelayedPDActuator` class by adding angle-dependent torque limits to the actuator.
The torque limits are applied by querying a lookup table describing the relationship between the joint angle
and the maximum output torque. The lookup table is provided in the configuration instance passed to the class.
The torque limits for this actuator are applied by querying a lookup table describing the relationship between
the joint angle and the maximum output torque.
The torque limits are interpolated based on the current joint positions and applied to the actuator commands.
"""

def __init__(
Expand Down Expand Up @@ -328,6 +314,10 @@ def __init__(
# define remotized joint torque limit
self._torque_limit = LinearInterpolation(self.angle_samples, self.max_torque_samples, device=device)

"""
Properties.
"""

@property
def angle_samples(self) -> torch.Tensor:
return self._joint_parameter_lookup[:, 0]
Expand All @@ -340,6 +330,10 @@ def transmission_ratio_samples(self) -> torch.Tensor:
def max_torque_samples(self) -> torch.Tensor:
return self._joint_parameter_lookup[:, 2]

"""
Operations.
"""

def compute(
self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor
) -> ArticulationActions:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,11 @@ def joint_names(self) -> list[str]:
"""Ordered names of joints in articulation."""
return self.root_physx_view.shared_metatype.dof_names

@property
def fixed_tendon_names(self) -> list[str]:
"""Ordered names of fixed tendons in articulation."""
return self._fixed_tendon_names

@property
def body_names(self) -> list[str]:
"""Ordered names of bodies in articulation."""
Expand Down Expand Up @@ -234,9 +239,10 @@ def find_fixed_tendons(
on the name matching.
Args:
name_keys: A regular expression or a list of regular expressions to match the joint names with fixed tendons.
tendon_subsets: A subset of joints with fixed tendons to search for. Defaults to None, which means all joints
in the articulation are searched.
name_keys: A regular expression or a list of regular expressions to match the joint
names with fixed tendons.
tendon_subsets: A subset of joints with fixed tendons to search for. Defaults to None, which means
all joints in the articulation are searched.
preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False.
Returns:
Expand Down Expand Up @@ -579,6 +585,10 @@ def set_joint_effort_target(
# set targets
self._data.joint_effort_target[env_ids, joint_ids] = target

"""
Operations - Tendons.
"""

def set_fixed_tendon_stiffness(
self,
stiffness: torch.Tensor,
Expand Down Expand Up @@ -1042,17 +1052,23 @@ def _process_actuators_cfg(self):

def _process_fixed_tendons(self):
"""Process fixed tendons."""
self.fixed_tendon_names = list()
# create a list to store the fixed tendon names
self._fixed_tendon_names = list()

# parse fixed tendons properties if they exist
if self.num_fixed_tendons > 0:
stage = stage_utils.get_current_stage()

# iterate over all joints to find tendons attached to them
for j in range(self.num_joints):
usd_joint_path = self.root_physx_view.dof_paths[0][j]
# check whether joint has tendons - tendon name follows the joint name it is attached to
joint = UsdPhysics.Joint.Get(stage, usd_joint_path)
if joint.GetPrim().HasAPI(PhysxSchema.PhysxTendonAxisRootAPI):
joint_name = usd_joint_path.split("/")[-1]
self.fixed_tendon_names.append(joint_name)
self._fixed_tendon_names.append(joint_name)

self._data.fixed_tendon_names = self._fixed_tendon_names
self._data.default_fixed_tendon_stiffness = self.root_physx_view.get_fixed_tendon_stiffnesses().clone()
self._data.default_fixed_tendon_damping = self.root_physx_view.get_fixed_tendon_dampings().clone()
self._data.default_fixed_tendon_limit_stiffness = (
Expand Down
Loading

0 comments on commit e28eeb7

Please sign in to comment.