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 @@
"arange",
"discretization",
"trimesh",
"uninstanceable"
"uninstanceable",
"coeff"
],
// This enables python language server. Seems to work slightly better than jedi:
"python.languageServer": "Pylance",
......
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.34.9"
version = "0.35.0"
# Description
title = "Isaac Lab framework for Robot Learning"
......
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)
~~~~~~~~~~~~~~~~~~~
......
......@@ -33,53 +33,58 @@ class ActuatorBaseCfg:
effort_limit: dict[str, float] | float | None = None
"""Force/Torque limit of the joints in the group. Defaults to None.
This limit is used to clip the computed torque sent to the simulation. If None, the limit is set to the value
specified in the USD joint prim.
This limit is used to clip the computed torque sent to the simulation. If None, the
limit is set to the value specified in the USD joint prim.
.. attention::
The :attr:`effort_limit_sim` attribute should be used to set the effort limit for the simulation physics
solver.
The :attr:`effort_limit_sim` attribute should be used to set the effort limit for
the simulation physics solver.
The :attr:`effort_limit` attribute is used for clipping the effort output of the actuator model *only*
in the case of explicit actuators, such as the :class:`~isaaclab.actuators.IdealPDActuator`.
The :attr:`effort_limit` attribute is used for clipping the effort output of the
actuator model **only** in the case of explicit actuators, such as the
:class:`~isaaclab.actuators.IdealPDActuator`.
.. note::
For implicit actuators, the attributes :attr:`effort_limit` and :attr:`effort_limit_sim` are equivalent.
However, we suggest using the :attr:`effort_limit_sim` attribute because it is more intuitive.
For implicit actuators, the attributes :attr:`effort_limit` and :attr:`effort_limit_sim`
are equivalent. However, we suggest using the :attr:`effort_limit_sim` attribute because
it is more intuitive.
"""
velocity_limit: dict[str, float] | float | None = None
"""Velocity limit of the joints in the group. Defaults to None.
This limit is used by the actuator model. If None, the limit is set to the value specified in the USD joint prim.
This limit is used by the actuator model. If None, the limit is set to the value specified
in the USD joint prim.
.. attention::
The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for the simulation physics
solver.
The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for
the simulation physics solver.
The :attr:`velocity_limit` attribute is used for clipping the effort output of the actuator model *only*
in the case of explicit actuators, such as the :class:`~isaaclab.actuators.IdealPDActuator`.
The :attr:`velocity_limit` attribute is used for clipping the effort output of the
actuator model **only** in the case of explicit actuators, such as the
:class:`~isaaclab.actuators.IdealPDActuator`.
.. note::
For implicit actuators, the attribute :attr:`velocity_limit` is not used. This is to stay backwards compatible
with previous versions of the Isaac Lab, where this parameter was unused since PhysX did not support setting
the velocity limit for the joints using the Tensor API.
For implicit actuators, the attribute :attr:`velocity_limit` is not used. This is to stay
backwards compatible with previous versions of the Isaac Lab, where this parameter was
unused since PhysX did not support setting the velocity limit for the joints using the
PhysX Tensor API.
"""
effort_limit_sim: dict[str, float] | float | None = None
"""Effort limit of the joints in the group applied to the simulation physics solver. Defaults to None.
The effort limit is used to constrain the computed joint efforts in the physics engine. If the computed effort
exceeds this limit, the physics engine will clip the effort to this value.
The effort limit is used to constrain the computed joint efforts in the physics engine. If the
computed effort exceeds this limit, the physics engine will clip the effort to this value.
Since explicit actuators (e.g. DC motor), compute and clip the effort in the actuator model, this limit is by
default set to a large value to prevent the physics engine from any additional clipping. However, at times,
it may be necessary to set this limit to a smaller value as a safety measure.
Since explicit actuators (e.g. DC motor), compute and clip the effort in the actuator model, this
limit is by default set to a large value to prevent the physics engine from any additional clipping.
However, at times, it may be necessary to set this limit to a smaller value as a safety measure.
If None, the limit is resolved based on the type of actuator model:
......@@ -91,40 +96,61 @@ class ActuatorBaseCfg:
velocity_limit_sim: dict[str, float] | float | None = None
"""Velocity limit of the joints in the group applied to the simulation physics solver. Defaults to None.
The velocity limit is used to constrain the joint velocities in the physics engine.
The joint will only be able to reach this velocity if the :attr:`effort_limit_sim` is sufficiently large.
If the joint is moving faster than this velocity, the physics engine will actually try to brake the joint
to reach this velocity.
The velocity limit is used to constrain the joint velocities in the physics engine. The joint will only
be able to reach this velocity if the joint's effort limit is sufficiently large. If the joint is moving
faster than this velocity, the physics engine will actually try to brake the joint to reach this velocity.
If None, the limit is set to the value specified in the USD joint prim for both implicit and explicit actuators.
.. tip::
If the velocity limit is too tight, the physics engine may have trouble converging to a solution.
In such cases, we recommend either keeping this value sufficiently large or tuning the stiffness and damping
parameters of the joint to ensure the limits are not violated.
In such cases, we recommend either keeping this value sufficiently large or tuning the stiffness and
damping parameters of the joint to ensure the limits are not violated.
"""
stiffness: dict[str, float] | float | None = MISSING
"""Stiffness gains (also known as p-gain) of the joints in the group.
The behavior of the stiffness is different for implicit and explicit actuators. For implicit actuators,
the stiffness gets set into the physics engine directly. For explicit actuators, the stiffness is used
by the actuator model to compute the joint efforts.
If None, the stiffness is set to the value from the USD joint prim.
"""
damping: dict[str, float] | float | None = MISSING
"""Damping gains (also known as d-gain) of the joints in the group.
The behavior of the damping is different for implicit and explicit actuators. For implicit actuators,
the damping gets set into the physics engine directly. For explicit actuators, the damping gain is used
by the actuator model to compute the joint efforts.
If None, the damping is set to the value from the USD joint prim.
"""
armature: dict[str, float] | float | None = None
"""Armature of the joints in the group. Defaults to None.
The armature is directly added to the corresponding joint-space inertia. It helps improve the
simulation stability by reducing the joint velocities.
It is a physics engine solver parameter that gets set into the simulation.
If None, the armature is set to the value from the USD joint prim.
"""
friction: dict[str, float] | float | None = None
"""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.
"""
......
......@@ -42,10 +42,13 @@ class ArticulationCfg(AssetBaseCfg):
"""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
"""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.
This is accessible in the articulation data through :attr:`ArticulationData.soft_joint_pos_limits` attribute.
The soft joint position limits are scaled by this factor to specify a safety region within the simulated
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
......
......@@ -31,21 +31,21 @@ if TYPE_CHECKING:
class RigidObjectCollection(AssetBase):
"""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
accessed and modified using a batched ``(env_ids, object_ids)`` API.
This class represents a collection of rigid objects in the simulation, where the state of the
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`_
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
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
:attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` in :class:`~isaaclab.assets.RigidObjectCollectionCfg`.
This differs from the class :class:`~isaaclab.assets.RigidObject`, where a rigid object is identified by
the name of the Xform where the `USD RigidBodyAPI`_ is applied. This would not be possible for the rigid object
collection since the :attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` dictionary could
contain the same rigid object multiple times, leading to ambiguity.
:attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` in the
:class:`~isaaclab.assets.RigidObjectCollectionCfg` configuration class.
This differs from the :class:`~isaaclab.assets.RigidObject` class, where a rigid object is identified by
the name of the Xform where the `USD RigidBodyAPI`_ is applied. This would not be possible for the rigid
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
"""
......@@ -59,12 +59,15 @@ class RigidObjectCollection(AssetBase):
Args:
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
cfg.validate()
# store inputs
self.cfg = cfg
self.cfg = cfg.copy()
# flag for whether the asset is initialized
self._is_initialized = False
# spawn the rigid objects
for rigid_object_cfg in self.cfg.rigid_objects.values():
# check if the rigid object path is valid
# note: currently the spawner does not work if there is a regex pattern in the leaf
......@@ -138,7 +141,7 @@ class RigidObjectCollection(AssetBase):
Note:
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.
......
......@@ -94,10 +94,11 @@ class RigidObjectCollectionData:
##
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
of the center of mass frame.
The position and quaternion are of each object's rigid body's actor frame. Meanwhile, the linear and
angular velocities are of the center of mass frame.
"""
default_mass: torch.Tensor = None
......@@ -116,7 +117,8 @@ class RigidObjectCollectionData:
@property
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
velocities are of the rigid body's center of mass frame.
......@@ -134,7 +136,8 @@ class RigidObjectCollectionData:
@property
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
world.
......@@ -157,7 +160,8 @@ class RigidObjectCollectionData:
@property
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
relative to the world. Center of mass frame is the orientation principle axes of inertia.
......@@ -449,7 +453,7 @@ class RigidObjectCollectionData:
"""Reshapes and arranges the data from the physics view to (num_instances, num_objects, data_size).
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:
The reshaped data. Shape is (num_objects, num_instances, data_size).
......
This diff is collapsed.
......@@ -614,8 +614,8 @@ class TestArticulation(unittest.TestCase):
# Check if articulation is initialized
self.assertFalse(articulation._is_initialized)
def test_joint_limits(self):
"""Test write_joint_limits_to_sim API and when default pos falls outside of the new limits."""
def test_joint_pos_limits(self):
"""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 device in ("cuda:0", "cpu"):
with self.subTest(num_articulations=num_articulations, device=device):
......@@ -639,10 +639,10 @@ class TestArticulation(unittest.TestCase):
torch.rand(num_articulations, articulation.num_joints, device=device) + 5.0
) * -1.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
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)
# Set new joint limits with indexing
......@@ -651,17 +651,17 @@ class TestArticulation(unittest.TestCase):
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[..., 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
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)
# Set new joint limits that invalidate default joint pos
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[..., 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
within_bounds = (articulation._data.default_joint_pos >= limits[..., 0]) & (
......@@ -673,7 +673,7 @@ class TestArticulation(unittest.TestCase):
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[..., 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
within_bounds = (
......
......@@ -50,7 +50,7 @@ class EventCfg:
"distribution": "log_uniform",
},
)
robot_joint_limits = EventTerm(
robot_joint_pos_limits = EventTerm(
func=mdp.randomize_joint_parameters,
min_step_count_between_reset=720,
mode="reset",
......
......@@ -48,7 +48,7 @@ class EventCfg:
"distribution": "log_uniform",
},
)
robot_joint_limits = EventTerm(
robot_joint_pos_limits = EventTerm(
func=mdp.randomize_joint_parameters,
min_step_count_between_reset=720,
mode="reset",
......
......@@ -140,8 +140,8 @@ class RewardsCfg:
# (6) Penalty for energy consumption
energy = RewTerm(func=mdp.power_consumption, weight=-0.05, params={"gear_ratio": {".*": 15.0}})
# (7) Penalty for reaching close to joint limits
joint_limits = RewTerm(
func=mdp.joint_limits_penalty_ratio, weight=-0.1, params={"threshold": 0.99, "gear_ratio": {".*": 15.0}}
joint_pos_limits = RewTerm(
func=mdp.joint_pos_limits_penalty_ratio, weight=-0.1, params={"threshold": 0.99, "gear_ratio": {".*": 15.0}}
)
......
......@@ -210,8 +210,8 @@ class RewardsCfg:
},
)
# (7) Penalty for reaching close to joint limits
joint_limits = RewTerm(
func=mdp.joint_limits_penalty_ratio,
joint_pos_limits = RewTerm(
func=mdp.joint_pos_limits_penalty_ratio,
weight=-0.25,
params={
"threshold": 0.98,
......
......@@ -77,8 +77,8 @@ class progress_reward(ManagerTermBase):
return self.potentials - self.prev_potentials
class joint_limits_penalty_ratio(ManagerTermBase):
"""Penalty for violating joint limits weighted by the gear ratio."""
class joint_pos_limits_penalty_ratio(ManagerTermBase):
"""Penalty for violating joint position limits weighted by the gear ratio."""
def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg):
# 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