Skip to content

Fixes DCMotor clipping for negative power and adds actuator tests #2300

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

Open
wants to merge 11 commits into
base: main
Choose a base branch
from
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
5 changes: 5 additions & 0 deletions docs/source/api/lab/isaaclab.actuators.rst
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,11 @@ Ideal PD Actuator
DC Motor Actuator
-----------------

.. figure:: ../../../_static/overview/actuator-group/dc_motor_clipping.jpg
:align: center
:figwidth: 100%
:alt: The effort clipping as a function of joint velocity for a linear DC Motor.

.. autoclass:: DCMotor
:members:
:inherited-members:
Expand Down
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.36.3"
version = "0.36.4"

# Description
title = "Isaac Lab framework for Robot Learning"
Expand Down
16 changes: 16 additions & 0 deletions source/isaaclab/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,6 +1,22 @@
Changelog
---------

0.37.0 (2025-04-10)
~~~~~~~~~~~~~~~~~~~

Added
^^^^^^^

* Added unit tests for :class:`~isaaclab.actuator.ImplicitActuator`, :class:`~isaaclab.actuator.IdealPDActuator`,
and :class:`~isaaclab.actuator.DCMotor` independent of :class:`~isaaclab.assets.Articulation`

Changed
^^^^^^^

* Changed the way clipping is handled for :class:`~isaaclab.actuator.DCMotor` for torque-speed points in when in
negative power regions.


0.36.4 (2025-03-24)
~~~~~~~~~~~~~~~~~~~

Expand Down
52 changes: 32 additions & 20 deletions source/isaaclab/isaaclab/actuators/actuator_pd.py
Original file line number Diff line number Diff line change
Expand Up @@ -201,8 +201,8 @@ def compute(
class DCMotor(IdealPDActuator):
r"""Direct control (DC) motor actuator model with velocity-based saturation model.

It uses the same model as the :class:`IdealActuator` for computing the torques from input commands.
However, it implements a saturation model defined by DC motor characteristics.
It uses the same model as the :class:`IdealPDActuator` for computing the torques from input commands.
However, it implements a saturation model defined by a linear four quadrant DC motor torque-speed curve.

A DC motor is a type of electric motor that is powered by direct current electricity. In most cases,
the motor is connected to a constant source of voltage supply, and the current is controlled by a rheostat.
Expand All @@ -211,18 +211,21 @@ class DCMotor(IdealPDActuator):

A DC motor characteristics are defined by the following parameters:

* Continuous-rated speed (:math:`\dot{q}_{motor, max}`) : The maximum-rated speed of the motor.
* Continuous-stall torque (:math:`\tau_{motor, max}`): The maximum-rated torque produced at 0 speed.
* Saturation torque (:math:`\tau_{motor, sat}`): The maximum torque that can be outputted for a short period.
* No-load speed (:math:`\dot{q}_{motor, max}`) : The maximum-rated speed of the motor at 0 Torque (:attr:`velocity_limit`).
* Stall torque (:math:`\tau_{motor, max}`): The maximum-rated torque produced at 0 speed (:attr:`saturation_effort`).
* Continuous torque (:math:`\tau_{motor, con}`): The maximum torque that can be outputted for a short period. This
is often enforced on the current drives for a DC motor to limit overheating, prevent mechanical damage, or
enforced by electrical limitations.(:attr:`effort_limit`).

Based on these parameters, the instantaneous minimum and maximum torques are defined as follows:
Based on these parameters, the instantaneous minimum and maximum torques for velocities between corner velocities
(where torque-speed curve intersects with continuous torque) are defined as follows:

.. math::

\tau_{j, max}(\dot{q}) & = clip \left (\tau_{j, sat} \times \left(1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right), 0.0, \tau_{j, max} \right) \\
\tau_{j, min}(\dot{q}) & = clip \left (\tau_{j, sat} \times \left( -1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right), - \tau_{j, max}, 0.0 \right)
\tau_{j, max}(\dot{q}) & = clip \left (\tau_{j, con} \times \left(1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right), inf, \tau_{j, max} \right) \\
\tau_{j, min}(\dot{q}) & = clip \left (\tau_{j, con} \times \left( -1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right), - \tau_{j, max}, inf \right)

where :math:`\gamma` is the gear ratio of the gear box connecting the motor and the actuated joint ends,
:math:`\dot{q}_{j, max} = \gamma^{-1} \times \dot{q}_{motor, max}`, :math:`\tau_{j, max} =
Expand All @@ -237,6 +240,7 @@ class DCMotor(IdealPDActuator):

\tau_{j, applied} = clip(\tau_{computed}, \tau_{j, min}(\dot{q}), \tau_{j, max}(\dot{q}))

The figure below demonstrates the clipping action for example (velocity,torque) pairs.
"""

cfg: DCMotorCfg
Expand All @@ -245,10 +249,11 @@ class DCMotor(IdealPDActuator):
def __init__(self, cfg: DCMotorCfg, *args, **kwargs):
super().__init__(cfg, *args, **kwargs)
# parse configuration
if self.cfg.saturation_effort is not None:
self._saturation_effort = self.cfg.saturation_effort
else:
self._saturation_effort = torch.inf
if self.cfg.saturation_effort is None:
raise ValueError("The saturation_effort must be provided for the DC motor actuator model.")
self._saturation_effort = self.cfg.saturation_effort
# find the velocity on the torque-speed curve that intersects effort_limit
self._vel_at_effort_lim = self.velocity_limit * (1 + self.effort_limit / self._saturation_effort)
# prepare joint vel buffer for max effort computation
self._joint_vel = torch.zeros_like(self.computed_effort)
# create buffer for zeros effort
Expand All @@ -275,15 +280,22 @@ def compute(

def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:
# compute torque limits

torque_speed_top = self._saturation_effort * (1.0 - self._joint_vel / self.velocity_limit)
torque_speed_bottom = self._saturation_effort * (-1.0 - self._joint_vel / self.velocity_limit)

# -- max limit
max_effort = self._saturation_effort * (1.0 - self._joint_vel / self.velocity_limit)
max_effort = torch.clip(max_effort, min=self._zeros_effort, max=self.effort_limit)
max_effort = torch.clip(torque_speed_top, max=self.effort_limit)
# -- min limit
min_effort = self._saturation_effort * (-1.0 - self._joint_vel / self.velocity_limit)
min_effort = torch.clip(min_effort, min=-self.effort_limit, max=self._zeros_effort)

min_effort = torch.clip(torque_speed_bottom, min=-self.effort_limit)
# clip the torques based on the motor limits
return torch.clip(effort, min=min_effort, max=max_effort)
clamped = torch.clip(effort, min=min_effort, max=max_effort)
gl_vel_at_effort_lim = self._joint_vel > self._vel_at_effort_lim
ls_vel_at_neg_effort_lim = self._joint_vel < -self._vel_at_effort_lim
clamped[gl_vel_at_effort_lim] = torque_speed_top[gl_vel_at_effort_lim]
clamped[ls_vel_at_neg_effort_lim] = torque_speed_bottom[ls_vel_at_neg_effort_lim]

return clamped


class DelayedPDActuator(IdealPDActuator):
Expand Down
3 changes: 3 additions & 0 deletions source/isaaclab/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,9 @@
"pillow==11.0.0",
# livestream
"starlette==0.46.0",
# testing
"pytest",
"pytest-mock",
]

PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu118"]
Expand Down
Loading