Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds velocity_limit_sim and effort_limit_sim to actuator #1654

Open
wants to merge 10 commits into
base: main
Choose a base branch
from
2 changes: 1 addition & 1 deletion source/isaaclab/config/extension.toml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
[package]

# Note: Semantic Versioning is used: https://semver.org/
version = "0.33.17"
version = "0.34.0"

# Description
title = "Isaac Lab framework for Robot Learning"
Expand Down
10 changes: 10 additions & 0 deletions source/isaaclab/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,6 +1,16 @@
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)
~~~~~~~~~~~~~~~~~~~~

Expand Down
2 changes: 2 additions & 0 deletions source/isaaclab/isaaclab/actuators/actuator_base.py
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,8 @@ def __init__(
# note: for velocity limits, we don't have USD parameter, so default is infinity
self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, effort_limit)
self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, velocity_limit)
self.effort_limit_sim = self._parse_joint_parameter(self.cfg.effort_limit_sim, effort_limit)
self.velocity_limit_sim = self._parse_joint_parameter(self.cfg.velocity_limit_sim, velocity_limit)

# create commands buffers for allocation
self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device)
Expand Down
36 changes: 34 additions & 2 deletions source/isaaclab/isaaclab/actuators/actuator_cfg.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,13 +33,45 @@ class ActuatorBaseCfg:
effort_limit: dict[str, float] | float | None = 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 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
pascal-roth marked this conversation as resolved.
Show resolved Hide resolved
jtigue-bdai marked this conversation as resolved.
Show resolved Hide resolved
"""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
Expand Down
36 changes: 32 additions & 4 deletions source/isaaclab/isaaclab/assets/articulation/articulation.py
Original file line number Diff line number Diff line change
Expand Up @@ -1339,20 +1339,48 @@ def _process_actuators_cfg(self):
# set the passed gains and limits into the simulation
if isinstance(actuator, ImplicitActuator):
self._has_implicit_actuators = True
# resolve actuator limit duplication for ImplicitActuators
# effort limits
if actuator.effort_limit_sim is None and actuator.effort_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has effort_limit_sim=None but is specifying effort_limit."
"effort_limit will be applied to effort_limit_sim for ImplicitActuators."
)
actuator.effort_limit_sim = actuator.effort_limit
elif actuator.effort_limit_sim is not None and actuator.effort_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has set both effort_limit_sim and effort_limit."
"Only effort_limit_sim will be used for ImplicitActuators."
)
# velocity limits
if actuator.velocity_limit_sim is None and actuator.velocity_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has velocity_limit_sim=None but is specifying"
" velocity_limit.velocity_limit will be applied to velocity_limit_sim for ImplicitActuators."
)
actuator.velocity_limit_sim = actuator.velocity_limit
elif actuator.velocity_limit_sim is not None and actuator.velocity_limit is not None:
omni.log.warn(
f"ImplicitActuatorCfg {actuator_name} has set both velocity_limit_sim and velocity_limit."
"Only velocity_limit_sim will be used for ImplicitActuators."
)
# the gains and limits are set into the simulation since actuator model is implicit
self.write_joint_stiffness_to_sim(actuator.stiffness, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(actuator.damping, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(actuator.effort_limit, joint_ids=actuator.joint_indices)
self.write_joint_velocity_limit_to_sim(actuator.velocity_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_sim, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
else:
# the gains and limits are processed by the actuator model
# we set gains to zero, and torque limit to a high value in simulation to avoid any interference
self.write_joint_stiffness_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(1.0e9, joint_ids=actuator.joint_indices)
self.write_joint_velocity_limit_to_sim(actuator.velocity_limit, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(
1.0e9 if actuator.effort_limit_sim is None else actuator.effort_limit_sim,
joint_ids=actuator.joint_indices,
)
self.write_joint_velocity_limit_to_sim(actuator.velocity_limit_sim, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
# Store the actual default stiffness and damping values for explicit and implicit actuators (not written the sim)
Expand Down
14 changes: 7 additions & 7 deletions source/isaaclab/test/assets/test_articulation.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,8 @@ def generate_articulation_cfg(
articulation_type: Literal["humanoid", "panda", "anymal", "shadow_hand", "single_joint"],
stiffness: float | None = 10.0,
damping: float | None = 2.0,
vel_limit: float | None = 100.0,
effort_limit: float | None = 400.0,
vel_limit_sim: float | None = None,
effort_limit_sim: float | None = None,
) -> ArticulationCfg:
"""Generate an articulation configuration.

Expand Down Expand Up @@ -75,8 +75,8 @@ def generate_articulation_cfg(
actuators={
"joint": ImplicitActuatorCfg(
joint_names_expr=[".*"],
effort_limit=effort_limit,
velocity_limit=vel_limit,
effort_limit_sim=effort_limit_sim,
velocity_limit_sim=vel_limit_sim,
stiffness=0.0,
damping=10.0,
),
Expand Down Expand Up @@ -900,7 +900,7 @@ def test_setting_gains_from_cfg_dict(self):
torch.testing.assert_close(articulation.actuators["body"].stiffness, expected_stiffness)
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."""
for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"):
Expand All @@ -911,7 +911,7 @@ def test_setting_velocity_limits(self):
) as sim:
sim._app_control_on_stop_handle = None
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_cfg=articulation_cfg, num_articulations=num_articulations, device=device
Expand All @@ -928,7 +928,7 @@ def test_setting_velocity_limits(self):
)
# Check that gains are loaded from USD file
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(
articulation.data.joint_velocity_limits, expected_velocity_limit
Expand Down