Unverified Commit 3d836ab1 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Reverts the setting of joint velocity limits for implicit actuators (#1873)

# Description

The previous fix in #1654 was checking if `effort_limits_sim` is None
instead of checking `cfg.effort_limits_sim` for explicit actuators. This
did NOT work as effort limits sim is a tensor that gets assigned the
value on initialization.

The new fix now adds an implicit/explicit model attribute to the
actuator model to ensure the right defaults are getting set. It moves
all the implicit actuator warning code to its class to keep the
articulation class cleaner.

We also revert the behavior of setting velocity limits for implicit
actuators to the state before #1509. Before that change, the parameter
`velocity_limit` was set in the configurations but not getting passed
through. The MR #1509 allowed these values to be set which caused many
of the assets to not train anymore or behave differently between
IsaacLab versions. We now revert this behavior with a warning. If users
want to set the limits, they should use the `effort_limit_sim` and
`velocity_limit_sim` quantities.

Fixes #1837

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## Screenshot

The training of Allegro hand environment:
* Green: The current main at 6a415df2
* Black: This MR
* Orange: Commenting out the setting of `write_joint_velocity_to_sim`
which was introduced in #1509.


![image](https://github.com/user-attachments/assets/8ca1ded2-7d8f-4123-aea8-9082559885d7)


## 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
- [ ] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [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

---------
Signed-off-by: 's avatarMayank Mittal <12863862+Mayankm96@users.noreply.github.com>
Co-authored-by: 's avatarJames Tigue <166445701+jtigue-bdai@users.noreply.github.com>
parent 46594e08
......@@ -81,6 +81,7 @@ Breaking Changes
* Restructures extension folders and removes old imitation learning scripts by @kellyguo11
* Renames default conda and venv Python environment from ``isaaclab`` to ``env_isaaclab`` by @Toni-SM
Migration Guide
---------------
......
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.34.3"
version = "0.34.4"
# Description
title = "Isaac Lab framework for Robot Learning"
......
Changelog
---------
0.34.4 (2025-03-01)
~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added a new attribute :attr:`is_implicit_model` to the :class:`isaaclab.actuators.ActuatorBase` class to
indicate if the actuator model is implicit or explicit. This helps checking that the correct model type
is being used when initializing the actuator models.
Fixed
^^^^^
* Added copy of configurations to :class:`~isaaclab.assets.AssetBase` and :class:`~isaaclab.sensors.SensorBase`
to prevent modifications of the configurations from leaking outside of the classes.
* Fixed the case where setting velocity/effort limits for the simulation in the
:class:`~isaaclab.actuators.ActuatorBaseCfg` class was not being used to update the actuator-specific
velocity/effort limits.
Changed
^^^^^^^
* Moved warnings and checks for implicit actuator models to the :class:`~isaaclab.actuators.ImplicitActuator` class.
* Reverted to IsaacLab v1.3 behavior where :attr:`isaaclab.actuators.ImplicitActuatorCfg.velocity_limit`
attribute was not used for setting the velocity limits in the simulation. This makes it possible to deploy
policies from previous release without any changes. If users want to set the velocity limits for the simulation,
they should use the :attr:`isaaclab.actuators.ImplicitActuatorCfg.velocity_limit_sim` attribute instead.
0.34.3 (2025-02-28)
~~~~~~~~~~~~~~~~~~~
......@@ -13,7 +42,7 @@ Added
0.34.2 (2025-02-21)
~~~~~~~~~~~~~~~~~~~~
~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
......@@ -38,8 +67,8 @@ Fixed
Fixed
^^^^^
* Adds attributes velocity_limits_sim and effort_limits_sim to :class:`isaaclab.actuators.AssetBaseCfg` to separate
solver limits from actuator limits.
* Added attributes :attr:`velocity_limits_sim` and :attr:`effort_limits_sim` to the
:class:`isaaclab.actuators.ActuatorBaseCfg` class to separate solver limits from actuator limits.
0.33.17 (2025-02-13)
......
......@@ -8,7 +8,7 @@ from __future__ import annotations
import torch
from abc import ABC, abstractmethod
from collections.abc import Sequence
from typing import TYPE_CHECKING
from typing import TYPE_CHECKING, ClassVar
import isaaclab.utils.string as string_utils
from isaaclab.utils.types import ArticulationActions
......@@ -36,20 +36,55 @@ class ActuatorBase(ABC):
To see how the class is used, check the :class:`isaaclab.assets.Articulation` class.
"""
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)."""
"""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)."""
"""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)."""
"""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.
"""
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)."""
......@@ -57,7 +92,7 @@ class ActuatorBase(ABC):
self,
cfg: ActuatorBaseCfg,
joint_names: list[str],
joint_ids: slice | Sequence[int],
joint_ids: slice | torch.Tensor,
num_envs: int,
device: str,
stiffness: torch.Tensor | float = 0.0,
......@@ -69,9 +104,12 @@ class ActuatorBase(ABC):
):
"""Initialize the actuator.
Note:
The actuator parameters are parsed from the configuration and stored as buffers. If the parameters
are not specified in the configuration, then the default values provided in the arguments are used.
The actuator parameters are parsed from the configuration and stored as buffers. If the parameters
are not specified in the configuration, then their values provided in the constructor are used.
.. note::
The values in the constructor are typically obtained through the USD schemas corresponding
to the joints in the actuator model.
Args:
cfg: The configuration of the actuator model.
......@@ -100,6 +138,11 @@ class ActuatorBase(ABC):
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 ActuatorBase.is_implicit_model and self.cfg.effort_limit_sim is None:
self.cfg.effort_limit_sim = 1.0e9
# 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)
......@@ -107,11 +150,12 @@ class ActuatorBase(ABC):
self.armature = self._parse_joint_parameter(self.cfg.armature, armature)
self.friction = self._parse_joint_parameter(self.cfg.friction, friction)
# parse joint limits
# note: for velocity limits, we don't have USD parameter, so default is infinity
self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, effort_limit)
self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, velocity_limit)
self.effort_limit_sim = self._parse_joint_parameter(self.cfg.effort_limit_sim, effort_limit)
# -- 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)
......@@ -123,8 +167,12 @@ class ActuatorBase(ABC):
joint_indices = self.joint_indices
if joint_indices == slice(None):
joint_indices = list(range(self.num_joints))
# resolve model type (implicit or explicit)
model_type = "implicit" if self.is_implicit_model else "explicit"
return (
f"<class {self.__class__.__name__}> object:\n"
f"\tModel type : {model_type}\n"
f"\tNumber of joints : {self.num_joints}\n"
f"\tJoint names expression: {self.cfg.joint_names_expr}\n"
f"\tJoint names : {self.joint_names}\n"
......@@ -146,7 +194,7 @@ class ActuatorBase(ABC):
return self._joint_names
@property
def joint_indices(self) -> slice | Sequence[int]:
def joint_indices(self) -> slice | torch.Tensor:
"""Articulation's joint indices that are part of the group.
Note:
......
......@@ -36,10 +36,19 @@ class ActuatorBaseCfg:
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 ImplicitActuators this value will be collapsed with effort_limit_sim due to duplicating functionality. If
both are set the effort_limit_sim will be used priority.
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
......@@ -47,31 +56,53 @@ class ActuatorBaseCfg:
This limit is used by the actuator model. If None, the limit is set to the value specified in the USD joint prim.
.. note::
.. attention::
The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for the simulation physics
solver.
velocity_limit is not used in ActuatorBaseCfg but is provided for inherited version like
:class:`isaaclab.actuators.DCMotor`.
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 ImplicitActuators this value will be collapsed with velocity_limit_sim due to duplicating functionality. If
both are set the effort_limit_sim will be used priority.
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 Tensor API.
"""
effort_limit_sim: dict[str, float] | float | None = None
"""Force/Torque limit of the joints in the group that will be propagated to the simulation physics solver. Defaults to 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.
If None, the limit is set to the value specified in the USD joint prim for ImplicitActuators or 1.0e9 for explicit
actuators (e.g. IdealPDActuator). The simulation effort limits prevent computed torques from exceeding the specified
limit. If effort limits are too tight issues with solver convergence may occur. It is suggested to keep these value large.
"""
velocity_limit_sim: dict[str, float] | float | None = None
"""Velocity limit of the joints in the group that will be propagated to the simulation physics solver. Defaults to 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 :attr:`effort_limit_sim` 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.
If None, the limit is set to the value specified in the USD joint prim. Resulting solver solutions will constrain
velocities by these limits. If velocity_limit_sim is too tight issues with solver convergence may occur. It is
suggested to keep these value large.
"""
stiffness: dict[str, float] | float | None = MISSING
......
......@@ -9,6 +9,8 @@ import torch
from collections.abc import Sequence
from typing import TYPE_CHECKING
import omni.log
from isaaclab.utils import DelayBuffer, LinearInterpolation
from isaaclab.utils.types import ArticulationActions
......@@ -37,10 +39,10 @@ class ImplicitActuator(ActuatorBase):
generally more accurate than the explicit PD control law used in :class:`IdealPDActuator` when the simulation
time-step is large.
.. note::
The articulation class sets the stiffness and damping parameters from the configuration into the simulation.
Thus, the parameters are not used in this class.
The articulation class sets the stiffness and damping parameters from the implicit actuator configuration
into the simulation. Thus, the class does not perform its own computations on the joint action that
needs to be applied to the simulation. However, it computes the approximate torques for the actuated joint
since PhysX does not expose this quantity explicitly.
.. caution::
......@@ -51,6 +53,57 @@ class ImplicitActuator(ActuatorBase):
cfg: ImplicitActuatorCfg
"""The configuration for the actuator model."""
def __init__(self, cfg: ImplicitActuatorCfg, *args, **kwargs):
# effort limits
if cfg.effort_limit_sim is None and cfg.effort_limit is not None:
# throw a warning that we have a replacement for the deprecated parameter
omni.log.warn(
"The <ImplicitActuatorCfg> object has a value for 'effort_limit'."
" This parameter will be removed in the future."
" To set the effort limit, please use 'effort_limit_sim' instead."
)
cfg.effort_limit_sim = cfg.effort_limit
elif cfg.effort_limit_sim is not None and cfg.effort_limit is None:
# TODO: Eventually we want to get rid of 'effort_limit' for implicit actuators.
# We should do this once all parameters have an "_sim" suffix.
cfg.effort_limit = cfg.effort_limit_sim
elif cfg.effort_limit_sim is not None and cfg.effort_limit is not None:
if cfg.effort_limit_sim != cfg.effort_limit:
raise ValueError(
"The <ImplicitActuatorCfg> object has set both 'effort_limit_sim' and 'effort_limit'"
f" and they have different values {cfg.effort_limit_sim} != {cfg.effort_limit}."
" Please only set 'effort_limit_sim' for implicit actuators."
)
# velocity limits
if cfg.velocity_limit_sim is None and cfg.velocity_limit is not None:
# throw a warning that previously this was not set
# it leads to different simulation behavior so we want to remain backwards compatible
omni.log.warn(
"The <ImplicitActuatorCfg> object has a value for 'velocity_limit'."
" Previously, although this value was specified, it was not getting used by implicit"
" actuators. Since this parameter affects the simulation behavior, we continue to not"
" use it. This parameter will be removed in the future."
" To set the velocity limit, please use 'velocity_limit_sim' instead."
)
cfg.velocity_limit = None
elif cfg.velocity_limit_sim is not None and cfg.velocity_limit is None:
# TODO: Eventually we want to get rid of 'velocity_limit' for implicit actuators.
# We should do this once all parameters have an "_sim" suffix.
cfg.velocity_limit = cfg.velocity_limit_sim
elif cfg.velocity_limit_sim is not None and cfg.velocity_limit is not None:
if cfg.velocity_limit_sim != cfg.velocity_limit:
raise ValueError(
"The <ImplicitActuatorCfg> object has set both 'velocity_limit_sim' and 'velocity_limit'"
f" and they have different values {cfg.velocity_limit_sim} != {cfg.velocity_limit}."
" Please only set 'velocity_limit_sim' for implicit actuators."
)
# set implicit actuator model flag
ImplicitActuator.is_implicit_model = True
# call the base class
super().__init__(cfg, *args, **kwargs)
"""
Operations.
"""
......
......@@ -1312,14 +1312,18 @@ class Articulation(AssetBase):
f"No joints found for actuator group: {actuator_name} with joint name expression:"
f" {actuator_cfg.joint_names_expr}."
)
# resolve joint indices
# we pass a slice if all joints are selected to avoid indexing overhead
if len(joint_names) == self.num_joints:
joint_ids = slice(None)
else:
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,
joint_ids=(
slice(None) if len(joint_names) == self.num_joints else torch.tensor(joint_ids, device=self.device)
),
joint_ids=joint_ids,
num_envs=self.num_instances,
device=self.device,
stiffness=self._data.default_joint_stiffness[:, joint_ids],
......@@ -1330,60 +1334,33 @@ class Articulation(AssetBase):
velocity_limit=self.root_physx_view.get_dof_max_velocities().to(self.device).clone()[:, joint_ids],
)
# log information on actuator groups
model_type = "implicit" if actuator.is_implicit_model else "explicit"
omni.log.info(
f"Actuator collection: {actuator_name} with model '{actuator_cfg.class_type.__name__}' and"
f" joint names: {joint_names} [{joint_ids}]."
f"Actuator collection: {actuator_name} with model '{actuator_cfg.class_type.__name__}'"
f" (type: {model_type}) and joint names: {joint_names} [{joint_ids}]."
)
# store actuator group
self.actuators[actuator_name] = actuator
# set the passed gains and limits into the simulation
if isinstance(actuator, ImplicitActuator):
self._has_implicit_actuators = True
# resolve actuator limit duplication for ImplicitActuators
# effort limits
if actuator.effort_limit_sim is None and actuator.effort_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has effort_limit_sim=None but is specifying effort_limit."
"effort_limit will be applied to effort_limit_sim for ImplicitActuators."
)
actuator.effort_limit_sim = actuator.effort_limit
elif actuator.effort_limit_sim is not None and actuator.effort_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has set both effort_limit_sim and effort_limit."
"Only effort_limit_sim will be used for ImplicitActuators."
)
# velocity limits
if actuator.velocity_limit_sim is None and actuator.velocity_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has velocity_limit_sim=None but is specifying"
" velocity_limit.velocity_limit will be applied to velocity_limit_sim for ImplicitActuators."
)
actuator.velocity_limit_sim = actuator.velocity_limit
elif actuator.velocity_limit_sim is not None and actuator.velocity_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has set both velocity_limit_sim and velocity_limit."
"Only velocity_limit_sim will be used for ImplicitActuators."
)
# the gains and limits are set into the simulation since actuator model is implicit
self.write_joint_stiffness_to_sim(actuator.stiffness, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(actuator.damping, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(actuator.effort_limit_sim, joint_ids=actuator.joint_indices)
self.write_joint_velocity_limit_to_sim(actuator.velocity_limit_sim, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
else:
# the gains and limits are processed by the actuator model
# we set gains to zero, and torque limit to a high value in simulation to avoid any interference
self.write_joint_stiffness_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(
1.0e9 if actuator.effort_limit_sim is None else actuator.effort_limit_sim,
joint_ids=actuator.joint_indices,
)
self.write_joint_velocity_limit_to_sim(actuator.velocity_limit_sim, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
# Store the actual default stiffness and damping values for explicit and implicit actuators (not written the sim)
# Set common properties into the simulation
self.write_joint_effort_limit_to_sim(actuator.effort_limit_sim, joint_ids=actuator.joint_indices)
self.write_joint_velocity_limit_to_sim(actuator.velocity_limit_sim, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
# Store the actual default stiffness and damping values
# note: this is the value configured in the actuator model (for implicit and explicit actuators)
self._data.default_joint_stiffness[:, actuator.joint_indices] = actuator.stiffness
self._data.default_joint_damping[:, actuator.joint_indices] = actuator.damping
......@@ -1521,9 +1498,9 @@ class Articulation(AssetBase):
velocity_limits = self.root_physx_view.get_dof_max_velocities()[0].tolist()
effort_limits = self.root_physx_view.get_dof_max_forces()[0].tolist()
# create table for term information
table = PrettyTable(float_format=".3f")
table.title = f"Simulation Joint Information (Prim path: {self.cfg.prim_path})"
table.field_names = [
joint_table = PrettyTable()
joint_table.title = f"Simulation Joint Information (Prim path: {self.cfg.prim_path})"
joint_table.field_names = [
"Index",
"Name",
"Stiffness",
......@@ -1534,11 +1511,13 @@ class Articulation(AssetBase):
"Velocity Limits",
"Effort Limits",
]
joint_table.float_format = ".3"
joint_table.custom_format["Position Limits"] = lambda f, v: f"[{v[0]:.3f}, {v[1]:.3f}]"
# set alignment of table columns
table.align["Name"] = "l"
joint_table.align["Name"] = "l"
# add info on each term
for index, name in enumerate(self.joint_names):
table.add_row([
joint_table.add_row([
index,
name,
stiffnesses[index],
......@@ -1550,7 +1529,7 @@ class Articulation(AssetBase):
effort_limits[index],
])
# convert table to string
omni.log.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + table.get_string())
omni.log.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + joint_table.get_string())
# read out all tendon parameters from simulation
if self.num_fixed_tendons > 0:
......@@ -1563,17 +1542,19 @@ class Articulation(AssetBase):
ft_rest_lengths = self.root_physx_view.get_fixed_tendon_rest_lengths()[0].tolist()
ft_offsets = self.root_physx_view.get_fixed_tendon_offsets()[0].tolist()
# create table for term information
tendon_table = PrettyTable(float_format=".3f")
tendon_table.title = f"Simulation Tendon Information (Prim path: {self.cfg.prim_path})"
tendon_table = PrettyTable()
tendon_table.title = f"Simulation Fixed Tendon Information (Prim path: {self.cfg.prim_path})"
tendon_table.field_names = [
"Index",
"Stiffness",
"Damping",
"Limit Stiffness",
"Limit",
"Limits",
"Rest Length",
"Offset",
]
tendon_table.float_format = ".3"
joint_table.custom_format["Limits"] = lambda f, v: f"[{v[0]:.3f}, {v[1]:.3f}]"
# add info on each term
for index in range(self.num_fixed_tendons):
tendon_table.add_row([
......
......@@ -62,7 +62,7 @@ class AssetBase(ABC):
# check that the config is valid
cfg.validate()
# store inputs
self.cfg = cfg
self.cfg = cfg.copy()
# flag for whether the asset is initialized
self._is_initialized = False
......
......@@ -51,7 +51,7 @@ class SensorBase(ABC):
# check that the config is valid
cfg.validate()
# store inputs
self.cfg = cfg
self.cfg = cfg.copy()
# flag for whether the sensor is initialized
self._is_initialized = False
# flag for whether the sensor is in visualization mode
......
......@@ -21,14 +21,13 @@ simulation_app = app_launcher.app
import ctypes
import torch
import unittest
from typing import Literal
import isaacsim.core.utils.prims as prim_utils
import isaaclab.sim as sim_utils
import isaaclab.utils.math as math_utils
import isaaclab.utils.string as string_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg
from isaaclab.assets import Articulation, ArticulationCfg
from isaaclab.sim import build_simulation_context
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
......@@ -40,18 +39,32 @@ from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, SHADOW_HAND_CFG # i
def generate_articulation_cfg(
articulation_type: Literal["humanoid", "panda", "anymal", "shadow_hand", "single_joint"],
articulation_type: str,
stiffness: float | None = 10.0,
damping: float | None = 2.0,
vel_limit_sim: float | None = None,
velocity_limit: float | None = None,
effort_limit: float | None = None,
velocity_limit_sim: float | None = None,
effort_limit_sim: float | None = None,
) -> ArticulationCfg:
"""Generate an articulation configuration.
Args:
articulation_type: Type of articulation to generate.
stiffness: Stiffness value for the articulation's actuators. Only currently used for humanoid.
damping: Damping value for the articulation's actuators. Only currently used for humanoid.
It should be one of: "humanoid", "panda", "anymal", "shadow_hand", "single_joint_implicit",
"single_joint_explicit".
stiffness: Stiffness value for the articulation's actuators. Only currently used for "humanoid".
Defaults to 10.0.
damping: Damping value for the articulation's actuators. Only currently used for "humanoid".
Defaults to 2.0.
velocity_limit: Velocity limit for the actuators. Only currently used for "single_joint_implicit"
and "single_joint_explicit".
effort_limit: Effort limit for the actuators. Only currently used for "single_joint_implicit"
and "single_joint_explicit".
velocity_limit_sim: Velocity limit for the actuators (set into the simulation).
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".
Returns:
The articulation configuration for the requested articulation type.
......@@ -69,14 +82,31 @@ def generate_articulation_cfg(
articulation_cfg = ANYMAL_C_CFG
elif articulation_type == "shadow_hand":
articulation_cfg = SHADOW_HAND_CFG
elif articulation_type == "single_joint":
elif articulation_type == "single_joint_implicit":
articulation_cfg = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd"),
actuators={
"joint": ImplicitActuatorCfg(
joint_names_expr=[".*"],
effort_limit_sim=effort_limit_sim,
velocity_limit_sim=vel_limit_sim,
velocity_limit_sim=velocity_limit_sim,
effort_limit=effort_limit,
velocity_limit=velocity_limit,
stiffness=0.0,
damping=10.0,
),
},
)
elif articulation_type == "single_joint_explicit":
articulation_cfg = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd"),
actuators={
"joint": IdealPDActuatorCfg(
joint_names_expr=[".*"],
effort_limit_sim=effort_limit_sim,
velocity_limit_sim=velocity_limit_sim,
effort_limit=effort_limit,
velocity_limit=velocity_limit,
stiffness=0.0,
damping=10.0,
),
......@@ -171,6 +201,12 @@ class TestArticulation(unittest.TestCase):
path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]
]
self.assertListEqual(prim_path_body_names, articulation.body_names)
# -- actuator type
for actuator_name, actuator in articulation.actuators.items():
is_implicit_model_cfg = isinstance(
articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg
)
self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg)
# Simulate physics
for _ in range(10):
......@@ -227,6 +263,12 @@ class TestArticulation(unittest.TestCase):
path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]
]
self.assertListEqual(prim_path_body_names, articulation.body_names)
# -- actuator type
for actuator_name, actuator in articulation.actuators.items():
is_implicit_model_cfg = isinstance(
articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg
)
self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg)
# Simulate physics
for _ in range(10):
......@@ -281,6 +323,12 @@ class TestArticulation(unittest.TestCase):
path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]
]
self.assertListEqual(prim_path_body_names, articulation.body_names)
# -- actuator type
for actuator_name, actuator in articulation.actuators.items():
is_implicit_model_cfg = isinstance(
articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg
)
self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg)
# Simulate physics
for _ in range(10):
......@@ -302,7 +350,7 @@ class TestArticulation(unittest.TestCase):
with self.subTest(num_articulations=num_articulations, device=device):
with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim:
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(articulation_type="single_joint")
articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit")
articulation, translations = generate_articulation(articulation_cfg, num_articulations, device)
# Check that boundedness of articulation is correct
......@@ -341,6 +389,12 @@ class TestArticulation(unittest.TestCase):
path.split("/")[-1] for path in articulation.root_physx_view.link_paths[0]
]
self.assertListEqual(prim_path_body_names, articulation.body_names)
# -- actuator type
for actuator_name, actuator in articulation.actuators.items():
is_implicit_model_cfg = isinstance(
articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg
)
self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg)
# Simulate physics
for _ in range(10):
......@@ -396,6 +450,12 @@ class TestArticulation(unittest.TestCase):
articulation.root_physx_view.max_links,
articulation.root_physx_view.shared_metatype.link_count,
)
# -- actuator type
for actuator_name, actuator in articulation.actuators.items():
is_implicit_model_cfg = isinstance(
articulation_cfg.actuators[actuator_name], ImplicitActuatorCfg
)
self.assertEqual(actuator.is_implicit_model, is_implicit_model_cfg)
# Simulate physics
for _ in range(10):
......@@ -900,44 +960,312 @@ class TestArticulation(unittest.TestCase):
torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness)
torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping)
def test_setting_velocity_sim_limits(self):
"""Test that velocity limits are loaded form the configuration correctly."""
def test_setting_velocity_limit_implicit(self):
"""Test setting of velocity limit for implicit actuators.
This test checks that the behavior of setting velocity limits are consistent for implicit actuators
with previously defined behaviors.
We do not set velocity limit to simulation when `velocity_limit` is specified. This is mainly for backwards
compatibility. To set the velocity limit to simulation, users should set `velocity_limit_sim`.
"""
for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"):
for limit in (5.0, None):
with self.subTest(num_articulations=num_articulations, device=device, limit=limit):
with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True
) as sim:
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(
articulation_type="single_joint", vel_limit_sim=limit, effort_limit_sim=limit
)
articulation, _ = generate_articulation(
articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device
)
# Play sim
sim.reset()
if limit is not None:
# Expected gains
expected_velocity_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
limit,
device=articulation.device,
for vel_limit_sim in (1e5, None):
for vel_limit in (1e2, None):
with self.subTest(
num_articulations=num_articulations,
device=device,
vel_limit_sim=vel_limit_sim,
vel_limit=vel_limit,
):
with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True
) as sim:
# create simulation
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(
articulation_type="single_joint_implicit",
velocity_limit_sim=vel_limit_sim,
velocity_limit=vel_limit,
)
# Check that gains are loaded from USD file
articulation, _ = generate_articulation(
articulation_cfg=articulation_cfg,
num_articulations=num_articulations,
device=device,
)
# Play sim
sim.reset()
if vel_limit_sim is not None and vel_limit is not None:
# Case 1: during initialization, the actuator will raise a ValueError and fail to
# initialize when both these attributes are set.
# note: The Exception is not caught with self.assertRaises or try-except
self.assertTrue(len(articulation.actuators) == 0)
continue
# read the values set into the simulation
physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device)
# check data buffer
torch.testing.assert_close(articulation.data.joint_velocity_limits, physx_vel_limit)
# check actuator has simulation velocity limit
torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit_sim, expected_velocity_limit
articulation.actuators["joint"].velocity_limit_sim, physx_vel_limit
)
if vel_limit_sim is None:
# Case 2: both velocity limit and velocity limit sim are not set
# This is the case where the velocity limit is set to the USD default value
# Case 3: velocity limit sim is not set but velocity limit is set
# For backwards compatibility, we do not set velocity limit to simulation
# Thus, both default to USD default value.
# check to make sure the root_physx_view dof limit is not modified
self.assertGreater(physx_vel_limit[0].item(), 1e10)
# check the two are parsed and equal
torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit_sim,
articulation.actuators["joint"].velocity_limit,
)
# check the value is set to USD default
torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit, physx_vel_limit
)
else:
# Case 4: only velocity limit sim is set
# In this case, the velocity limit is set to the USD default value
# read the values set into the simulation
expected_velocity_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
vel_limit_sim,
device=articulation.device,
)
# check root_physx_view
torch.testing.assert_close(physx_vel_limit, expected_velocity_limit)
torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit_sim,
articulation.actuators["joint"].velocity_limit,
)
def test_setting_velocity_limit_explicit(self):
"""Test setting of velocity limit for explicit actuators.
This test checks that the behavior of setting velocity limits are consistent for explicit actuators
with previously defined behaviors.
Velocity limits to simulation for explicit actuators are only configured through `velocity_limit_sim`.
"""
for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"):
for vel_limit_sim in (1e5, None):
for vel_limit in (1e2, None):
with self.subTest(
num_articulations=num_articulations,
device=device,
vel_limit_sim=vel_limit_sim,
vel_limit=vel_limit,
):
with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True
) as sim:
# create simulation
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(
articulation_type="single_joint_explicit",
velocity_limit_sim=vel_limit_sim,
velocity_limit=vel_limit,
)
articulation, _ = generate_articulation(
articulation_cfg=articulation_cfg,
num_articulations=num_articulations,
device=device,
)
# Play sim
sim.reset()
# collect limit init values
physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device)
actuator_vel_limit = articulation.actuators["joint"].velocity_limit
actuator_vel_limit_sim = articulation.actuators["joint"].velocity_limit_sim
# check data buffer for joint_velocity_limits_sim
torch.testing.assert_close(articulation.data.joint_velocity_limits, physx_vel_limit)
# check actuator velocity_limit_sim is set to physx
torch.testing.assert_close(actuator_vel_limit_sim, physx_vel_limit)
if vel_limit is not None:
expected_actuator_vel_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
vel_limit,
device=articulation.device,
)
# check actuator is set
torch.testing.assert_close(actuator_vel_limit, expected_actuator_vel_limit)
# check physx is not velocity_limit
self.assertFalse(torch.allclose(actuator_vel_limit, physx_vel_limit))
else:
# check actuator velocity_limit is the same as the PhysX default
torch.testing.assert_close(actuator_vel_limit, physx_vel_limit)
if vel_limit_sim is not None:
expected_vel_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
vel_limit_sim,
device=articulation.device,
)
# check actuator is set
# check physx is set to expected value
torch.testing.assert_close(physx_vel_limit, expected_vel_limit)
else:
# check physx is not set by vel_limit_sim
self.assertGreater(physx_vel_limit[0].item(), 1.0e9)
def test_setting_effort_limit_implicit(self):
"""Test setting of the effort limit for implicit actuators.
In this case, the `effort_limit` and `effort_limit_sim` are treated as equivalent parameters.
"""
for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"):
for effort_limit_sim in (1e5, None):
for effort_limit in (1e2, None):
with self.subTest(
num_articulations=num_articulations,
device=device,
effort_limit_sim=effort_limit_sim,
effort_limit=effort_limit,
):
with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True
) as sim:
# create simulation
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(
articulation_type="single_joint_implicit",
effort_limit_sim=effort_limit_sim,
effort_limit=effort_limit,
)
articulation, _ = generate_articulation(
articulation_cfg=articulation_cfg,
num_articulations=num_articulations,
device=device,
)
# Play sim
sim.reset()
if effort_limit_sim is not None and effort_limit is not None:
# during initialization, the actuator will raise a ValueError and fail to
# initialize. The Exception is not caught with self.assertRaises or try-except
self.assertTrue(len(articulation.actuators) == 0)
continue
# obtain the physx effort limits
physx_effort_limit = articulation.root_physx_view.get_dof_max_forces()
physx_effort_limit = physx_effort_limit.to(device=device)
# check that the two are equivalent
torch.testing.assert_close(
articulation.data.joint_velocity_limits, expected_velocity_limit
articulation.actuators["joint"].effort_limit_sim,
articulation.actuators["joint"].effort_limit,
)
torch.testing.assert_close(
articulation.root_physx_view.get_dof_max_velocities().to(device),
expected_velocity_limit,
articulation.actuators["joint"].effort_limit_sim, physx_effort_limit
)
if effort_limit_sim is None and effort_limit is None:
# check to make sure the root_physx_view does not match either:
# effort_limit or effort_limit_sim
self.assertNotEqual(physx_effort_limit[0].item(), effort_limit_sim)
self.assertNotEqual(physx_effort_limit[0].item(), effort_limit)
else:
# decide the limit based on what is set
if effort_limit_sim is not None and effort_limit is None:
limit = effort_limit_sim
elif effort_limit_sim is None and effort_limit is not None:
limit = effort_limit
expected_effort_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
limit,
device=articulation.device,
)
# check root_physx_view
torch.testing.assert_close(physx_effort_limit, expected_effort_limit)
def test_setting_effort_limit_explicit(self):
"""Test setting of effort limit for explicit actuators.
This test checks that the behavior of setting effort limits are consistent for explicit actuators
with previously defined behaviors.
Effort limits to simulation for explicit actuators are only configured through `effort_limit_sim`.
"""
for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"):
for effort_limit_sim in (1e5, None):
for effort_limit in (1e2, None):
with self.subTest(
num_articulations=num_articulations,
device=device,
effort_limit_sim=effort_limit_sim,
effort_limit=effort_limit,
):
with build_simulation_context(
device=device, add_ground_plane=False, auto_add_lighting=True
) as sim:
# create simulation
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(
articulation_type="single_joint_explicit",
effort_limit_sim=effort_limit_sim,
effort_limit=effort_limit,
)
articulation, _ = generate_articulation(
articulation_cfg=articulation_cfg,
num_articulations=num_articulations,
device=device,
)
# Play sim
sim.reset()
# collect limit init values
physx_effort_limit = articulation.root_physx_view.get_dof_max_forces().to(device)
actuator_effort_limit = articulation.actuators["joint"].effort_limit
actuator_effort_limit_sim = articulation.actuators["joint"].effort_limit_sim
# check actuator effort_limit_sim is set to physx
torch.testing.assert_close(actuator_effort_limit_sim, physx_effort_limit)
if effort_limit is not None:
expected_actuator_effort_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
effort_limit,
device=articulation.device,
)
# check actuator is set
torch.testing.assert_close(actuator_effort_limit, expected_actuator_effort_limit)
# check physx is not effort_limit
# both USD and effort_limit_sim are larger than effort_limit
self.assertGreater(physx_effort_limit[0].item(), effort_limit)
else:
# check actuator effort_limit is the same as the PhysX default
torch.testing.assert_close(actuator_effort_limit, physx_effort_limit)
if effort_limit_sim is not None:
expected_effort_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
effort_limit_sim,
device=articulation.device,
)
# check physx is set to expected value
torch.testing.assert_close(physx_effort_limit, expected_effort_limit)
else:
# check physx is not set by vel_limit_sim
self.assertAlmostEqual(physx_effort_limit[0].item(), 1.0e9)
def test_reset(self):
"""Test that reset method works properly.
......@@ -970,6 +1298,7 @@ class TestArticulation(unittest.TestCase):
self.assertEqual(torch.count_nonzero(articulation._external_torque_b), 0)
def test_apply_joint_command(self):
"""Test applying of joint position target functions correctly for a robotic arm."""
for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"):
with self.subTest(num_articulations=num_articulations, device=device):
......@@ -1011,7 +1340,7 @@ class TestArticulation(unittest.TestCase):
assert not torch.allclose(articulation.data.joint_pos, joint_pos)
def test_body_root_state(self):
"""Test for the root_state_w property"""
"""Test for reading the `body_state_w` property"""
for num_articulations in (1, 2):
# for num_articulations in ( 2,):
for device in ("cuda:0", "cpu"):
......@@ -1023,7 +1352,7 @@ class TestArticulation(unittest.TestCase):
device=device, add_ground_plane=False, auto_add_lighting=True
) as sim:
sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg(articulation_type="single_joint")
articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit")
articulation, env_pos = generate_articulation(articulation_cfg, num_articulations, device)
env_idx = torch.tensor([x for x in range(num_articulations)])
# Check that boundedness of articulation is correct
......@@ -1195,30 +1524,6 @@ class TestArticulation(unittest.TestCase):
elif state_location == "link":
torch.testing.assert_close(rand_state, articulation.data.root_link_state_w)
def test_transform_inverses(self):
"""Test if math utilities are true inverses of each other."""
pose01 = torch.rand(1, 7)
pose01[:, 3:7] = torch.nn.functional.normalize(pose01[..., 3:7], dim=-1)
pose12 = torch.rand(1, 7)
pose12[:, 3:7] = torch.nn.functional.normalize(pose12[..., 3:7], dim=-1)
pos02, quat02 = math_utils.combine_frame_transforms(
pose01[..., :3], pose01[..., 3:7], pose12[:, :3], pose12[:, 3:7]
)
pos01, quat01 = math_utils.combine_frame_transforms(
pos02,
quat02,
math_utils.quat_rotate(math_utils.quat_inv(pose12[:, 3:7]), -pose12[:, :3]),
math_utils.quat_inv(pose12[:, 3:7]),
)
print("")
print(pose01)
print(pos01, quat01)
torch.testing.assert_close(pose01, torch.cat((pos01, quat01), dim=-1))
if __name__ == "__main__":
run_tests()
......@@ -20,21 +20,19 @@ simulation_app = AppLauncher(headless=True).app
import math
import numpy as np
import scipy.spatial.transform as scipy_tf
import torch
import torch.utils.benchmark as benchmark
from math import pi as PI
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp
import isaaclab.utils.math as math_utils
# Number of iterations to run the batched tests
NUM_ITERS = 100
# This value is set to because "float operations are inexact".
# Details: https://github.com/pytorch/pytorch/issues/17678
DECIMAL_PRECISION = 5
ATOL = 10 ** (-DECIMAL_PRECISION)
RTOL = 10 ** (-DECIMAL_PRECISION)
"""Precision of the test.
This value is used since float operations are inexact. For reference:
https://github.com/pytorch/pytorch/issues/17678
"""
class TestMathUtilities(unittest.TestCase):
......@@ -446,54 +444,76 @@ class TestMathUtilities(unittest.TestCase):
# Assert that the output is close to the expected result
torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth)
"""
Tests for math_utils.pose_inv function
This class checks the pose_inv function's output against the np.linalg.inv function's output.
1. test_single_numpy_comparison: Checks if the inverse of a random transformation matrix
matches NumPy's built-in inverse.
2. test_multi_numpy_comparison: Verifies the same for a batch of NUM_ITERS random matrices.
"""
def test_combine_frame_transform(self):
"""Test combine_frame_transforms function."""
for device in ["cpu", "cuda:0"]:
# create random poses
pose01 = torch.rand(1, 7, device=device)
pose01[:, 3:7] = torch.nn.functional.normalize(pose01[..., 3:7], dim=-1)
pose12 = torch.rand(1, 7, device=device)
pose12[:, 3:7] = torch.nn.functional.normalize(pose12[..., 3:7], dim=-1)
# apply combination of poses
pos02, quat02 = math_utils.combine_frame_transforms(
pose01[..., :3], pose01[..., 3:7], pose12[:, :3], pose12[:, 3:7]
)
# apply combination of poses w.r.t. inverse to get original frame
pos01, quat01 = math_utils.combine_frame_transforms(
pos02,
quat02,
math_utils.quat_rotate(math_utils.quat_inv(pose12[:, 3:7]), -pose12[:, :3]),
math_utils.quat_inv(pose12[:, 3:7]),
)
torch.testing.assert_close(pose01, torch.cat((pos01, quat01), dim=-1))
def test_single_numpy_comparison(self):
for _ in range(NUM_ITERS):
def test_pose_inv(self):
"""Test pose_inv function.
This test checks the output from the :meth:`~isaaclab.utils.math_utils.pose_inv` function against
the output from :func:`np.linalg.inv`. Two test cases are performed:
1. Checking the inverse of a random transformation matrix matches Numpy's built-in inverse.
2. Checking the inverse of a batch of random transformation matrices matches Numpy's built-in inverse.
"""
# Check against a single matrix
for _ in range(100):
test_mat = math_utils.generate_random_transformation_matrix(pos_boundary=10, rot_boundary=(2 * np.pi))
result = np.array(math_utils.pose_inv(test_mat))
expected = np.linalg.inv(np.array(test_mat))
np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION)
def test_multi_numpy_comparison(self):
# Generate NUM_ITERS random transformation matrices
# Check against a batch of matrices
test_mats = torch.stack([
math_utils.generate_random_transformation_matrix(pos_boundary=10, rot_boundary=(2 * math.pi))
for _ in range(NUM_ITERS)
for _ in range(100)
])
result = np.array(math_utils.pose_inv(test_mats))
expected = np.linalg.inv(np.array(test_mats))
np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION)
"""
Tests for math_utils.quat_slerp function.
This class checks the quat_slerp function's output against the output from scipy.spatial.transform.Slerp.
1. test_quat_slerp_multi_scipy_comparison: Generates 20x2 random rotation matrices, find the interpolation rotation
and compares it to the output of scipy.spatial.transform.Slerp.
"""
def test_quat_slerp(self):
"""Test quat_slerp function.
def test_quat_slerp_multi_scipy_comparison(self):
# Generate NUM_ITERS random rotation matrices
random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(NUM_ITERS)]
random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(NUM_ITERS)]
This test checks the output from the :meth:`~isaaclab.utils.math_utils.quat_slerp` function against
the output from :func:`scipy.spatial.transform.Slerp`.
"""
# Generate 100 random rotation matrices
random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(100)]
random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(100)]
tau_values = np.random.rand(10) # Random values in the range [0, 1]
for rmat1, rmat2 in zip(random_rotation_matrices_1, random_rotation_matrices_2):
# Convert the rotation matrices to quaternions
q1 = R.from_matrix(rmat1).as_quat() # (x, y, z, w)
q2 = R.from_matrix(rmat2).as_quat() # (x, y, z, w)
q1 = scipy_tf.Rotation.from_matrix(rmat1).as_quat() # (x, y, z, w)
q2 = scipy_tf.Rotation.from_matrix(rmat2).as_quat() # (x, y, z, w)
# Compute expected results using scipy's Slerp
key_rots = R.from_quat(np.array([q1, q2]))
key_rots = scipy_tf.Rotation.from_quat(np.array([q1, q2]))
key_times = [0, 1]
slerp = Slerp(key_times, key_rots)
slerp = scipy_tf.Slerp(key_times, key_rots)
for tau in tau_values:
expected = slerp(tau).as_quat() # (x, y, z, w)
......@@ -501,27 +521,25 @@ class TestMathUtilities(unittest.TestCase):
# Assert that the result is almost equal to the expected quaternion
np.testing.assert_array_almost_equal(result, expected, decimal=DECIMAL_PRECISION)
"""
Tests for math_utils.interpolate_rotations function.
This class checks the interpolate_rotations function's output against the output from scipy.spatial.transform.Slerp.
1. test_interpolate_rotations_multi_scipy_comparison: Generates 20x2 random rotation matrices, finds an array of interpolated rotations
and compares it to the output of scipy.spatial.transform.Slerp.
"""
def test_interpolate_rotations(self):
"""Test interpolate_rotations function.
def test_interpolate_rotations_multi_scipy_comparison(self):
This test checks the output from the :meth:`~isaaclab.utils.math_utils.interpolate_rotations` function against
the output from :func:`scipy.spatial.transform.Slerp`.
"""
# Generate NUM_ITERS random rotation matrices
random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(NUM_ITERS)]
random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(NUM_ITERS)]
random_rotation_matrices_1 = [math_utils.generate_random_rotation() for _ in range(100)]
random_rotation_matrices_2 = [math_utils.generate_random_rotation() for _ in range(100)]
for rmat1, rmat2 in zip(random_rotation_matrices_1, random_rotation_matrices_2):
# Compute expected results using scipy's Slerp
key_rots = R.from_matrix(np.array([rmat1, rmat2]))
key_rots = scipy_tf.Rotation.from_matrix(np.array([rmat1, rmat2]))
# Create a Slerp object and interpolate create the interpolated matrices
# Minimum 2 required because Interpolate_rotations returns one extra rotation matrix
num_steps = np.random.randint(2, 51)
key_times = [0, 1]
slerp = Slerp(key_times, key_rots)
slerp = scipy_tf.Slerp(key_times, key_rots)
interp_times = np.linspace(0, 1, num_steps)
expected = slerp(interp_times).as_matrix()
......@@ -541,31 +559,28 @@ class TestMathUtilities(unittest.TestCase):
# Assert that the result is almost equal to the expected quaternion
np.testing.assert_array_almost_equal(result_axis_angle, expected, decimal=DECIMAL_PRECISION)
"""
Tests for math_utils.interpolate_poses function.
This class checks the interpolate_poses function's output against the output from scipy.spatial.transform.Slerp.
1. test_interpolate_poses_multi_scipy_comparison: Generates 20x2 random transformation matrices,
computes an array of interpolated transformations
and compares it to the output of scipy.spatial.transform.Slerp and np.linspace
"""
def test_interpolate_poses(self):
"""Test interpolate_poses function.
def test_interpolate_poses_multi_scipy_comparison(self):
# Generate NUM_ITERS random transformation matrices
random_mat_1 = [math_utils.generate_random_transformation_matrix() for _ in range(NUM_ITERS)]
random_mat_2 = [math_utils.generate_random_transformation_matrix() for _ in range(NUM_ITERS)]
This test checks the output from the :meth:`~isaaclab.utils.math_utils.interpolate_poses` function against
the output from :func:`scipy.spatial.transform.Slerp` and :func:`np.linspace`.
"""
# Generate 100 random transformation matrices
random_mat_1 = [math_utils.generate_random_transformation_matrix() for _ in range(100)]
random_mat_2 = [math_utils.generate_random_transformation_matrix() for _ in range(100)]
for mat1, mat2 in zip(random_mat_1, random_mat_2):
pos_1, rmat1 = math_utils.unmake_pose(mat1)
pos_2, rmat2 = math_utils.unmake_pose(mat2)
# Compute expected results using scipy's Slerp
key_rots = R.from_matrix(np.array([rmat1, rmat2]))
key_rots = scipy_tf.Rotation.from_matrix(np.array([rmat1, rmat2]))
# Create a Slerp object and interpolate create the interpolated rotation matrices
# Minimum 3 required because interpolate_poses returns extra staring and ending pose matrices
num_steps = np.random.randint(3, 51)
key_times = [0, 1]
slerp = Slerp(key_times, key_rots)
slerp = scipy_tf.Slerp(key_times, key_rots)
interp_times = np.linspace(0, 1, num_steps)
expected_quat = slerp(interp_times).as_matrix()
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment