Unverified Commit 46cbb5d6 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Clarifies the default parameters in ArticulationData (#1875)

# Description

There was a lot of code duplication happening inside the `Articulation`
class. This MR takes a step towards untangling some of the confusion
that gets caused by default and non-default data attributes inside the
class.

The MR includes the following changes:

* Removes operations where we set the data to "torch.zeros" and in the
next function assign them the value from PhysX. This made the code
bulkier and complex for no reason.
* Adds docstring clarifications to make it clear what is what. The
default values are now whatever the user configures from their
configuration of the articulation class.
* Updates the `soft_joint_pos_limits` when the user writes the joint pos
limits to the simulator.
* Renames parameters for a consistent nomenclature:
* ``joint_velocity_limits`` → ``joint_vel_limits`` (to match attribute
``joint_vel`` and ``joint_vel_limits``)
* ``joint_limits`` → ``joint_pos_limits`` (to match attribute
``joint_pos`` and ``soft_joint_pos_limits``)
  * ``default_joint_limits`` → ``default_joint_pos_limits``
* ``write_joint_limits_to_sim`` → ``write_joint_position_limit_to_sim``
  * ``joint_friction`` → ``joint_friction_coeff``
  * ``default_joint_friction`` → ``default_joint_friction_coeff``
* ``write_joint_friction_to_sim`` →
``write_joint_friction_coefficient_to_sim``
  * ``fixed_tendon_limit`` → ``fixed_tendon_pos_limits``
  * ``default_fixed_tendon_limit`` → ``default_fixed_tendon_pos_limits``
  * ``set_fixed_tendon_limit`` → ``set_fixed_tendon_position_limit``

Fixes #1583, #1904

## Type of change

- Bug fix (non-breaking change which fixes an issue)
- 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
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] 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 b5fa0eb0
...@@ -55,7 +55,8 @@ ...@@ -55,7 +55,8 @@
"arange", "arange",
"discretization", "discretization",
"trimesh", "trimesh",
"uninstanceable" "uninstanceable",
"coeff"
], ],
// This enables python language server. Seems to work slightly better than jedi: // This enables python language server. Seems to work slightly better than jedi:
"python.languageServer": "Pylance", "python.languageServer": "Pylance",
......
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.34.9" version = "0.35.0"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.35.0 (2025-03-07)
~~~~~~~~~~~~~~~~~~~
* Improved documentation of various attributes in the :class:`~isaaclab.assets.ArticulationData` class to make
it clearer which values represent the simulation and internal class values. In the new convention,
the ``default_xxx`` attributes are whatever the user configured from their configuration of the articulation
class, while the ``xxx`` attributes are the values from the simulation.
* Updated the soft joint position limits inside the :meth:`~isaaclab.assets.Articulation.write_joint_pos_limits_to_sim`
method to use the new limits passed to the function.
* Added setting of :attr:`~isaaclab.assets.ArticulationData.default_joint_armature` and
:attr:`~isaaclab.assets.ArticulationData.default_joint_friction` attributes in the
:class:`~isaaclab.assets.Articulation` class based on user configuration.
Changed
^^^^^^^
* Removed unnecessary buffer creation operations inside the :class:`~isaaclab.assets.Articulation` class.
Earlier, the class initialized a variety of buffer data with zeros and in the next function assigned
them the value from PhysX. This made the code bulkier and more complex for no reason.
* Renamed parameters for a consistent nomenclature. These changes are backwards compatible with previous releases
with a deprecation warning for the old names.
* ``joint_velocity_limits`` → ``joint_vel_limits`` (to match attribute ``joint_vel`` and ``joint_vel_limits``)
* ``joint_limits`` → ``joint_pos_limits`` (to match attribute ``joint_pos`` and ``soft_joint_pos_limits``)
* ``default_joint_limits`` → ``default_joint_pos_limits``
* ``write_joint_limits_to_sim`` → ``write_joint_position_limit_to_sim``
* ``joint_friction`` → ``joint_friction_coeff``
* ``default_joint_friction`` → ``default_joint_friction_coeff``
* ``write_joint_friction_to_sim`` → ``write_joint_friction_coefficient_to_sim``
* ``fixed_tendon_limit`` → ``fixed_tendon_pos_limits``
* ``default_fixed_tendon_limit`` → ``default_fixed_tendon_pos_limits``
* ``set_fixed_tendon_limit`` → ``set_fixed_tendon_position_limit``
0.34.9 (2025-03-04) 0.34.9 (2025-03-04)
~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~
......
...@@ -33,53 +33,58 @@ class ActuatorBaseCfg: ...@@ -33,53 +33,58 @@ class ActuatorBaseCfg:
effort_limit: dict[str, float] | float | None = None effort_limit: dict[str, float] | float | None = None
"""Force/Torque limit of the joints in the group. Defaults to 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 This limit is used to clip the computed torque sent to the simulation. If None, the
specified in the USD joint prim. limit is set to the value specified in the USD joint prim.
.. attention:: .. attention::
The :attr:`effort_limit_sim` attribute should be used to set the effort limit for the simulation physics The :attr:`effort_limit_sim` attribute should be used to set the effort limit for
solver. the simulation physics solver.
The :attr:`effort_limit` attribute is used for clipping the effort output of the actuator model *only* The :attr:`effort_limit` attribute is used for clipping the effort output of the
in the case of explicit actuators, such as the :class:`~isaaclab.actuators.IdealPDActuator`. actuator model **only** in the case of explicit actuators, such as the
:class:`~isaaclab.actuators.IdealPDActuator`.
.. note:: .. note::
For implicit actuators, the attributes :attr:`effort_limit` and :attr:`effort_limit_sim` are equivalent. For implicit actuators, the attributes :attr:`effort_limit` and :attr:`effort_limit_sim`
However, we suggest using the :attr:`effort_limit_sim` attribute because it is more intuitive. 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: dict[str, float] | float | None = None
"""Velocity limit of the joints in the group. Defaults to 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. This limit is used by the actuator model. If None, the limit is set to the value specified
in the USD joint prim.
.. attention:: .. attention::
The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for the simulation physics The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for
solver. the simulation physics solver.
The :attr:`velocity_limit` attribute is used for clipping the effort output of the actuator model *only* The :attr:`velocity_limit` attribute is used for clipping the effort output of the
in the case of explicit actuators, such as the :class:`~isaaclab.actuators.IdealPDActuator`. actuator model **only** in the case of explicit actuators, such as the
:class:`~isaaclab.actuators.IdealPDActuator`.
.. note:: .. note::
For implicit actuators, the attribute :attr:`velocity_limit` is not used. This is to stay backwards compatible For implicit actuators, the attribute :attr:`velocity_limit` is not used. This is to stay
with previous versions of the Isaac Lab, where this parameter was unused since PhysX did not support setting backwards compatible with previous versions of the Isaac Lab, where this parameter was
the velocity limit for the joints using the Tensor API. 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_sim: dict[str, float] | float | None = None
"""Effort limit of the joints in the group applied 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 The effort limit is used to constrain the computed joint efforts in the physics engine. If the
exceeds this limit, the physics engine will clip the effort to this value. 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 Since explicit actuators (e.g. DC motor), compute and clip the effort in the actuator model, this
default set to a large value to prevent the physics engine from any additional clipping. However, at times, limit is by default set to a large value to prevent the physics engine from any additional clipping.
it may be necessary to set this limit to a smaller value as a safety measure. 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: If None, the limit is resolved based on the type of actuator model:
...@@ -91,40 +96,61 @@ class ActuatorBaseCfg: ...@@ -91,40 +96,61 @@ class ActuatorBaseCfg:
velocity_limit_sim: dict[str, float] | float | None = None 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. """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 velocity limit is used to constrain the joint velocities in the physics engine. The joint will only
The joint will only be able to reach this velocity if the :attr:`effort_limit_sim` is sufficiently large. be able to reach this velocity if the joint's effort limit is sufficiently large. If the joint is moving
If the joint is moving faster than this velocity, the physics engine will actually try to brake the joint faster than this velocity, the physics engine will actually try to brake the joint to reach this velocity.
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. If None, the limit is set to the value specified in the USD joint prim for both implicit and explicit actuators.
.. tip:: .. tip::
If the velocity limit is too tight, the physics engine may have trouble converging to a solution. 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 In such cases, we recommend either keeping this value sufficiently large or tuning the stiffness and
parameters of the joint to ensure the limits are not violated. damping parameters of the joint to ensure the limits are not violated.
""" """
stiffness: dict[str, float] | float | None = MISSING stiffness: dict[str, float] | float | None = MISSING
"""Stiffness gains (also known as p-gain) of the joints in the group. """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. If None, the stiffness is set to the value from the USD joint prim.
""" """
damping: dict[str, float] | float | None = MISSING damping: dict[str, float] | float | None = MISSING
"""Damping gains (also known as d-gain) of the joints in the group. """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. If None, the damping is set to the value from the USD joint prim.
""" """
armature: dict[str, float] | float | None = None armature: dict[str, float] | float | None = None
"""Armature of the joints in the group. Defaults to 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. If None, the armature is set to the value from the USD joint prim.
""" """
friction: dict[str, float] | float | None = None friction: dict[str, float] | float | None = None
"""Joint friction of the joints in the group. Defaults to None. r"""The 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
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.
If None, the joint friction is set to the value from the USD joint prim. If None, the joint friction is set to the value from the USD joint prim.
""" """
......
...@@ -267,7 +267,7 @@ class Articulation(AssetBase): ...@@ -267,7 +267,7 @@ class Articulation(AssetBase):
return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order) return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order)
""" """
Operations - Writers. Operations - State Writers.
""" """
def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None):
...@@ -550,6 +550,10 @@ class Articulation(AssetBase): ...@@ -550,6 +550,10 @@ class Articulation(AssetBase):
# set into simulation # set into simulation
self.root_physx_view.set_dof_velocities(self._data.joint_vel, indices=physx_env_ids) self.root_physx_view.set_dof_velocities(self._data.joint_vel, indices=physx_env_ids)
"""
Operations - Simulation Parameters Writers.
"""
def write_joint_stiffness_to_sim( def write_joint_stiffness_to_sim(
self, self,
stiffness: torch.Tensor | float, stiffness: torch.Tensor | float,
...@@ -589,10 +593,8 @@ class Articulation(AssetBase): ...@@ -589,10 +593,8 @@ class Articulation(AssetBase):
Args: Args:
damping: Joint damping. Shape is (len(env_ids), len(joint_ids)). damping: Joint damping. Shape is (len(env_ids), len(joint_ids)).
joint_ids: The joint indices to set the damping for. joint_ids: The joint indices to set the damping for. Defaults to None (all joints).
Defaults to None (all joints). env_ids: The environment indices to set the damping for. Defaults to None (all environments).
env_ids: The environment indices to set the damping for.
Defaults to None (all environments).
""" """
# note: This function isn't setting the values for actuator models. (#128) # note: This function isn't setting the values for actuator models. (#128)
# resolve indices # resolve indices
...@@ -610,6 +612,66 @@ class Articulation(AssetBase): ...@@ -610,6 +612,66 @@ class Articulation(AssetBase):
# set into simulation # set into simulation
self.root_physx_view.set_dof_dampings(self._data.joint_damping.cpu(), indices=physx_env_ids.cpu()) self.root_physx_view.set_dof_dampings(self._data.joint_damping.cpu(), indices=physx_env_ids.cpu())
def write_joint_position_limit_to_sim(
self,
limits: torch.Tensor | float,
joint_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
warn_limit_violation: bool = True,
):
"""Write joint position limits into the simulation.
Args:
limits: Joint limits. Shape is (len(env_ids), len(joint_ids), 2).
joint_ids: The joint indices to set the limits for. Defaults to None (all joints).
env_ids: The environment indices to set the limits for. Defaults to None (all environments).
warn_limit_violation: Whether to use warning or info level logging when default joint positions
exceed the new limits. Defaults to True.
"""
# note: This function isn't setting the values for actuator models. (#128)
# 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]
# set into internal buffers
self._data.joint_pos_limits[env_ids, joint_ids] = limits
# update default joint pos to stay within the new limits
if torch.any(
(self._data.default_joint_pos[env_ids, joint_ids] < limits[..., 0])
| (self._data.default_joint_pos[env_ids, joint_ids] > limits[..., 1])
):
self._data.default_joint_pos[env_ids, joint_ids] = torch.clamp(
self._data.default_joint_pos[env_ids, joint_ids], limits[..., 0], limits[..., 1]
)
violation_message = (
"Some default joint positions are outside of the range of the new joint limits. Default joint positions"
" will be clamped to be within the new joint limits."
)
if warn_limit_violation:
# warn level will show in console
omni.log.warn(violation_message)
else:
# info level is only written to log file
omni.log.info(violation_message)
# set into simulation
self.root_physx_view.set_dof_limits(self._data.joint_pos_limits.cpu(), indices=physx_env_ids.cpu())
# compute the soft limits based on the joint limits
# TODO: Optimize this computation for only selected joints
# soft joint position limits (recommended not to be too close to limits).
joint_pos_mean = (self._data.joint_pos_limits[..., 0] + self._data.joint_pos_limits[..., 1]) / 2
joint_pos_range = self._data.joint_pos_limits[..., 1] - self._data.joint_pos_limits[..., 0]
soft_limit_factor = self.cfg.soft_joint_pos_limit_factor
# add to data
self._data.soft_joint_pos_limits[..., 0] = joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor
self._data.soft_joint_pos_limits[..., 1] = joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor
def write_joint_velocity_limit_to_sim( def write_joint_velocity_limit_to_sim(
self, self,
limits: torch.Tensor | float, limits: torch.Tensor | float,
...@@ -618,6 +680,10 @@ class Articulation(AssetBase): ...@@ -618,6 +680,10 @@ class Articulation(AssetBase):
): ):
"""Write joint max velocity to the simulation. """Write joint max velocity to the simulation.
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.
Args: Args:
limits: Joint max velocity. Shape is (len(env_ids), len(joint_ids)). limits: Joint max velocity. Shape is (len(env_ids), len(joint_ids)).
joint_ids: The joint indices to set the max velocity for. Defaults to None (all joints). joint_ids: The joint indices to set the max velocity for. Defaults to None (all joints).
...@@ -636,12 +702,10 @@ class Articulation(AssetBase): ...@@ -636,12 +702,10 @@ class Articulation(AssetBase):
# move tensor to cpu if needed # move tensor to cpu if needed
if isinstance(limits, torch.Tensor): if isinstance(limits, torch.Tensor):
limits = limits.to(self.device) limits = limits.to(self.device)
# set into internal buffers # set into internal buffers
self._data.joint_velocity_limits = self.root_physx_view.get_dof_max_velocities().to(self.device) self._data.joint_vel_limits[env_ids, joint_ids] = limits
self._data.joint_velocity_limits[env_ids, joint_ids] = limits
# set into simulation # set into simulation
self.root_physx_view.set_dof_max_velocities(self._data.joint_velocity_limits.cpu(), indices=physx_env_ids.cpu()) self.root_physx_view.set_dof_max_velocities(self._data.joint_vel_limits.cpu(), indices=physx_env_ids.cpu())
def write_joint_effort_limit_to_sim( def write_joint_effort_limit_to_sim(
self, self,
...@@ -651,6 +715,9 @@ class Articulation(AssetBase): ...@@ -651,6 +715,9 @@ class Articulation(AssetBase):
): ):
"""Write joint effort limits into the simulation. """Write joint effort limits into the simulation.
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.
Args: Args:
limits: Joint torque limits. Shape is (len(env_ids), len(joint_ids)). limits: Joint torque limits. Shape is (len(env_ids), len(joint_ids)).
joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints).
...@@ -669,12 +736,11 @@ class Articulation(AssetBase): ...@@ -669,12 +736,11 @@ class Articulation(AssetBase):
env_ids = env_ids[:, None] env_ids = env_ids[:, None]
# move tensor to cpu if needed # move tensor to cpu if needed
if isinstance(limits, torch.Tensor): if isinstance(limits, torch.Tensor):
limits = limits.cpu() limits = limits.to(self.device)
# set into internal buffers # set into internal buffers
torque_limit_all = self.root_physx_view.get_dof_max_forces() self._data.joint_effort_limits[env_ids, joint_ids] = limits
torque_limit_all[env_ids, joint_ids] = limits
# set into simulation # set into simulation
self.root_physx_view.set_dof_max_forces(torque_limit_all.cpu(), indices=physx_env_ids.cpu()) self.root_physx_view.set_dof_max_forces(self._data.joint_effort_limits.cpu(), indices=physx_env_ids.cpu())
def write_joint_armature_to_sim( def write_joint_armature_to_sim(
self, self,
...@@ -684,6 +750,9 @@ class Articulation(AssetBase): ...@@ -684,6 +750,9 @@ class Articulation(AssetBase):
): ):
"""Write joint armature into the simulation. """Write joint armature into the simulation.
The armature is directly added to the corresponding joint-space inertia. It helps improve the
simulation stability by reducing the joint velocities.
Args: Args:
armature: Joint armature. Shape is (len(env_ids), len(joint_ids)). armature: Joint armature. Shape is (len(env_ids), len(joint_ids)).
joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints).
...@@ -704,13 +773,22 @@ class Articulation(AssetBase): ...@@ -704,13 +773,22 @@ class Articulation(AssetBase):
# set into simulation # set into simulation
self.root_physx_view.set_dof_armatures(self._data.joint_armature.cpu(), indices=physx_env_ids.cpu()) self.root_physx_view.set_dof_armatures(self._data.joint_armature.cpu(), indices=physx_env_ids.cpu())
def write_joint_friction_to_sim( def write_joint_friction_coefficient_to_sim(
self, self,
joint_friction: torch.Tensor | float, joint_friction_coeff: torch.Tensor | float,
joint_ids: Sequence[int] | slice | None = None, joint_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None,
): ):
"""Write joint friction into the simulation. r"""Write joint friction coefficients into the simulation.
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
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.
Args: Args:
joint_friction: Joint friction. Shape is (len(env_ids), len(joint_ids)). joint_friction: Joint friction. Shape is (len(env_ids), len(joint_ids)).
...@@ -728,58 +806,11 @@ class Articulation(AssetBase): ...@@ -728,58 +806,11 @@ class Articulation(AssetBase):
if env_ids != slice(None) and joint_ids != slice(None): if env_ids != slice(None) and joint_ids != slice(None):
env_ids = env_ids[:, None] env_ids = env_ids[:, None]
# set into internal buffers # set into internal buffers
self._data.joint_friction[env_ids, joint_ids] = joint_friction self._data.joint_friction_coeff[env_ids, joint_ids] = joint_friction_coeff
# set into simulation # set into simulation
self.root_physx_view.set_dof_friction_coefficients(self._data.joint_friction.cpu(), indices=physx_env_ids.cpu()) self.root_physx_view.set_dof_friction_coefficients(
self._data.joint_friction_coeff.cpu(), indices=physx_env_ids.cpu()
def write_joint_limits_to_sim(
self,
limits: torch.Tensor | float,
joint_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
warn_limit_violation: bool = True,
):
"""Write joint limits into the simulation.
Args:
limits: Joint limits. Shape is (len(env_ids), len(joint_ids), 2).
joint_ids: The joint indices to set the limits for. Defaults to None (all joints).
env_ids: The environment indices to set the limits for. Defaults to None (all environments).
warn_limit_violation: Whether to use warning or info level logging when default joint positions exceed the new limits. Defaults to True.
"""
# note: This function isn't setting the values for actuator models. (#128)
# 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]
# set into internal buffers
self._data.joint_limits[env_ids, joint_ids] = limits
# update default joint pos to stay within the new limits
if torch.any(
(self._data.default_joint_pos[env_ids, joint_ids] < limits[..., 0])
| (self._data.default_joint_pos[env_ids, joint_ids] > limits[..., 1])
):
self._data.default_joint_pos[env_ids, joint_ids] = torch.clamp(
self._data.default_joint_pos[env_ids, joint_ids], limits[..., 0], limits[..., 1]
)
violation_message = (
"Some default joint positions are outside of the range of the new joint limits. Default joint positions"
" will be clamped to be within the new joint limits."
) )
if warn_limit_violation:
# warn level will show in console
omni.log.warn(violation_message)
else:
# info level is only written to log file
omni.log.info(violation_message)
# set into simulation
self.root_physx_view.set_dof_limits(self._data.joint_limits.cpu(), indices=physx_env_ids.cpu())
""" """
Operations - Setters. Operations - Setters.
...@@ -852,7 +883,6 @@ class Articulation(AssetBase): ...@@ -852,7 +883,6 @@ class Articulation(AssetBase):
): ):
"""Set joint position targets into internal buffers. """Set joint position targets into internal buffers.
.. note::
This function does not apply the joint targets to the simulation. It only fills the buffers with This function does not apply the joint targets to the simulation. It only fills the buffers with
the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function.
...@@ -877,7 +907,6 @@ class Articulation(AssetBase): ...@@ -877,7 +907,6 @@ class Articulation(AssetBase):
): ):
"""Set joint velocity targets into internal buffers. """Set joint velocity targets into internal buffers.
.. note::
This function does not apply the joint targets to the simulation. It only fills the buffers with This function does not apply the joint targets to the simulation. It only fills the buffers with
the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function.
...@@ -902,7 +931,6 @@ class Articulation(AssetBase): ...@@ -902,7 +931,6 @@ class Articulation(AssetBase):
): ):
"""Set joint efforts into internal buffers. """Set joint efforts into internal buffers.
.. note::
This function does not apply the joint targets to the simulation. It only fills the buffers with This function does not apply the joint targets to the simulation. It only fills the buffers with
the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function.
...@@ -934,7 +962,6 @@ class Articulation(AssetBase): ...@@ -934,7 +962,6 @@ class Articulation(AssetBase):
): ):
"""Set fixed tendon stiffness into internal buffers. """Set fixed tendon stiffness into internal buffers.
.. note::
This function does not apply the tendon stiffness to the simulation. It only fills the buffers with This function does not apply the tendon stiffness to the simulation. It only fills the buffers with
the desired values. To apply the tendon stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function. the desired values. To apply the tendon stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function.
...@@ -961,7 +988,6 @@ class Articulation(AssetBase): ...@@ -961,7 +988,6 @@ class Articulation(AssetBase):
): ):
"""Set fixed tendon damping into internal buffers. """Set fixed tendon damping into internal buffers.
.. note::
This function does not apply the tendon damping to the simulation. It only fills the buffers with This function does not apply the tendon damping to the simulation. It only fills the buffers with
the desired values. To apply the tendon damping, call the :meth:`write_fixed_tendon_properties_to_sim` function. the desired values. To apply the tendon damping, call the :meth:`write_fixed_tendon_properties_to_sim` function.
...@@ -988,7 +1014,6 @@ class Articulation(AssetBase): ...@@ -988,7 +1014,6 @@ class Articulation(AssetBase):
): ):
"""Set fixed tendon limit stiffness efforts into internal buffers. """Set fixed tendon limit stiffness efforts into internal buffers.
.. note::
This function does not apply the tendon limit stiffness to the simulation. It only fills the buffers with This function does not apply the tendon limit stiffness to the simulation. It only fills the buffers with
the desired values. To apply the tendon limit stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function. the desired values. To apply the tendon limit stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function.
...@@ -1007,7 +1032,7 @@ class Articulation(AssetBase): ...@@ -1007,7 +1032,7 @@ class Articulation(AssetBase):
# set limit_stiffness # set limit_stiffness
self._data.fixed_tendon_limit_stiffness[env_ids, fixed_tendon_ids] = limit_stiffness self._data.fixed_tendon_limit_stiffness[env_ids, fixed_tendon_ids] = limit_stiffness
def set_fixed_tendon_limit( def set_fixed_tendon_position_limit(
self, self,
limit: torch.Tensor, limit: torch.Tensor,
fixed_tendon_ids: Sequence[int] | slice | None = None, fixed_tendon_ids: Sequence[int] | slice | None = None,
...@@ -1015,7 +1040,6 @@ class Articulation(AssetBase): ...@@ -1015,7 +1040,6 @@ class Articulation(AssetBase):
): ):
"""Set fixed tendon limit efforts into internal buffers. """Set fixed tendon limit efforts into internal buffers.
.. note::
This function does not apply the tendon limit to the simulation. It only fills the buffers with This function does not apply the tendon limit to the simulation. It only fills the buffers with
the desired values. To apply the tendon limit, call the :meth:`write_fixed_tendon_properties_to_sim` function. the desired values. To apply the tendon limit, call the :meth:`write_fixed_tendon_properties_to_sim` function.
...@@ -1032,7 +1056,7 @@ class Articulation(AssetBase): ...@@ -1032,7 +1056,7 @@ class Articulation(AssetBase):
if env_ids != slice(None) and fixed_tendon_ids != slice(None): if env_ids != slice(None) and fixed_tendon_ids != slice(None):
env_ids = env_ids[:, None] env_ids = env_ids[:, None]
# set limit # set limit
self._data.fixed_tendon_limit[env_ids, fixed_tendon_ids] = limit self._data.fixed_tendon_pos_limits[env_ids, fixed_tendon_ids] = limit
def set_fixed_tendon_rest_length( def set_fixed_tendon_rest_length(
self, self,
...@@ -1042,7 +1066,6 @@ class Articulation(AssetBase): ...@@ -1042,7 +1066,6 @@ class Articulation(AssetBase):
): ):
"""Set fixed tendon rest length efforts into internal buffers. """Set fixed tendon rest length efforts into internal buffers.
.. note::
This function does not apply the tendon rest length to the simulation. It only fills the buffers with This function does not apply the tendon rest length to the simulation. It only fills the buffers with
the desired values. To apply the tendon rest length, call the :meth:`write_fixed_tendon_properties_to_sim` function. the desired values. To apply the tendon rest length, call the :meth:`write_fixed_tendon_properties_to_sim` function.
...@@ -1069,7 +1092,6 @@ class Articulation(AssetBase): ...@@ -1069,7 +1092,6 @@ class Articulation(AssetBase):
): ):
"""Set fixed tendon offset efforts into internal buffers. """Set fixed tendon offset efforts into internal buffers.
.. note::
This function does not apply the tendon offset to the simulation. It only fills the buffers with This function does not apply the tendon offset to the simulation. It only fills the buffers with
the desired values. To apply the tendon offset, call the :meth:`write_fixed_tendon_properties_to_sim` function. the desired values. To apply the tendon offset, call the :meth:`write_fixed_tendon_properties_to_sim` function.
...@@ -1111,7 +1133,7 @@ class Articulation(AssetBase): ...@@ -1111,7 +1133,7 @@ class Articulation(AssetBase):
self._data.fixed_tendon_stiffness, self._data.fixed_tendon_stiffness,
self._data.fixed_tendon_damping, self._data.fixed_tendon_damping,
self._data.fixed_tendon_limit_stiffness, self._data.fixed_tendon_limit_stiffness,
self._data.fixed_tendon_limit, self._data.fixed_tendon_pos_limits,
self._data.fixed_tendon_rest_length, self._data.fixed_tendon_rest_length,
self._data.fixed_tendon_offset, self._data.fixed_tendon_offset,
indices=physx_env_ids, indices=physx_env_ids,
...@@ -1180,7 +1202,7 @@ class Articulation(AssetBase): ...@@ -1180,7 +1202,7 @@ class Articulation(AssetBase):
# update the robot data # update the robot data
self.update(0.0) self.update(0.0)
# log joint information # log joint information
self._log_articulation_joint_info() self._log_articulation_info()
def _create_buffers(self): def _create_buffers(self):
# constants # constants
...@@ -1191,99 +1213,58 @@ class Articulation(AssetBase): ...@@ -1191,99 +1213,58 @@ class Articulation(AssetBase):
self._external_force_b = torch.zeros((self.num_instances, self.num_bodies, 3), device=self.device) self._external_force_b = torch.zeros((self.num_instances, self.num_bodies, 3), device=self.device)
self._external_torque_b = torch.zeros_like(self._external_force_b) self._external_torque_b = torch.zeros_like(self._external_force_b)
# asset data # asset named data
# -- properties
self._data.joint_names = self.joint_names self._data.joint_names = self.joint_names
self._data.body_names = self.body_names self._data.body_names = self.body_names
# tendon names are set in _process_fixed_tendons function
# -- bodies # -- joint properties
self._data.default_joint_pos_limits = self.root_physx_view.get_dof_limits().to(self.device).clone()
self._data.default_joint_stiffness = self.root_physx_view.get_dof_stiffnesses().to(self.device).clone()
self._data.default_joint_damping = self.root_physx_view.get_dof_dampings().to(self.device).clone()
self._data.default_joint_armature = self.root_physx_view.get_dof_armatures().to(self.device).clone()
self._data.default_joint_friction_coeff = (
self.root_physx_view.get_dof_friction_coefficients().to(self.device).clone()
)
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()
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()
self._data.joint_friction_coeff = self._data.default_joint_friction_coeff.clone()
# -- body properties
self._data.default_mass = self.root_physx_view.get_masses().clone() self._data.default_mass = self.root_physx_view.get_masses().clone()
self._data.default_inertia = self.root_physx_view.get_inertias().clone() self._data.default_inertia = self.root_physx_view.get_inertias().clone()
# -- default joint state # -- joint commands (sent to the actuator from the user)
self._data.default_joint_pos = torch.zeros(self.num_instances, self.num_joints, device=self.device) self._data.joint_pos_target = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.default_joint_vel = torch.zeros_like(self._data.default_joint_pos) self._data.joint_vel_target = torch.zeros_like(self._data.joint_pos_target)
self._data.joint_effort_target = torch.zeros_like(self._data.joint_pos_target)
# -- joint commands (sent to the simulation after actuator processing)
self._joint_pos_target_sim = torch.zeros_like(self._data.joint_pos_target)
self._joint_vel_target_sim = torch.zeros_like(self._data.joint_pos_target)
self._joint_effort_target_sim = torch.zeros_like(self._data.joint_pos_target)
# -- joint commands # -- computed joint efforts from the actuator models
self._data.joint_pos_target = torch.zeros_like(self._data.default_joint_pos) self._data.computed_torque = torch.zeros_like(self._data.joint_pos_target)
self._data.joint_vel_target = torch.zeros_like(self._data.default_joint_pos) self._data.applied_torque = torch.zeros_like(self._data.joint_pos_target)
self._data.joint_effort_target = torch.zeros_like(self._data.default_joint_pos)
self._data.joint_stiffness = torch.zeros_like(self._data.default_joint_pos)
self._data.joint_damping = torch.zeros_like(self._data.default_joint_pos)
self._data.joint_armature = torch.zeros_like(self._data.default_joint_pos)
self._data.joint_friction = torch.zeros_like(self._data.default_joint_pos)
self._data.joint_limits = torch.zeros(self.num_instances, self.num_joints, 2, device=self.device)
# -- joint commands (explicit)
self._data.computed_torque = torch.zeros_like(self._data.default_joint_pos)
self._data.applied_torque = torch.zeros_like(self._data.default_joint_pos)
# -- tendons
if self.num_fixed_tendons > 0:
self._data.fixed_tendon_stiffness = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.fixed_tendon_damping = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.fixed_tendon_limit_stiffness = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.fixed_tendon_limit = torch.zeros(
self.num_instances, self.num_fixed_tendons, 2, device=self.device
)
self._data.fixed_tendon_rest_length = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.fixed_tendon_offset = torch.zeros(self.num_instances, self.num_fixed_tendons, device=self.device)
# -- other data # -- other data that are filled based on explicit actuator models
self._data.soft_joint_pos_limits = torch.zeros(self.num_instances, self.num_joints, 2, device=self.device)
self._data.soft_joint_vel_limits = torch.zeros(self.num_instances, self.num_joints, device=self.device) self._data.soft_joint_vel_limits = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.gear_ratio = torch.ones(self.num_instances, self.num_joints, device=self.device) self._data.gear_ratio = torch.ones(self.num_instances, self.num_joints, device=self.device)
# -- initialize default buffers related to joint properties
self._data.default_joint_stiffness = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.default_joint_damping = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.default_joint_armature = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.default_joint_friction = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.default_joint_limits = torch.zeros(self.num_instances, self.num_joints, 2, device=self.device)
# -- initialize default buffers related to fixed tendon properties
if self.num_fixed_tendons > 0:
self._data.default_fixed_tendon_stiffness = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.default_fixed_tendon_damping = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.default_fixed_tendon_limit_stiffness = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.default_fixed_tendon_limit = torch.zeros(
self.num_instances, self.num_fixed_tendons, 2, device=self.device
)
self._data.default_fixed_tendon_rest_length = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
self._data.default_fixed_tendon_offset = torch.zeros(
self.num_instances, self.num_fixed_tendons, device=self.device
)
# soft joint position limits (recommended not to be too close to limits). # soft joint position limits (recommended not to be too close to limits).
joint_pos_limits = self.root_physx_view.get_dof_limits() joint_pos_mean = (self._data.joint_pos_limits[..., 0] + self._data.joint_pos_limits[..., 1]) / 2
joint_pos_mean = (joint_pos_limits[..., 0] + joint_pos_limits[..., 1]) / 2 joint_pos_range = self._data.joint_pos_limits[..., 1] - self._data.joint_pos_limits[..., 0]
joint_pos_range = joint_pos_limits[..., 1] - joint_pos_limits[..., 0]
soft_limit_factor = self.cfg.soft_joint_pos_limit_factor soft_limit_factor = self.cfg.soft_joint_pos_limit_factor
# add to data # add to data
self._data.soft_joint_pos_limits = torch.zeros(self.num_instances, self.num_joints, 2, device=self.device)
self._data.soft_joint_pos_limits[..., 0] = joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor self._data.soft_joint_pos_limits[..., 0] = joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor
self._data.soft_joint_pos_limits[..., 1] = joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor self._data.soft_joint_pos_limits[..., 1] = joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor
# create buffers to store processed actions from actuator models
self._joint_pos_target_sim = torch.zeros_like(self._data.joint_pos_target)
self._joint_vel_target_sim = torch.zeros_like(self._data.joint_pos_target)
self._joint_effort_target_sim = torch.zeros_like(self._data.joint_pos_target)
def _process_cfg(self): def _process_cfg(self):
"""Post processing of configuration parameters.""" """Post processing of configuration parameters."""
# default state # default state
...@@ -1297,7 +1278,10 @@ class Articulation(AssetBase): ...@@ -1297,7 +1278,10 @@ class Articulation(AssetBase):
) )
default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device) default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device)
self._data.default_root_state = default_root_state.repeat(self.num_instances, 1) self._data.default_root_state = default_root_state.repeat(self.num_instances, 1)
# -- joint state # -- joint state
self._data.default_joint_pos = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.default_joint_vel = torch.zeros_like(self._data.default_joint_pos)
# joint pos # joint pos
indices_list, _, values_list = string_utils.resolve_matching_names_values( indices_list, _, values_list = string_utils.resolve_matching_names_values(
self.cfg.init_state.joint_pos, self.joint_names self.cfg.init_state.joint_pos, self.joint_names
...@@ -1309,10 +1293,6 @@ class Articulation(AssetBase): ...@@ -1309,10 +1293,6 @@ class Articulation(AssetBase):
) )
self._data.default_joint_vel[:, indices_list] = torch.tensor(values_list, device=self.device) self._data.default_joint_vel[:, indices_list] = torch.tensor(values_list, device=self.device)
# -- joint limits
self._data.default_joint_limits = self.root_physx_view.get_dof_limits().to(device=self.device).clone()
self._data.joint_limits = self._data.default_joint_limits.clone()
""" """
Internal simulation callbacks. Internal simulation callbacks.
""" """
...@@ -1337,12 +1317,6 @@ class Articulation(AssetBase): ...@@ -1337,12 +1317,6 @@ class Articulation(AssetBase):
# if this is false, we by-pass certain checks when doing actuator-related operations # if this is false, we by-pass certain checks when doing actuator-related operations
self._has_implicit_actuators = False self._has_implicit_actuators = False
# cache the values coming from the usd
self._data.default_joint_stiffness = self.root_physx_view.get_dof_stiffnesses().to(self.device).clone()
self._data.default_joint_damping = self.root_physx_view.get_dof_dampings().to(self.device).clone()
self._data.default_joint_armature = self.root_physx_view.get_dof_armatures().to(self.device).clone()
self._data.default_joint_friction = self.root_physx_view.get_dof_friction_coefficients().to(self.device).clone()
# iterate over all actuator configurations # iterate over all actuator configurations
for actuator_name, actuator_cfg in self.cfg.actuators.items(): for actuator_name, actuator_cfg in self.cfg.actuators.items():
# type annotation for type checkers # type annotation for type checkers
...@@ -1372,9 +1346,9 @@ class Articulation(AssetBase): ...@@ -1372,9 +1346,9 @@ class Articulation(AssetBase):
stiffness=self._data.default_joint_stiffness[:, joint_ids], stiffness=self._data.default_joint_stiffness[:, joint_ids],
damping=self._data.default_joint_damping[:, joint_ids], damping=self._data.default_joint_damping[:, joint_ids],
armature=self._data.default_joint_armature[:, joint_ids], armature=self._data.default_joint_armature[:, joint_ids],
friction=self._data.default_joint_friction[:, joint_ids], friction=self._data.default_joint_friction_coeff[:, joint_ids],
effort_limit=self.root_physx_view.get_dof_max_forces().to(self.device).clone()[:, joint_ids], effort_limit=self._data.joint_effort_limits[:, joint_ids],
velocity_limit=self.root_physx_view.get_dof_max_velocities().to(self.device).clone()[:, joint_ids], velocity_limit=self._data.joint_vel_limits[:, joint_ids],
) )
# log information on actuator groups # log information on actuator groups
model_type = "implicit" if actuator.is_implicit_model else "explicit" model_type = "implicit" if actuator.is_implicit_model else "explicit"
...@@ -1400,12 +1374,14 @@ class Articulation(AssetBase): ...@@ -1400,12 +1374,14 @@ class Articulation(AssetBase):
self.write_joint_effort_limit_to_sim(actuator.effort_limit_sim, 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_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_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices) self.write_joint_friction_coefficient_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
# Store the actual default stiffness and damping values # Store the configured values from the actuator model
# note: this is the value configured in the actuator model (for implicit and explicit actuators) # 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_stiffness[:, actuator.joint_indices] = actuator.stiffness
self._data.default_joint_damping[:, actuator.joint_indices] = actuator.damping self._data.default_joint_damping[:, actuator.joint_indices] = actuator.damping
self._data.default_joint_armature[:, actuator.joint_indices] = actuator.armature
self._data.default_joint_friction_coeff[:, actuator.joint_indices] = actuator.friction
# perform some sanity checks to ensure actuators are prepared correctly # perform some sanity checks to ensure actuators are prepared correctly
total_act_joints = sum(actuator.num_joints for actuator in self.actuators.values()) total_act_joints = sum(actuator.num_joints for actuator in self.actuators.values())
...@@ -1434,16 +1410,27 @@ class Articulation(AssetBase): ...@@ -1434,16 +1410,27 @@ class Articulation(AssetBase):
joint_name = usd_joint_path.split("/")[-1] joint_name = usd_joint_path.split("/")[-1]
self._fixed_tendon_names.append(joint_name) self._fixed_tendon_names.append(joint_name)
# store the fixed tendon names
self._data.fixed_tendon_names = self._fixed_tendon_names self._data.fixed_tendon_names = self._fixed_tendon_names
# store the current USD fixed tendon properties
self._data.default_fixed_tendon_stiffness = self.root_physx_view.get_fixed_tendon_stiffnesses().clone() 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_damping = self.root_physx_view.get_fixed_tendon_dampings().clone()
self._data.default_fixed_tendon_limit_stiffness = ( self._data.default_fixed_tendon_limit_stiffness = (
self.root_physx_view.get_fixed_tendon_limit_stiffnesses().clone() self.root_physx_view.get_fixed_tendon_limit_stiffnesses().clone()
) )
self._data.default_fixed_tendon_limit = self.root_physx_view.get_fixed_tendon_limits().clone() self._data.default_fixed_tendon_pos_limits = self.root_physx_view.get_fixed_tendon_limits().clone()
self._data.default_fixed_tendon_rest_length = self.root_physx_view.get_fixed_tendon_rest_lengths().clone() self._data.default_fixed_tendon_rest_length = self.root_physx_view.get_fixed_tendon_rest_lengths().clone()
self._data.default_fixed_tendon_offset = self.root_physx_view.get_fixed_tendon_offsets().clone() self._data.default_fixed_tendon_offset = self.root_physx_view.get_fixed_tendon_offsets().clone()
# store a copy of the default values for the fixed tendons
self._data.fixed_tendon_stiffness = self._data.default_fixed_tendon_stiffness.clone()
self._data.fixed_tendon_damping = self._data.default_fixed_tendon_damping.clone()
self._data.fixed_tendon_limit_stiffness = self._data.default_fixed_tendon_limit_stiffness.clone()
self._data.fixed_tendon_pos_limits = self._data.default_fixed_tendon_pos_limits.clone()
self._data.fixed_tendon_rest_length = self._data.default_fixed_tendon_rest_length.clone()
self._data.fixed_tendon_offset = self._data.default_fixed_tendon_offset.clone()
def _apply_actuator_model(self): def _apply_actuator_model(self):
"""Processes joint commands for the articulation by forwarding them to the actuators. """Processes joint commands for the articulation by forwarding them to the actuators.
...@@ -1506,10 +1493,10 @@ class Articulation(AssetBase): ...@@ -1506,10 +1493,10 @@ class Articulation(AssetBase):
msg = "The following joints have default positions out of the limits: \n" msg = "The following joints have default positions out of the limits: \n"
for idx in violated_indices: for idx in violated_indices:
joint_name = self.data.joint_names[idx] joint_name = self.data.joint_names[idx]
joint_limits = joint_pos_limits[idx] joint_limit = joint_pos_limits[idx]
joint_pos = self.data.default_joint_pos[0, idx] joint_pos = self.data.default_joint_pos[0, idx]
# add to message # add to message
msg += f"\t- '{joint_name}': {joint_pos:.3f} not in [{joint_limits[0]:.3f}, {joint_limits[1]:.3f}]\n" msg += f"\t- '{joint_name}': {joint_pos:.3f} not in [{joint_limit[0]:.3f}, {joint_limit[1]:.3f}]\n"
raise ValueError(msg) raise ValueError(msg)
# check that the default joint velocities are within the limits # check that the default joint velocities are within the limits
...@@ -1521,14 +1508,17 @@ class Articulation(AssetBase): ...@@ -1521,14 +1508,17 @@ class Articulation(AssetBase):
msg = "The following joints have default velocities out of the limits: \n" msg = "The following joints have default velocities out of the limits: \n"
for idx in violated_indices: for idx in violated_indices:
joint_name = self.data.joint_names[idx] joint_name = self.data.joint_names[idx]
joint_limits = [-joint_max_vel[idx], joint_max_vel[idx]] joint_limit = [-joint_max_vel[idx], joint_max_vel[idx]]
joint_vel = self.data.default_joint_vel[0, idx] joint_vel = self.data.default_joint_vel[0, idx]
# add to message # add to message
msg += f"\t- '{joint_name}': {joint_vel:.3f} not in [{joint_limits[0]:.3f}, {joint_limits[1]:.3f}]\n" msg += f"\t- '{joint_name}': {joint_vel:.3f} not in [{joint_limit[0]:.3f}, {joint_limit[1]:.3f}]\n"
raise ValueError(msg) raise ValueError(msg)
def _log_articulation_joint_info(self): def _log_articulation_info(self):
"""Log information about the articulation's simulated joints.""" """Log information about the articulation.
Note: We purposefully read the values from the simulator to ensure that the values are configured as expected.
"""
# read out all joint parameters from simulation # read out all joint parameters from simulation
# -- gains # -- gains
stiffnesses = self.root_physx_view.get_dof_stiffnesses()[0].tolist() stiffnesses = self.root_physx_view.get_dof_stiffnesses()[0].tolist()
...@@ -1611,3 +1601,61 @@ class Articulation(AssetBase): ...@@ -1611,3 +1601,61 @@ class Articulation(AssetBase):
]) ])
# convert table to string # convert table to string
omni.log.info(f"Simulation parameters for tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string()) omni.log.info(f"Simulation parameters for tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string())
"""
Deprecated methods.
"""
def write_joint_friction_to_sim(
self,
joint_friction: torch.Tensor | float,
joint_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
):
"""Write joint friction coefficients into the simulation.
.. deprecated:: 2.1.0
Please use :meth:`write_joint_friction_coefficient_to_sim` instead.
"""
omni.log.warn(
"The function 'write_joint_friction_to_sim' will be deprecated in a future release. Please"
" use 'write_joint_friction_coefficient_to_sim' instead."
)
self.write_joint_friction_coefficient_to_sim(joint_friction, joint_ids=joint_ids, env_ids=env_ids)
def write_joint_limits_to_sim(
self,
limits: torch.Tensor | float,
joint_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
warn_limit_violation: bool = True,
):
"""Write joint limits into the simulation.
.. deprecated:: 2.1.0
Please use :meth:`write_joint_position_limit_to_sim` instead.
"""
omni.log.warn(
"The function 'write_joint_limits_to_sim' will be deprecated in a future release. Please"
" use 'write_joint_position_limit_to_sim' instead."
)
self.write_joint_position_limit_to_sim(
limits, joint_ids=joint_ids, env_ids=env_ids, warn_limit_violation=warn_limit_violation
)
def set_fixed_tendon_limit(
self,
limit: torch.Tensor,
fixed_tendon_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
):
"""Set fixed tendon position limits into internal buffers.
.. deprecated:: 2.1.0
Please use :meth:`set_fixed_tendon_position_limit` instead.
"""
omni.log.warn(
"The function 'set_fixed_tendon_limit' will be deprecated in a future release. Please"
" use 'set_fixed_tendon_position_limit' instead."
)
self.set_fixed_tendon_position_limit(limit, fixed_tendon_ids=fixed_tendon_ids, env_ids=env_ids)
...@@ -42,10 +42,13 @@ class ArticulationCfg(AssetBaseCfg): ...@@ -42,10 +42,13 @@ class ArticulationCfg(AssetBaseCfg):
"""Initial state of the articulated object. Defaults to identity pose with zero velocity and zero joint state.""" """Initial state of the articulated object. Defaults to identity pose with zero velocity and zero joint state."""
soft_joint_pos_limit_factor: float = 1.0 soft_joint_pos_limit_factor: float = 1.0
"""Fraction specifying the range of DOF position limits (parsed from the asset) to use. Defaults to 1.0. """Fraction specifying the range of joint position limits (parsed from the asset) to use. Defaults to 1.0.
The joint position limits are scaled by this factor to allow for a limited range of motion. The soft joint position limits are scaled by this factor to specify a safety region within the simulated
This is accessible in the articulation data through :attr:`ArticulationData.soft_joint_pos_limits` attribute. joint position limits. This isn't used by the simulation, but is useful for learning agents to prevent the joint
positions from violating the limits, such as for termination conditions.
The soft joint position limits are accessible through the :attr:`ArticulationData.soft_joint_pos_limits` attribute.
""" """
actuators: dict[str, ActuatorBaseCfg] = MISSING actuators: dict[str, ActuatorBaseCfg] = MISSING
......
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
import torch import torch
import weakref import weakref
import omni.log
import omni.physics.tensors.impl.api as physx import omni.physics.tensors.impl.api as physx
import isaaclab.utils.math as math_utils import isaaclab.utils.math as math_utils
...@@ -95,64 +96,131 @@ class ArticulationData: ...@@ -95,64 +96,131 @@ class ArticulationData:
"""Fixed tendon names in the order parsed by the simulation view.""" """Fixed tendon names in the order parsed by the simulation view."""
## ##
# Defaults. # Defaults - Initial state.
## ##
default_root_state: torch.Tensor = None default_root_state: torch.Tensor = None
"""Default root state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. Shape is (num_instances, 13). """Default root state ``[pos, quat, lin_vel, ang_vel]`` in the local environment frame. Shape is (num_instances, 13).
The position and quaternion are of the articulation root's actor frame. Meanwhile, the linear and angular The position and quaternion are of the articulation root's actor frame. Meanwhile, the linear and angular
velocities are of its center of mass frame. velocities are of its center of mass frame.
This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter.
"""
default_joint_pos: torch.Tensor = None
"""Default joint positions of all joints. Shape is (num_instances, num_joints).
This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter.
""" """
default_joint_vel: torch.Tensor = None
"""Default joint velocities of all joints. Shape is (num_instances, num_joints).
This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter.
"""
##
# Defaults - Physical properties.
##
default_mass: torch.Tensor = None default_mass: torch.Tensor = None
"""Default mass read from the simulation. Shape is (num_instances, num_bodies).""" """Default mass for all the bodies in the articulation. Shape is (num_instances, num_bodies).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_inertia: torch.Tensor = None default_inertia: torch.Tensor = None
"""Default inertia read from the simulation. Shape is (num_instances, num_bodies, 9). """Default inertia for all the bodies in the articulation. Shape is (num_instances, num_bodies, 9).
The inertia is the inertia tensor relative to the center of mass frame. The values are stored in The inertia is the inertia tensor relative to the center of mass frame. The values are stored in
the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`.
This quantity is parsed from the USD schema at the time of initialization.
""" """
default_joint_pos: torch.Tensor = None default_joint_stiffness: torch.Tensor = None
"""Default joint positions of all joints. Shape is (num_instances, num_joints).""" """Default joint stiffness of all joints. Shape is (num_instances, num_joints).
default_joint_vel: torch.Tensor = None This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.stiffness`
"""Default joint velocities of all joints. Shape is (num_instances, num_joints).""" parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization,
is used.
default_joint_stiffness: torch.Tensor = None .. attention::
"""Default joint stiffness of all joints. Shape is (num_instances, num_joints).""" The default stiffness is the value configured by the user or the value parsed from the USD schema.
It should not be confused with :attr:`joint_stiffness`, which is the value set into the simulation.
"""
default_joint_damping: torch.Tensor = None default_joint_damping: torch.Tensor = None
"""Default joint damping of all joints. Shape is (num_instances, num_joints).""" """Default joint damping of all joints. Shape is (num_instances, num_joints).
This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.damping`
parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization,
is used.
.. attention::
The default stiffness is the value configured by the user or the value parsed from the USD schema.
It should not be confused with :attr:`joint_damping`, which is the value set into the simulation.
"""
default_joint_armature: torch.Tensor = None default_joint_armature: torch.Tensor = None
"""Default joint armature of all joints. Shape is (num_instances, num_joints).""" """Default joint armature of all joints. Shape is (num_instances, num_joints).
This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.armature`
parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization,
is used.
"""
default_joint_friction: torch.Tensor = None default_joint_friction_coeff: torch.Tensor = None
"""Default joint friction of all joints. Shape is (num_instances, num_joints).""" """Default joint friction coefficient of all joints. Shape is (num_instances, num_joints).
default_joint_limits: torch.Tensor = None This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.friction`
"""Default joint limits of all joints. Shape is (num_instances, num_joints, 2).""" parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization,
is used.
"""
default_joint_pos_limits: torch.Tensor = None
"""Default joint position limits of all joints. Shape is (num_instances, num_joints, 2).
The limits are in the order :math:`[lower, upper]`. They are parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_stiffness: torch.Tensor = None default_fixed_tendon_stiffness: torch.Tensor = None
"""Default tendon stiffness of all tendons. Shape is (num_instances, num_fixed_tendons).""" """Default tendon stiffness of all tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_damping: torch.Tensor = None default_fixed_tendon_damping: torch.Tensor = None
"""Default tendon damping of all tendons. Shape is (num_instances, num_fixed_tendons).""" """Default tendon damping of all tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_limit_stiffness: torch.Tensor = None default_fixed_tendon_limit_stiffness: torch.Tensor = None
"""Default tendon limit stiffness of all tendons. Shape is (num_instances, num_fixed_tendons).""" """Default tendon limit stiffness of all tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_rest_length: torch.Tensor = None default_fixed_tendon_rest_length: torch.Tensor = None
"""Default tendon rest length of all tendons. Shape is (num_instances, num_fixed_tendons).""" """Default tendon rest length of all tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_offset: torch.Tensor = None default_fixed_tendon_offset: torch.Tensor = None
"""Default tendon offset of all tendons. Shape is (num_instances, num_fixed_tendons).""" """Default tendon offset of all tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_limit: torch.Tensor = None default_fixed_tendon_pos_limits: torch.Tensor = None
"""Default tendon limits of all tendons. Shape is (num_instances, num_fixed_tendons, 2).""" """Default tendon position limits of all tendons. Shape is (num_instances, num_fixed_tendons, 2).
The position limits are in the order :math:`[lower, upper]`. They are parsed from the USD schema at the time of
initialization.
"""
## ##
# Joint commands -- Set into simulation. # Joint commands -- Set into simulation.
...@@ -192,8 +260,6 @@ class ArticulationData: ...@@ -192,8 +260,6 @@ class ArticulationData:
This quantity is the raw torque output from the actuator mode, before any clipping is applied. This quantity is the raw torque output from the actuator mode, before any clipping is applied.
It is exposed for users who want to inspect the computations inside the actuator model. It is exposed for users who want to inspect the computations inside the actuator model.
For instance, to penalize the learning agent for a difference between the computed and applied torques. For instance, to penalize the learning agent for a difference between the computed and applied torques.
Note: The torques are zero for implicit actuator models.
""" """
applied_torque: torch.Tensor = None applied_torque: torch.Tensor = None
...@@ -201,8 +267,6 @@ class ArticulationData: ...@@ -201,8 +267,6 @@ class ArticulationData:
These torques are set into the simulation, after clipping the :attr:`computed_torque` based on the These torques are set into the simulation, after clipping the :attr:`computed_torque` based on the
actuator model. actuator model.
Note: The torques are zero for implicit actuator models.
""" """
## ##
...@@ -210,52 +274,90 @@ class ArticulationData: ...@@ -210,52 +274,90 @@ class ArticulationData:
## ##
joint_stiffness: torch.Tensor = None joint_stiffness: torch.Tensor = None
"""Joint stiffness provided to simulation. Shape is (num_instances, num_joints).""" """Joint stiffness provided to the simulation. Shape is (num_instances, num_joints).
joint_damping: torch.Tensor = None In the case of explicit actuators, the value for the corresponding joints is zero.
"""Joint damping provided to simulation. Shape is (num_instances, num_joints).""" """
joint_limits: torch.Tensor = None
"""Joint limits provided to simulation. Shape is (num_instances, num_joints, 2)."""
joint_velocity_limits: torch.Tensor = None joint_damping: torch.Tensor = None
"""Joint maximum velocity provided to simulation. Shape is (num_instances, num_joints).""" """Joint damping provided to the simulation. Shape is (num_instances, num_joints)
## In the case of explicit actuators, the value for the corresponding joints is zero.
# Fixed tendon properties. """
##
fixed_tendon_stiffness: torch.Tensor = None joint_armature: torch.Tensor = None
"""Fixed tendon stiffness provided to simulation. Shape is (num_instances, num_fixed_tendons).""" """Joint armature provided to the simulation. Shape is (num_instances, num_joints)."""
fixed_tendon_damping: torch.Tensor = None joint_friction_coeff: torch.Tensor = None
"""Fixed tendon damping provided to simulation. Shape is (num_instances, num_fixed_tendons).""" """Joint friction coefficient provided to the simulation. Shape is (num_instances, num_joints)."""
fixed_tendon_limit_stiffness: torch.Tensor = None joint_pos_limits: torch.Tensor = None
"""Fixed tendon limit stiffness provided to simulation. Shape is (num_instances, num_fixed_tendons).""" """Joint position limits provided to the simulation. Shape is (num_instances, num_joints, 2).
fixed_tendon_rest_length: torch.Tensor = None The limits are in the order :math:`[lower, upper]`.
"""Fixed tendon rest length provided to simulation. Shape is (num_instances, num_fixed_tendons).""" """
fixed_tendon_offset: torch.Tensor = None joint_vel_limits: torch.Tensor = None
"""Fixed tendon offset provided to simulation. Shape is (num_instances, num_fixed_tendons).""" """Joint maximum velocity provided to the simulation. Shape is (num_instances, num_joints)."""
fixed_tendon_limit: torch.Tensor = None joint_effort_limits: torch.Tensor = None
"""Fixed tendon limits provided to simulation. Shape is (num_instances, num_fixed_tendons, 2).""" """Joint maximum effort provided to the simulation. Shape is (num_instances, num_joints)."""
## ##
# Other Data. # Joint properties - Custom.
## ##
soft_joint_pos_limits: torch.Tensor = None soft_joint_pos_limits: torch.Tensor = None
"""Joint positions limits for all joints. Shape is (num_instances, num_joints, 2).""" r"""Soft joint positions limits for all joints. Shape is (num_instances, num_joints, 2).
The limits are in the order :math:`[lower, upper]`.The soft joint position limits are computed as
a sub-region of the :attr:`joint_pos_limits` based on the
:attr:`~isaaclab.assets.ArticulationCfg.soft_joint_pos_limit_factor` parameter.
Consider the joint position limits :math:`[lower, upper]` and the soft joint position limits
:math:`[soft_lower, soft_upper]`. The soft joint position limits are computed as:
.. math::
soft\_lower = (lower + upper) / 2 - factor * (upper - lower) / 2
soft\_upper = (lower + upper) / 2 + factor * (upper - lower) / 2
The soft joint position limits help specify a safety region around the joint limits. It isn't used by the
simulation, but is useful for learning agents to prevent the joint positions from violating the limits.
"""
soft_joint_vel_limits: torch.Tensor = None soft_joint_vel_limits: torch.Tensor = None
"""Joint velocity limits for all joints. Shape is (num_instances, num_joints).""" """Soft joint velocity limits for all joints. Shape is (num_instances, num_joints).
These are obtained from the actuator model. It may differ from :attr:`joint_vel_limits` if the actuator model
has a variable velocity limit model. For instance, in a variable gear ratio actuator model.
"""
gear_ratio: torch.Tensor = None gear_ratio: torch.Tensor = None
"""Gear ratio for relating motor torques to applied Joint torques. Shape is (num_instances, num_joints).""" """Gear ratio for relating motor torques to applied Joint torques. Shape is (num_instances, num_joints)."""
##
# Fixed tendon properties.
##
fixed_tendon_stiffness: torch.Tensor = None
"""Fixed tendon stiffness provided to the simulation. Shape is (num_instances, num_fixed_tendons)."""
fixed_tendon_damping: torch.Tensor = None
"""Fixed tendon damping provided to the simulation. Shape is (num_instances, num_fixed_tendons)."""
fixed_tendon_limit_stiffness: torch.Tensor = None
"""Fixed tendon limit stiffness provided to the simulation. Shape is (num_instances, num_fixed_tendons)."""
fixed_tendon_rest_length: torch.Tensor = None
"""Fixed tendon rest length provided to the simulation. Shape is (num_instances, num_fixed_tendons)."""
fixed_tendon_offset: torch.Tensor = None
"""Fixed tendon offset provided to the simulation. Shape is (num_instances, num_fixed_tendons)."""
fixed_tendon_pos_limits: torch.Tensor = None
"""Fixed tendon position limits provided to the simulation. Shape is (num_instances, num_fixed_tendons, 2)."""
## ##
# Properties. # Properties.
## ##
...@@ -267,7 +369,6 @@ class ArticulationData: ...@@ -267,7 +369,6 @@ class ArticulationData:
The position and quaternion are of the articulation root's actor frame relative to the world. Meanwhile, The position and quaternion are of the articulation root's actor frame relative to the world. Meanwhile,
the linear and angular velocities are of the articulation root's center of mass frame. the linear and angular velocities are of the articulation root's center of mass frame.
""" """
if self._root_state_w.timestamp < self._sim_timestamp: if self._root_state_w.timestamp < self._sim_timestamp:
# read data from simulation # read data from simulation
pose = self._root_physx_view.get_root_transforms().clone() pose = self._root_physx_view.get_root_transforms().clone()
...@@ -517,9 +618,9 @@ class ArticulationData: ...@@ -517,9 +618,9 @@ class ArticulationData:
""" """
return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w) return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w)
# ##
# Derived Root Link Frame Properties # Derived Root Link Frame Properties
# ##
@property @property
def root_link_pos_w(self) -> torch.Tensor: def root_link_pos_w(self) -> torch.Tensor:
...@@ -589,9 +690,9 @@ class ArticulationData: ...@@ -589,9 +690,9 @@ class ArticulationData:
""" """
return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w)
# ##
# Root Center of Mass state properties # Root Center of Mass state properties
# ##
@property @property
def root_com_pos_w(self) -> torch.Tensor: def root_com_pos_w(self) -> torch.Tensor:
...@@ -721,9 +822,9 @@ class ArticulationData: ...@@ -721,9 +822,9 @@ class ArticulationData:
""" """
return self.body_acc_w[..., 3:6] return self.body_acc_w[..., 3:6]
# ##
# Link body properties # Link body properties
# ##
@property @property
def body_link_pos_w(self) -> torch.Tensor: def body_link_pos_w(self) -> torch.Tensor:
...@@ -777,9 +878,9 @@ class ArticulationData: ...@@ -777,9 +878,9 @@ class ArticulationData:
""" """
return self.body_link_state_w[..., 10:13] return self.body_link_state_w[..., 10:13]
# ##
# Center of mass body properties # Center of mass body properties
# ##
@property @property
def body_com_pos_w(self) -> torch.Tensor: def body_com_pos_w(self) -> torch.Tensor:
...@@ -846,9 +947,75 @@ class ArticulationData: ...@@ -846,9 +947,75 @@ class ArticulationData:
@property @property
def com_quat_b(self) -> torch.Tensor: def com_quat_b(self) -> torch.Tensor:
"""Orientation (w,x,y,z) of the prinicple axies of inertia of all of the bodies in simulation world frame. Shape is (num_instances, num_bodies, 4). """Orientation (w,x,y,z) of the principle axies of inertia of all of the bodies in simulation world frame. Shape is (num_instances, num_bodies, 4).
This quantity is the orientation of the principles axes of inertia relative to its body frame. This quantity is the orientation of the principles axes of inertia relative to its body frame.
""" """
quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7] quat = self._root_physx_view.get_coms().to(self.device)[..., 3:7]
return math_utils.convert_quat(quat, to="wxyz") return math_utils.convert_quat(quat, to="wxyz")
##
# Backward compatibility.
##
@property
def joint_limits(self) -> torch.Tensor:
"""Deprecated property. Please use :attr:`joint_pos_limits` instead."""
omni.log.warn(
"The `joint_limits` property will be deprecated in a future release. Please use `joint_pos_limits` instead."
)
return self.joint_pos_limits
@property
def default_joint_limits(self) -> torch.Tensor:
"""Deprecated property. Please use :attr:`default_joint_pos_limits` instead."""
omni.log.warn(
"The `default_joint_limits` property will be deprecated in a future release. Please use"
" `default_joint_pos_limits` instead."
)
return self.default_joint_pos_limits
@property
def joint_velocity_limits(self) -> torch.Tensor:
"""Deprecated property. Please use :attr:`joint_vel_limits` instead."""
omni.log.warn(
"The `joint_velocity_limits` property will be deprecated in a future release. Please use"
" `joint_vel_limits` instead."
)
return self.joint_vel_limits
@property
def joint_friction(self) -> torch.Tensor:
"""Deprecated property. Please use :attr:`joint_friction_coeff` instead."""
omni.log.warn(
"The `joint_friction` property will be deprecated in a future release. Please use"
" `joint_friction_coeff` instead."
)
return self.joint_friction_coeff
@property
def default_joint_friction(self) -> torch.Tensor:
"""Deprecated property. Please use :attr:`default_joint_friction_coeff` instead."""
omni.log.warn(
"The `default_joint_friction` property will be deprecated in a future release. Please use"
" `default_joint_friction_coeff` instead."
)
return self.default_joint_friction_coeff
@property
def fixed_tendon_limit(self) -> torch.Tensor:
"""Deprecated property. Please use :attr:`fixed_tendon_pos_limits` instead."""
omni.log.warn(
"The `fixed_tendon_limit` property will be deprecated in a future release. Please use"
" `fixed_tendon_pos_limits` instead."
)
return self.fixed_tendon_pos_limits
@property
def default_fixed_tendon_limit(self) -> torch.Tensor:
"""Deprecated property. Please use :attr:`default_fixed_tendon_pos_limits` instead."""
omni.log.warn(
"The `default_fixed_tendon_limit` property will be deprecated in a future release. Please use"
" `default_fixed_tendon_pos_limits` instead."
)
return self.default_fixed_tendon_pos_limits
...@@ -31,21 +31,21 @@ if TYPE_CHECKING: ...@@ -31,21 +31,21 @@ if TYPE_CHECKING:
class RigidObjectCollection(AssetBase): class RigidObjectCollection(AssetBase):
"""A rigid object collection class. """A rigid object collection class.
This class represents a collection of rigid objects in the simulation, where the state of the rigid objects can be This class represents a collection of rigid objects in the simulation, where the state of the
accessed and modified using a batched ``(env_ids, object_ids)`` API. rigid objects can be accessed and modified using a batched ``(env_ids, object_ids)`` API.
For each rigid body in the collection, the root prim of the asset must have the `USD RigidBodyAPI`_ For each rigid body in the collection, the root prim of the asset must have the `USD RigidBodyAPI`_
applied to it. This API is used to define the simulation properties of the rigid bodies. On playing the applied to it. This API is used to define the simulation properties of the rigid bodies. On playing the
simulation, the physics engine will automatically register the rigid bodies and create a corresponding simulation, the physics engine will automatically register the rigid bodies and create a corresponding
rigid body handle. This handle can be accessed using the :attr:`root_physx_view` attribute. rigid body handle. This handle can be accessed using the :attr:`root_physx_view` attribute.
.. note::
Rigid objects in the collection are uniquely identified via the key of the dictionary Rigid objects in the collection are uniquely identified via the key of the dictionary
:attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` in :class:`~isaaclab.assets.RigidObjectCollectionCfg`. :attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` in the
This differs from the class :class:`~isaaclab.assets.RigidObject`, where a rigid object is identified by :class:`~isaaclab.assets.RigidObjectCollectionCfg` configuration class.
the name of the Xform where the `USD RigidBodyAPI`_ is applied. This would not be possible for the rigid object This differs from the :class:`~isaaclab.assets.RigidObject` class, where a rigid object is identified by
collection since the :attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` dictionary could the name of the Xform where the `USD RigidBodyAPI`_ is applied. This would not be possible for the rigid
contain the same rigid object multiple times, leading to ambiguity. object collection since the :attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` dictionary
could contain the same rigid object multiple times, leading to ambiguity.
.. _`USD RigidBodyAPI`: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html .. _`USD RigidBodyAPI`: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html
""" """
...@@ -59,12 +59,15 @@ class RigidObjectCollection(AssetBase): ...@@ -59,12 +59,15 @@ class RigidObjectCollection(AssetBase):
Args: Args:
cfg: A configuration instance. cfg: A configuration instance.
""" """
# Note: We never call the parent constructor as it tries to call its own spawning which we don't want.
# check that the config is valid # check that the config is valid
cfg.validate() cfg.validate()
# store inputs # store inputs
self.cfg = cfg self.cfg = cfg.copy()
# flag for whether the asset is initialized # flag for whether the asset is initialized
self._is_initialized = False self._is_initialized = False
# spawn the rigid objects
for rigid_object_cfg in self.cfg.rigid_objects.values(): for rigid_object_cfg in self.cfg.rigid_objects.values():
# check if the rigid object path is valid # check if the rigid object path is valid
# note: currently the spawner does not work if there is a regex pattern in the leaf # note: currently the spawner does not work if there is a regex pattern in the leaf
...@@ -138,7 +141,7 @@ class RigidObjectCollection(AssetBase): ...@@ -138,7 +141,7 @@ class RigidObjectCollection(AssetBase):
Note: Note:
Use this view with caution. It requires handling of tensors in a specific way. Use this view with caution. It requires handling of tensors in a specific way.
""" """
return self._root_physx_view return self._root_physx_view # type: ignore
""" """
Operations. Operations.
......
...@@ -94,10 +94,11 @@ class RigidObjectCollectionData: ...@@ -94,10 +94,11 @@ class RigidObjectCollectionData:
## ##
default_object_state: torch.Tensor = None default_object_state: torch.Tensor = None
"""Default object state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. Shape is (num_instances, num_objects, 13). """Default object state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame.
Shape is (num_instances, num_objects, 13).
The position and quaternion are of each object's rigid body's actor frame. Meanwhile, the linear and angular velocities are The position and quaternion are of each object's rigid body's actor frame. Meanwhile, the linear and
of the center of mass frame. angular velocities are of the center of mass frame.
""" """
default_mass: torch.Tensor = None default_mass: torch.Tensor = None
...@@ -116,7 +117,8 @@ class RigidObjectCollectionData: ...@@ -116,7 +117,8 @@ class RigidObjectCollectionData:
@property @property
def object_state_w(self): def object_state_w(self):
"""Object state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, num_objects, 13). """Object state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame.
Shape is (num_instances, num_objects, 13).
The position and orientation are of the rigid body's actor frame. Meanwhile, the linear and angular The position and orientation are of the rigid body's actor frame. Meanwhile, the linear and angular
velocities are of the rigid body's center of mass frame. velocities are of the rigid body's center of mass frame.
...@@ -134,7 +136,8 @@ class RigidObjectCollectionData: ...@@ -134,7 +136,8 @@ class RigidObjectCollectionData:
@property @property
def object_link_state_w(self): def object_link_state_w(self):
"""Object center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, num_objects, 13). """Object center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame.
Shape is (num_instances, num_objects, 13).
The position, quaternion, and linear/angular velocity are of the rigid body root frame relative to the The position, quaternion, and linear/angular velocity are of the rigid body root frame relative to the
world. world.
...@@ -157,7 +160,8 @@ class RigidObjectCollectionData: ...@@ -157,7 +160,8 @@ class RigidObjectCollectionData:
@property @property
def object_com_state_w(self): def object_com_state_w(self):
"""Object state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, num_objects, 13). """Object state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame.
Shape is (num_instances, num_objects, 13).
The position, quaternion, and linear/angular velocity are of the rigid body's center of mass frame The position, quaternion, and linear/angular velocity are of the rigid body's center of mass frame
relative to the world. Center of mass frame is the orientation principle axes of inertia. relative to the world. Center of mass frame is the orientation principle axes of inertia.
...@@ -449,7 +453,7 @@ class RigidObjectCollectionData: ...@@ -449,7 +453,7 @@ class RigidObjectCollectionData:
"""Reshapes and arranges the data from the physics view to (num_instances, num_objects, data_size). """Reshapes and arranges the data from the physics view to (num_instances, num_objects, data_size).
Args: Args:
data: The data from the physics view. Shape is (num_instances*num_objects, data_size). data: The data from the physics view. Shape is (num_instances * num_objects, data_size).
Returns: Returns:
The reshaped data. Shape is (num_objects, num_instances, data_size). The reshaped data. Shape is (num_objects, num_instances, data_size).
......
...@@ -421,14 +421,15 @@ def randomize_joint_parameters( ...@@ -421,14 +421,15 @@ def randomize_joint_parameters(
operation: Literal["add", "scale", "abs"] = "abs", operation: Literal["add", "scale", "abs"] = "abs",
distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform",
): ):
"""Randomize the joint parameters of an articulation by adding, scaling, or setting random values. """Randomize the simulated joint parameters of an articulation by adding, scaling, or setting random values.
This function allows randomizing the joint parameters of the asset. This function allows randomizing the joint parameters of the asset. These correspond to the physics engine
These correspond to the physics engine joint properties that affect the joint behavior. joint properties that affect the joint behavior. The properties include the joint friction coefficient, armature,
and joint position limits.
The function samples random values from the given distribution parameters and applies the operation to the joint properties. The function samples random values from the given distribution parameters and applies the operation to the
It then sets the values into the physics simulation. If the distribution parameters are not provided for a joint properties. It then sets the values into the physics simulation. If the distribution parameters are
particular property, the function does not modify the property. not provided for a particular property, the function does not modify the property.
.. tip:: .. tip::
This function uses CPU tensors to assign the joint properties. It is recommended to use this function This function uses CPU tensors to assign the joint properties. It is recommended to use this function
...@@ -448,53 +449,66 @@ def randomize_joint_parameters( ...@@ -448,53 +449,66 @@ def randomize_joint_parameters(
joint_ids = torch.tensor(asset_cfg.joint_ids, dtype=torch.int, device=asset.device) joint_ids = torch.tensor(asset_cfg.joint_ids, dtype=torch.int, device=asset.device)
# sample joint properties from the given ranges and set into the physics simulation # sample joint properties from the given ranges and set into the physics simulation
# -- friction # joint friction coefficient
if friction_distribution_params is not None: if friction_distribution_params is not None:
friction = asset.data.default_joint_friction.to(asset.device).clone() friction_coeff = _randomize_prop_by_op(
friction = _randomize_prop_by_op( asset.data.default_joint_friction_coeff.clone(),
friction, friction_distribution_params, env_ids, joint_ids, operation=operation, distribution=distribution friction_distribution_params,
)[env_ids][:, joint_ids] env_ids,
asset.write_joint_friction_to_sim(friction, joint_ids=joint_ids, env_ids=env_ids) joint_ids,
# -- armature operation=operation,
distribution=distribution,
)
asset.write_joint_friction_coefficient_to_sim(
friction_coeff[env_ids[:, None], joint_ids], joint_ids=joint_ids, env_ids=env_ids
)
# joint armature
if armature_distribution_params is not None: if armature_distribution_params is not None:
armature = asset.data.default_joint_armature.to(asset.device).clone()
armature = _randomize_prop_by_op( armature = _randomize_prop_by_op(
armature, armature_distribution_params, env_ids, joint_ids, operation=operation, distribution=distribution asset.data.default_joint_armature.clone(),
)[env_ids][:, joint_ids] armature_distribution_params,
asset.write_joint_armature_to_sim(armature, joint_ids=joint_ids, env_ids=env_ids) env_ids,
# -- dof limits joint_ids,
operation=operation,
distribution=distribution,
)
asset.write_joint_armature_to_sim(armature[env_ids[:, None], joint_ids], joint_ids=joint_ids, env_ids=env_ids)
# joint position limits
if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None: if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None:
dof_limits = asset.data.default_joint_limits.to(asset.device).clone() joint_pos_limits = asset.data.default_joint_pos_limits.clone()
# -- randomize the lower limits
if lower_limit_distribution_params is not None: if lower_limit_distribution_params is not None:
lower_limits = dof_limits[..., 0] joint_pos_limits[..., 0] = _randomize_prop_by_op(
lower_limits = _randomize_prop_by_op( joint_pos_limits[..., 0],
lower_limits,
lower_limit_distribution_params, lower_limit_distribution_params,
env_ids, env_ids,
joint_ids, joint_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, joint_ids] )
dof_limits[env_ids[:, None], joint_ids, 0] = lower_limits # -- randomize the upper limits
if upper_limit_distribution_params is not None: if upper_limit_distribution_params is not None:
upper_limits = dof_limits[..., 1] joint_pos_limits[..., 1] = _randomize_prop_by_op(
upper_limits = _randomize_prop_by_op( joint_pos_limits[..., 1],
upper_limits,
upper_limit_distribution_params, upper_limit_distribution_params,
env_ids, env_ids,
joint_ids, joint_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, joint_ids] )
dof_limits[env_ids[:, None], joint_ids, 1] = upper_limits
if (dof_limits[env_ids[:, None], joint_ids, 0] > dof_limits[env_ids[:, None], joint_ids, 1]).any(): # extract the position limits for the concerned joints
joint_pos_limits = joint_pos_limits[env_ids[:, None], joint_ids]
if (joint_pos_limits[..., 0] > joint_pos_limits[..., 1]).any():
raise ValueError( raise ValueError(
"Randomization term 'randomize_joint_parameters' is setting lower joint limits that are greater than" "Randomization term 'randomize_joint_parameters' is setting lower joint limits that are greater than"
" upper joint limits." " upper joint limits. Please check the distribution parameters for the joint position limits."
) )
# set the position limits into the physics simulation
asset.write_joint_limits_to_sim( asset.write_joint_position_limit_to_sim(
dof_limits[env_ids][:, joint_ids], joint_ids=joint_ids, env_ids=env_ids, warn_limit_violation=False joint_pos_limits, joint_ids=joint_ids, env_ids=env_ids, warn_limit_violation=False
) )
...@@ -512,7 +526,7 @@ def randomize_fixed_tendon_parameters( ...@@ -512,7 +526,7 @@ def randomize_fixed_tendon_parameters(
operation: Literal["add", "scale", "abs"] = "abs", operation: Literal["add", "scale", "abs"] = "abs",
distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform",
): ):
"""Randomize the fixed tendon parameters of an articulation by adding, scaling, or setting random values. """Randomize the simulated fixed tendon parameters of an articulation by adding, scaling, or setting random values.
This function allows randomizing the fixed tendon parameters of the asset. This function allows randomizing the fixed tendon parameters of the asset.
These correspond to the physics engine tendon properties that affect the joint behavior. These correspond to the physics engine tendon properties that affect the joint behavior.
...@@ -531,106 +545,106 @@ def randomize_fixed_tendon_parameters( ...@@ -531,106 +545,106 @@ def randomize_fixed_tendon_parameters(
# resolve joint indices # resolve joint indices
if asset_cfg.fixed_tendon_ids == slice(None): if asset_cfg.fixed_tendon_ids == slice(None):
fixed_tendon_ids = slice(None) # for optimization purposes tendon_ids = slice(None) # for optimization purposes
else: else:
fixed_tendon_ids = torch.tensor(asset_cfg.fixed_tendon_ids, dtype=torch.int, device=asset.device) tendon_ids = torch.tensor(asset_cfg.fixed_tendon_ids, dtype=torch.int, device=asset.device)
# sample tendon properties from the given ranges and set into the physics simulation # sample tendon properties from the given ranges and set into the physics simulation
# -- stiffness # stiffness
if stiffness_distribution_params is not None: if stiffness_distribution_params is not None:
stiffness = asset.data.default_fixed_tendon_stiffness.clone()
stiffness = _randomize_prop_by_op( stiffness = _randomize_prop_by_op(
stiffness, asset.data.default_fixed_tendon_stiffness.clone(),
stiffness_distribution_params, stiffness_distribution_params,
env_ids, env_ids,
fixed_tendon_ids, tendon_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, fixed_tendon_ids] )
asset.set_fixed_tendon_stiffness(stiffness, fixed_tendon_ids, env_ids) asset.set_fixed_tendon_stiffness(stiffness[env_ids[:, None], tendon_ids], tendon_ids, env_ids)
# -- damping
# damping
if damping_distribution_params is not None: if damping_distribution_params is not None:
damping = asset.data.default_fixed_tendon_damping.clone()
damping = _randomize_prop_by_op( damping = _randomize_prop_by_op(
damping, asset.data.default_fixed_tendon_damping.clone(),
damping_distribution_params, damping_distribution_params,
env_ids, env_ids,
fixed_tendon_ids, tendon_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, fixed_tendon_ids] )
asset.set_fixed_tendon_damping(damping, fixed_tendon_ids, env_ids) asset.set_fixed_tendon_damping(damping[env_ids[:, None], tendon_ids], tendon_ids, env_ids)
# -- limit stiffness
# limit stiffness
if limit_stiffness_distribution_params is not None: if limit_stiffness_distribution_params is not None:
limit_stiffness = asset.data.default_fixed_tendon_limit_stiffness.clone()
limit_stiffness = _randomize_prop_by_op( limit_stiffness = _randomize_prop_by_op(
limit_stiffness, asset.data.default_fixed_tendon_limit_stiffness.clone(),
limit_stiffness_distribution_params, limit_stiffness_distribution_params,
env_ids, env_ids,
fixed_tendon_ids, tendon_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, fixed_tendon_ids] )
asset.set_fixed_tendon_limit_stiffness(limit_stiffness, fixed_tendon_ids, env_ids) asset.set_fixed_tendon_limit_stiffness(limit_stiffness[env_ids[:, None], tendon_ids], tendon_ids, env_ids)
# -- limits
# position limits
if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None: if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None:
limit = asset.data.default_fixed_tendon_limit.clone() limit = asset.data.default_fixed_tendon_pos_limits.clone()
# -- lower limit # -- lower limit
if lower_limit_distribution_params is not None: if lower_limit_distribution_params is not None:
lower_limit = limit[..., 0] limit[..., 0] = _randomize_prop_by_op(
lower_limit = _randomize_prop_by_op( limit[..., 0],
lower_limit,
lower_limit_distribution_params, lower_limit_distribution_params,
env_ids, env_ids,
fixed_tendon_ids, tendon_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, fixed_tendon_ids] )
limit[env_ids[:, None], fixed_tendon_ids, 0] = lower_limit
# -- upper limit # -- upper limit
if upper_limit_distribution_params is not None: if upper_limit_distribution_params is not None:
upper_limit = limit[..., 1] limit[..., 1] = _randomize_prop_by_op(
upper_limit = _randomize_prop_by_op( limit[..., 1],
upper_limit,
upper_limit_distribution_params, upper_limit_distribution_params,
env_ids, env_ids,
fixed_tendon_ids, tendon_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, fixed_tendon_ids] )
limit[env_ids[:, None], fixed_tendon_ids, 1] = upper_limit
if (limit[env_ids[:, None], fixed_tendon_ids, 0] > limit[env_ids[:, None], fixed_tendon_ids, 1]).any(): # check if the limits are valid
tendon_limits = limit[env_ids[:, None], tendon_ids]
if (tendon_limits[..., 0] > tendon_limits[..., 1]).any():
raise ValueError( raise ValueError(
"Randomization term 'randomize_fixed_tendon_parameters' is setting lower tendon limits that are greater" "Randomization term 'randomize_fixed_tendon_parameters' is setting lower tendon limits that are greater"
" than upper tendon limits." " than upper tendon limits."
) )
asset.set_fixed_tendon_limit(limit, fixed_tendon_ids, env_ids) asset.set_fixed_tendon_position_limit(tendon_limits, tendon_ids, env_ids)
# -- rest length
# rest length
if rest_length_distribution_params is not None: if rest_length_distribution_params is not None:
rest_length = asset.data.default_fixed_tendon_rest_length.clone()
rest_length = _randomize_prop_by_op( rest_length = _randomize_prop_by_op(
rest_length, asset.data.default_fixed_tendon_rest_length.clone(),
rest_length_distribution_params, rest_length_distribution_params,
env_ids, env_ids,
fixed_tendon_ids, tendon_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, fixed_tendon_ids] )
asset.set_fixed_tendon_rest_length(rest_length, fixed_tendon_ids, env_ids) asset.set_fixed_tendon_rest_length(rest_length[env_ids[:, None], tendon_ids], tendon_ids, env_ids)
# -- offset
# offset
if offset_distribution_params is not None: if offset_distribution_params is not None:
offset = asset.data.default_fixed_tendon_offset.clone()
offset = _randomize_prop_by_op( offset = _randomize_prop_by_op(
offset, asset.data.default_fixed_tendon_offset.clone(),
offset_distribution_params, offset_distribution_params,
env_ids, env_ids,
fixed_tendon_ids, tendon_ids,
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
)[env_ids][:, fixed_tendon_ids] )
asset.set_fixed_tendon_offset(offset, fixed_tendon_ids, env_ids) asset.set_fixed_tendon_offset(offset[env_ids[:, None], tendon_ids], tendon_ids, env_ids)
asset.write_fixed_tendon_properties_to_sim(fixed_tendon_ids, env_ids) # write the fixed tendon properties into the simulation
asset.write_fixed_tendon_properties_to_sim(tendon_ids, env_ids)
def apply_external_force_torque( def apply_external_force_torque(
......
...@@ -614,8 +614,8 @@ class TestArticulation(unittest.TestCase): ...@@ -614,8 +614,8 @@ class TestArticulation(unittest.TestCase):
# Check if articulation is initialized # Check if articulation is initialized
self.assertFalse(articulation._is_initialized) self.assertFalse(articulation._is_initialized)
def test_joint_limits(self): def test_joint_pos_limits(self):
"""Test write_joint_limits_to_sim API and when default pos falls outside of the new limits.""" """Test write_joint_position_limit_to_sim API and when default pos falls outside of the new limits."""
for num_articulations in (1, 2): for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"): for device in ("cuda:0", "cpu"):
with self.subTest(num_articulations=num_articulations, device=device): with self.subTest(num_articulations=num_articulations, device=device):
...@@ -639,10 +639,10 @@ class TestArticulation(unittest.TestCase): ...@@ -639,10 +639,10 @@ class TestArticulation(unittest.TestCase):
torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0
) * -1.0 ) * -1.0
limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0 limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0
articulation.write_joint_limits_to_sim(limits) articulation.write_joint_position_limit_to_sim(limits)
# Check new limits are in place # Check new limits are in place
torch.testing.assert_close(articulation._data.joint_limits, limits) torch.testing.assert_close(articulation._data.joint_pos_limits, limits)
torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos)
# Set new joint limits with indexing # Set new joint limits with indexing
...@@ -651,17 +651,17 @@ class TestArticulation(unittest.TestCase): ...@@ -651,17 +651,17 @@ class TestArticulation(unittest.TestCase):
limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device)
limits[..., 0] = (torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0) * -1.0 limits[..., 0] = (torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0) * -1.0
limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0 limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) + 5.0
articulation.write_joint_limits_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids) articulation.write_joint_position_limit_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids)
# Check new limits are in place # Check new limits are in place
torch.testing.assert_close(articulation._data.joint_limits[env_ids][:, joint_ids], limits) torch.testing.assert_close(articulation._data.joint_pos_limits[env_ids][:, joint_ids], limits)
torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos)
# Set new joint limits that invalidate default joint pos # Set new joint limits that invalidate default joint pos
limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device)
limits[..., 0] = torch.rand(num_articulations, articulation.num_joints, device=device) * -0.1 limits[..., 0] = torch.rand(num_articulations, articulation.num_joints, device=device) * -0.1
limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1 limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1
articulation.write_joint_limits_to_sim(limits) articulation.write_joint_position_limit_to_sim(limits)
# Check if all values are within the bounds # Check if all values are within the bounds
within_bounds = (articulation._data.default_joint_pos >= limits[..., 0]) & ( within_bounds = (articulation._data.default_joint_pos >= limits[..., 0]) & (
...@@ -673,7 +673,7 @@ class TestArticulation(unittest.TestCase): ...@@ -673,7 +673,7 @@ class TestArticulation(unittest.TestCase):
limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device)
limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1
limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1
articulation.write_joint_limits_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids) articulation.write_joint_position_limit_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids)
# Check if all values are within the bounds # Check if all values are within the bounds
within_bounds = ( within_bounds = (
......
...@@ -50,7 +50,7 @@ class EventCfg: ...@@ -50,7 +50,7 @@ class EventCfg:
"distribution": "log_uniform", "distribution": "log_uniform",
}, },
) )
robot_joint_limits = EventTerm( robot_joint_pos_limits = EventTerm(
func=mdp.randomize_joint_parameters, func=mdp.randomize_joint_parameters,
min_step_count_between_reset=720, min_step_count_between_reset=720,
mode="reset", mode="reset",
......
...@@ -48,7 +48,7 @@ class EventCfg: ...@@ -48,7 +48,7 @@ class EventCfg:
"distribution": "log_uniform", "distribution": "log_uniform",
}, },
) )
robot_joint_limits = EventTerm( robot_joint_pos_limits = EventTerm(
func=mdp.randomize_joint_parameters, func=mdp.randomize_joint_parameters,
min_step_count_between_reset=720, min_step_count_between_reset=720,
mode="reset", mode="reset",
......
...@@ -140,8 +140,8 @@ class RewardsCfg: ...@@ -140,8 +140,8 @@ class RewardsCfg:
# (6) Penalty for energy consumption # (6) Penalty for energy consumption
energy = RewTerm(func=mdp.power_consumption, weight=-0.05, params={"gear_ratio": {".*": 15.0}}) energy = RewTerm(func=mdp.power_consumption, weight=-0.05, params={"gear_ratio": {".*": 15.0}})
# (7) Penalty for reaching close to joint limits # (7) Penalty for reaching close to joint limits
joint_limits = RewTerm( joint_pos_limits = RewTerm(
func=mdp.joint_limits_penalty_ratio, weight=-0.1, params={"threshold": 0.99, "gear_ratio": {".*": 15.0}} func=mdp.joint_pos_limits_penalty_ratio, weight=-0.1, params={"threshold": 0.99, "gear_ratio": {".*": 15.0}}
) )
......
...@@ -210,8 +210,8 @@ class RewardsCfg: ...@@ -210,8 +210,8 @@ class RewardsCfg:
}, },
) )
# (7) Penalty for reaching close to joint limits # (7) Penalty for reaching close to joint limits
joint_limits = RewTerm( joint_pos_limits = RewTerm(
func=mdp.joint_limits_penalty_ratio, func=mdp.joint_pos_limits_penalty_ratio,
weight=-0.25, weight=-0.25,
params={ params={
"threshold": 0.98, "threshold": 0.98,
......
...@@ -77,8 +77,8 @@ class progress_reward(ManagerTermBase): ...@@ -77,8 +77,8 @@ class progress_reward(ManagerTermBase):
return self.potentials - self.prev_potentials return self.potentials - self.prev_potentials
class joint_limits_penalty_ratio(ManagerTermBase): class joint_pos_limits_penalty_ratio(ManagerTermBase):
"""Penalty for violating joint limits weighted by the gear ratio.""" """Penalty for violating joint position limits weighted by the gear ratio."""
def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg):
# add default argument # add default argument
......
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