Unverified Commit 4c4377db authored by James Tigue's avatar James Tigue Committed by GitHub

Adds velocity_limit_sim and effort_limit_sim to actuator (#1654)

# Description

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link:
https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html
-->

This PR follows up on [Issue
1384](https://github.com/isaac-sim/IsaacLab/issues/1384) and [PR
1509](https://github.com/isaac-sim/IsaacLab/pull/1509) by seperating
actuator limits for calculating computed torques from physics solver
limits.

Fixes # (#1384)

<!-- As a practice, it is recommended to open an issue to have
discussions on the proposed pull request.
This makes it easier for the community to keep track of what is being
developed or added, and if a given feature
is demanded by more than one party. -->

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

- Bug fix (non-breaking change which fixes an issue)
- New feature (non-breaking change which adds functionality)

## Screenshots

Please attach before and after screenshots of the change if applicable.

<!--
Example:

| Before | After |
| ------ | ----- |
| _gif/png before_ | _gif/png after_ |

To upload images to a PR -- simply drag and drop an image while in edit
mode and it should upload the image directly. You can then paste that
source into the above before/after sections.
-->

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [ ] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

---------
Signed-off-by: 's avatarJames Tigue <166445701+jtigue-bdai@users.noreply.github.com>
Co-authored-by: 's avatarPascal Roth <57946385+pascal-roth@users.noreply.github.com>
parent 157c6b74
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.33.17" version = "0.34.0"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.34.0 (2025-02-14)
~~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
* Adds attributes velocity_limits_sim and effort_limits_sim to :class:`isaaclab.actuators.AssetBaseCfg` to separate
solver limits from actuator limits.
0.33.17 (2025-02-13) 0.33.17 (2025-02-13)
~~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~
......
...@@ -110,6 +110,8 @@ class ActuatorBase(ABC): ...@@ -110,6 +110,8 @@ class ActuatorBase(ABC):
# note: for velocity limits, we don't have USD parameter, so default is infinity # note: for velocity limits, we don't have USD parameter, so default is infinity
self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, effort_limit) self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, effort_limit)
self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, velocity_limit) self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, velocity_limit)
self.effort_limit_sim = self._parse_joint_parameter(self.cfg.effort_limit_sim, effort_limit)
self.velocity_limit_sim = self._parse_joint_parameter(self.cfg.velocity_limit_sim, velocity_limit)
# create commands buffers for allocation # create commands buffers for allocation
self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device) self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device)
......
...@@ -33,13 +33,45 @@ class ActuatorBaseCfg: ...@@ -33,13 +33,45 @@ 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.
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.
.. note::
For ImplicitActuators this value will be collapsed with effort_limit_sim due to duplicating functionality. If
both are set the effort_limit_sim will be used priority.
""" """
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.
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.
.. note::
velocity_limit is not used in ActuatorBaseCfg but is provided for inherited version like
:class:`isaaclab.actuators.DCMotor`.
.. note::
For ImplicitActuators this value will be collapsed with velocity_limit_sim due to duplicating functionality. If
both are set the effort_limit_sim will be used priority.
"""
effort_limit_sim: dict[str, float] | float | None = None
"""Force/Torque limit of the joints in the group that will be propagated to the simulation physics solver. Defaults to None.
If None, the limit is set to the value specified in the USD joint prim for ImplicitActuators or 1.0e9 for explicit
actuators (e.g. IdealPDActuator). The simulation effort limits prevent computed torques from exceeding the specified
limit. If effort limits are too tight issues with solver convergence may occur. It is suggested to keep these value large.
"""
velocity_limit_sim: dict[str, float] | float | None = None
"""Velocity limit of the joints in the group that will be propagated to the simulation physics solver. Defaults to None.
If None, the limit is set to the value specified in the USD joint prim. Resulting solver solutions will constrain
velocities by these limits. If velocity_limit_sim is too tight issues with solver convergence may occur. It is
suggested to keep these value large.
""" """
stiffness: dict[str, float] | float | None = MISSING stiffness: dict[str, float] | float | None = MISSING
......
...@@ -1339,11 +1339,36 @@ class Articulation(AssetBase): ...@@ -1339,11 +1339,36 @@ class Articulation(AssetBase):
# set the passed gains and limits into the simulation # set the passed gains and limits into the simulation
if isinstance(actuator, ImplicitActuator): if isinstance(actuator, ImplicitActuator):
self._has_implicit_actuators = True self._has_implicit_actuators = True
# resolve actuator limit duplication for ImplicitActuators
# effort limits
if actuator.effort_limit_sim is None and actuator.effort_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has effort_limit_sim=None but is specifying effort_limit."
"effort_limit will be applied to effort_limit_sim for ImplicitActuators."
)
actuator.effort_limit_sim = actuator.effort_limit
elif actuator.effort_limit_sim is not None and actuator.effort_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has set both effort_limit_sim and effort_limit."
"Only effort_limit_sim will be used for ImplicitActuators."
)
# velocity limits
if actuator.velocity_limit_sim is None and actuator.velocity_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has velocity_limit_sim=None but is specifying"
" velocity_limit.velocity_limit will be applied to velocity_limit_sim for ImplicitActuators."
)
actuator.velocity_limit_sim = actuator.velocity_limit
elif actuator.velocity_limit_sim is not None and actuator.velocity_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has set both velocity_limit_sim and velocity_limit."
"Only velocity_limit_sim will be used for ImplicitActuators."
)
# the gains and limits are set into the simulation since actuator model is implicit # the gains and limits are set into the simulation since actuator model is implicit
self.write_joint_stiffness_to_sim(actuator.stiffness, joint_ids=actuator.joint_indices) self.write_joint_stiffness_to_sim(actuator.stiffness, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(actuator.damping, joint_ids=actuator.joint_indices) self.write_joint_damping_to_sim(actuator.damping, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(actuator.effort_limit, 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, 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_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
else: else:
...@@ -1351,8 +1376,11 @@ class Articulation(AssetBase): ...@@ -1351,8 +1376,11 @@ class Articulation(AssetBase):
# we set gains to zero, and torque limit to a high value in simulation to avoid any interference # we set gains to zero, and torque limit to a high value in simulation to avoid any interference
self.write_joint_stiffness_to_sim(0.0, joint_ids=actuator.joint_indices) self.write_joint_stiffness_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(0.0, joint_ids=actuator.joint_indices) self.write_joint_damping_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(1.0e9, joint_ids=actuator.joint_indices) self.write_joint_effort_limit_to_sim(
self.write_joint_velocity_limit_to_sim(actuator.velocity_limit, joint_ids=actuator.joint_indices) 1.0e9 if actuator.effort_limit_sim is None else actuator.effort_limit_sim,
joint_ids=actuator.joint_indices,
)
self.write_joint_velocity_limit_to_sim(actuator.velocity_limit_sim, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices) self.write_joint_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_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
# Store the actual default stiffness and damping values for explicit and implicit actuators (not written the sim) # Store the actual default stiffness and damping values for explicit and implicit actuators (not written the sim)
......
...@@ -43,8 +43,8 @@ def generate_articulation_cfg( ...@@ -43,8 +43,8 @@ def generate_articulation_cfg(
articulation_type: Literal["humanoid", "panda", "anymal", "shadow_hand", "single_joint"], articulation_type: Literal["humanoid", "panda", "anymal", "shadow_hand", "single_joint"],
stiffness: float | None = 10.0, stiffness: float | None = 10.0,
damping: float | None = 2.0, damping: float | None = 2.0,
vel_limit: float | None = 100.0, vel_limit_sim: float | None = None,
effort_limit: float | None = 400.0, effort_limit_sim: float | None = None,
) -> ArticulationCfg: ) -> ArticulationCfg:
"""Generate an articulation configuration. """Generate an articulation configuration.
...@@ -75,8 +75,8 @@ def generate_articulation_cfg( ...@@ -75,8 +75,8 @@ def generate_articulation_cfg(
actuators={ actuators={
"joint": ImplicitActuatorCfg( "joint": ImplicitActuatorCfg(
joint_names_expr=[".*"], joint_names_expr=[".*"],
effort_limit=effort_limit, effort_limit_sim=effort_limit_sim,
velocity_limit=vel_limit, velocity_limit_sim=vel_limit_sim,
stiffness=0.0, stiffness=0.0,
damping=10.0, damping=10.0,
), ),
...@@ -900,7 +900,7 @@ class TestArticulation(unittest.TestCase): ...@@ -900,7 +900,7 @@ class TestArticulation(unittest.TestCase):
torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness) torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness)
torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping)
def test_setting_velocity_limits(self): def test_setting_velocity_sim_limits(self):
"""Test that velocity limits are loaded form the configuration correctly.""" """Test that velocity limits are loaded form the configuration correctly."""
for num_articulations in (1, 2): for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"): for device in ("cuda:0", "cpu"):
...@@ -911,7 +911,7 @@ class TestArticulation(unittest.TestCase): ...@@ -911,7 +911,7 @@ class TestArticulation(unittest.TestCase):
) as sim: ) as sim:
sim._app_control_on_stop_handle = None sim._app_control_on_stop_handle = None
articulation_cfg = generate_articulation_cfg( articulation_cfg = generate_articulation_cfg(
articulation_type="single_joint", vel_limit=limit, effort_limit=limit articulation_type="single_joint", vel_limit_sim=limit, effort_limit_sim=limit
) )
articulation, _ = generate_articulation( articulation, _ = generate_articulation(
articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device
...@@ -928,7 +928,7 @@ class TestArticulation(unittest.TestCase): ...@@ -928,7 +928,7 @@ class TestArticulation(unittest.TestCase):
) )
# Check that gains are loaded from USD file # Check that gains are loaded from USD file
torch.testing.assert_close( torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit, expected_velocity_limit articulation.actuators["joint"].velocity_limit_sim, expected_velocity_limit
) )
torch.testing.assert_close( torch.testing.assert_close(
articulation.data.joint_velocity_limits, expected_velocity_limit articulation.data.joint_velocity_limits, expected_velocity_limit
......
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